diff --git a/.github/lcovrc b/.github/lcovrc
new file mode 100644
index 00000000..2eb80cc7
--- /dev/null
+++ b/.github/lcovrc
@@ -0,0 +1,45 @@
+# lcov configuration for .github/workflows/coverage.yml.
+#
+# A copy of colcon-lcov-result 0.5.0's default (colcon_lcov_result/verb/configuration/lcovrc),
+# with branch coverage turned off below. The whole file is needed: --lcov-config-file
+# replaces the default rather than adding to it.
+#
+# Why no branch coverage: gcc emits a hidden "it threw" branch on nearly every line that
+# calls a function, and no test takes it. Each such line -- a declaration like
+# 'rtabmap::Transform t;' included -- then counts as a partial, and Codecov reports a
+# partial line as not covered. lcov's geninfo_no_exception_branch would drop those
+# branches, but not with the lcov 1.15 of Ubuntu 22.04 and gcc 11: its JSON reader
+# ignores the flag, and its text reader cannot read gcc 11's .gcno files. So only line
+# coverage is reported: a line is covered when a test ran it.
+
+geninfo_auto_base=1
+
+# Specify size of tabs
+genhtml_num_spaces = 2
+
+# Include color legend in HTML output if non-zero
+genhtml_legend = 1
+
+# Include function coverage data display
+genhtml_function_coverage = 1
+
+# Include branch coverage data display
+genhtml_branch_coverage = 0
+
+# Specify whether to capture coverage data for external source
+# files
+geninfo_external = 0
+
+# Less verbose output
+lcov_quiet = 1
+
+# Specify if function coverage data should be collected and
+# processed.
+lcov_function_coverage = 1
+
+# Specify if branch coverage data should be collected and
+# processed.
+lcov_branch_coverage = 0
+
+## Follow symlinks
+lcov_follow = 1
diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml
new file mode 100644
index 00000000..71ba59d3
--- /dev/null
+++ b/.github/workflows/coverage.yml
@@ -0,0 +1,187 @@
+name: Coverage
+
+on:
+  push:
+    branches: [ ros2 ]
+  pull_request:
+    branches: [ ros2 ]
+  workflow_dispatch:
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
+  cancel-in-progress: ${{ github.event_name == 'pull_request' }}
+
+permissions:
+  contents: read
+  id-token: write
+
+jobs:
+  coverage:
+    name: Coverage (humble)
+    runs-on: ubuntu-latest
+    container:
+      # RTAB-Map master, already built and installed on top of ROS Humble
+      # (jammy = 22.04) -- the same base the repo's own docker/humble image
+      # uses. Saves building the library here, and pins the coverage run to
+      # RTAB-Map master rather than whatever the released binaries carry.
+      image: introlab3it/rtabmap:jammy
+    env:
+      CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }}
+
+    steps:
+      - uses: actions/checkout@v4
+
+      # The container runs as root, so no sudo (it is not installed).
+      #
+      # colcon lcov-result is not a built-in verb -- it comes from the
+      # colcon-lcov-result package, which action-ros-ci calls but does not
+      # install. ros2.yml gets it for free because it runs ros-tooling/setup-ros
+      # first; this job calls action-ros-ci directly, so install it here.
+      # Without it action-ros-ci logs "invalid choice: 'lcov-result'", ignores
+      # the failure, and the job goes green having measured nothing.
+      # Versions pinned to the ones setup-ros uses.
+      - run: |
+          export DEBIAN_FRONTEND=noninteractive
+          apt-get update
+          # lcov for genhtml (the browsable artifact below); colcon-lcov-result
+          # shells out to it too.
+          apt-get install -y lcov python3-pip
+          pip3 install -U \
+            colcon-lcov-result==0.5.0 \
+            colcon-coveragepy-result==0.0.8
+          colcon lcov-result --help > /dev/null
+          # The image ships rosdep but has never initialized it -- the repo's
+          # own docker/humble Dockerfile runs `rosdep init` for the same reason.
+          # action-ros-ci only runs `rosdep update`, which fails with "no
+          # sources directory exists" until this has happened once.
+          rosdep init || true
+
+      # Only the packages that have tests. The others are still built when a
+      # tested package depends on them (--packages-up-to), just not measured.
+      - uses: ros-tooling/action-ros-ci@v0.4
+        with:
+          package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam rtabmap_python
+          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.
+          rosdep-skip-keys: rtabmap
+          # The coverage-gcc mixin only adds --coverage; it sets no build type,
+          # and neither action-ros-ci nor this repo's CMakeLists do. Without
+          # this the build type is empty, which happens to mean -O0 but is not
+          # guaranteed to stay that way -- and at -O2 inlining and dead-code
+          # elimination make gcov's line attribution unreliable.
+          extra-cmake-args: -DCMAKE_BUILD_TYPE=Debug
+          colcon-defaults: |
+            {
+              "build": {
+                "mixin": ["coverage-gcc"]
+              },
+              "test": {
+                "pytest-with-coverage": true
+              }
+            }
+          # Pinned so a change in the mixin repository cannot break this job.
+          colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/b8436aa16c0bdbc01081b12caa253cbf16e0fb82/index.yaml
+          # Run the coverage passes ourselves, in the step below. The action's
+          # own baseline pass is a bare `colcon lcov-result --initial` with no
+          # package selection, so it walks every package in the workspace --
+          # including the ones --packages-up-to never built -- and fails with
+          # "cannot read .../build/rtabmap_viz".
+          coverage-result: false
+
+      # Both passes restricted to the packages that were actually built.
+      #
+      # lcov captures each package's whole build directory, so more than this
+      # repo's sources land in it. Filtered:
+      #   */test/*  the test sources -- the instrument, not the subject. A line
+      #             there is uncovered only when the test skipped it, which says
+      #             nothing about the code under test, and they are near-fully
+      #             covered by construction.
+      #   /usr/*, /opt/*  everything outside the workspace. RTAB-Map's own .cpp
+      #             files are never captured (the library is installed in the
+      #             image, not built here), but its inline and template code is
+      #             emitted into the objects of the packages that include it,
+      #             as are PCL, Eigen and the standard library.
+      #   *CompilerId*, */CMakeFiles/*  CMake's compiler-probe translation
+      #             units. colcon-lcov-result tries to delete their .gcno files
+      #             but misses these, and they are recorded against a path that
+      #             does not exist -- which kills the genhtml pass colcon runs
+      #             at the end ("cannot read .../CMakeCCompilerId.c", exit 2).
+      #             Filters run before that pass, so dropping them here is
+      #             enough.
+      # Quoting them matters: action-ros-ci injects filters unquoted, so the
+      # shell can glob them away before colcon ever sees them.
+      - name: Coverage report
+        working-directory: ros_ws
+        run: |
+          # `.` not `source`: steps in this container run under sh (dash), where
+          # `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
+          # C++ packages only -- rtabmap_python emits no .gcno for lcov to read,
+          # and is measured by the coveragepy step below instead.
+          PKGS="rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_slam"
+          # 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.
+          # Line coverage only, no branch coverage: see the comment in .github/lcovrc.
+          LCOVRC=src/rtabmap_ros/.github/lcovrc
+          colcon lcov-result --initial --packages-select $PKGS --lcov-config-file $LCOVRC
+          # The verb ends by running genhtml and returns *its* exit code, so a
+          # cosmetic HTML hiccup fails the whole job even though the report was
+          # written. The deliverable is total_coverage.info; the HTML is a
+          # convenience. Tolerate the former, then gate on the latter.
+          colcon lcov-result --packages-select $PKGS --lcov-config-file $LCOVRC --verbose \
+            --filter '*/test/*' '/usr/*' '/opt/*' \
+              '*CompilerId*' '*/CMakeFiles/*' || true
+          test -s lcov/total_coverage.info
+
+      # Python coverage is a separate mechanism: the coverage-gcc mixin only adds
+      # --coverage to the compiler, which does nothing for an ament_python package.
+      # colcon test --pytest-with-coverage (set above) writes a Cobertura report into
+      # the package's own build directory instead.
+      #
+      # Its paths are relative to the package rather than the repository, so
+      # cv_compression.py arrives as "rtabmap_python/cv_compression.py" -- one level
+      # short of where it really lives. Rewrite them here rather than leave Codecov to
+      # guess, which it does by suffix and can get wrong.
+      - name: Python coverage report
+        working-directory: ros_ws
+        run: |
+          python3 - <<'EOF'
+          import pathlib
+          import xml.etree.ElementTree as ET
+          p = pathlib.Path('build/rtabmap_python/coverage.xml')
+          if not p.is_file():
+              print('::warning::no python coverage produced for rtabmap_python')
+              raise SystemExit(0)
+          tree = ET.parse(p)
+          root = tree.getroot()
+          for source in root.iter('source'):
+              source.text = '.'
+          for cls in root.iter('class'):
+              cls.set('filename', 'rtabmap_python/' + cls.get('filename'))
+          tree.write(p, xml_declaration=True, encoding='utf-8')
+          print('rewrote', p, 'to repository-relative paths')
+          EOF
+
+      # colcon lcov-result runs genhtml itself, into the same lcov/ directory.
+      - name: Upload HTML coverage artifact
+        uses: actions/upload-artifact@v4
+        with:
+          name: coverage-html
+          path: ros_ws/lcov
+          retention-days: 14
+
+      - name: Upload to Codecov
+        if: ${{ env.CODECOV_TOKEN != '' }}
+        uses: codecov/codecov-action@v5
+        with:
+          files: ros_ws/lcov/total_coverage.info,ros_ws/build/rtabmap_python/coverage.xml
+          # Upload ONLY the aggregated lcov file. By default the CLI also walks
+          # the tree and runs gcov over every .gcno it finds, which re-adds the
+          # test sources the --filter above just dropped.
+          disable_search: true
+          plugins: noop
+          token: ${{ env.CODECOV_TOKEN }}
+          fail_ci_if_error: false
diff --git a/.github/workflows/docker-ros2.yml b/.github/workflows/docker-ros2.yml
index 3c280a56..22264c6e 100644
--- a/.github/workflows/docker-ros2.yml
+++ b/.github/workflows/docker-ros2.yml
@@ -1,73 +1,62 @@
-name: docker
+name: docker-ros2
+
+# ROS2 images: humble, jazzy, kilted and lyrical.
+#
+# The images built from this tree (docker/*/latest) compile the workspace, which
+# is far too slow to emulate, so every arch of those is built natively: amd64 on
+# an x86 runner, arm64 on a GitHub arm64 runner. Because a single Docker Hub tag
+# cannot hold two independently pushed architectures, each build pushes an
+# arch-suffixed tag (e.g. :humble-latest-amd64 / :humble-latest-arm64) and a
+# final job joins them into the real multi-arch tag (:humble-latest) with
+# `imagetools create`.
+#
+# The runner image only hosts the build; it does not have to match the Ubuntu
+# release inside the image, so ubuntu-26.04{,-arm} is used for all of them
+# (ubuntu-22.04{,-arm} and ubuntu-24.04{,-arm} also exist, if ever needed).
 
 on:
   push:
     branches: [ ros2 ]
   pull_request:
     branches: [ ros2 ]
+  workflow_dispatch:
 
 concurrency:
   group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
   cancel-in-progress: ${{ github.event_name == 'pull_request' }}
 
 jobs:
+  # Images built from this tree (docker/*/latest), the ones a change here can break.
   docker:
-    runs-on: ubuntu-latest
-    
+    # A manual dispatch is honored only on ros2, the only ref we push from.
+    if: ${{ github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2' }}
+    runs-on: ${{ matrix.runner }}
+
     strategy:
       fail-fast: false
       matrix:
-        docker_tag: [humble, humble-latest, jazzy, jazzy-latest, kilted, kilted-latest, lyrical-latest]
+        docker_tag: [humble-latest, jazzy-latest, kilted-latest, lyrical-latest]
+        arch: [amd64, arm64]
         include:
-        - docker_tag: humble
-          docker_path: 'humble'
-          docker_platforms: |
-            linux/amd64
         - docker_tag: humble-latest
           docker_path: 'humble/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-        - docker_tag: jazzy
-          docker_path: 'jazzy'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
         - docker_tag: jazzy-latest
           docker_path: 'jazzy/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-        - docker_tag: kilted
-          docker_path: 'kilted'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
         - docker_tag: kilted-latest
           docker_path: 'kilted/latest'
-          docker_platforms: |
-            linux/amd64
-        #Disabled till rtabmap_ros is released on lyrical
-        #- docker_tag: lyrical
-        #  docker_path: 'lyrical'
-        #  docker_platforms: |
-        #    linux/amd64
-        #    linux/arm64
         - docker_tag: lyrical-latest
           docker_path: 'lyrical/latest'
-          docker_platforms: |
-            linux/amd64
-            linux/arm64
-    
+        - arch: amd64
+          runner: ubuntu-26.04
+          docker_platform: linux/amd64
+        - arch: arm64
+          runner: ubuntu-26.04-arm
+          docker_platform: linux/arm64
+
     steps:
       -
         name: Checkout
         uses: actions/checkout@v4
-      -
-        name: Set up QEMU
-        uses: docker/setup-qemu-action@v3
-        with:
-          platforms: all
       -
         name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3
@@ -86,9 +75,105 @@ jobs:
         with:
           context: .
           push: ${{ github.event_name != 'pull_request' }}
-          platforms: ${{ github.event_name == 'pull_request' && 'linux/amd64' || matrix.docker_platforms }}
+          platforms: ${{ matrix.docker_platform }}
+          # Run the test suites inside the image being built. Nothing of them is kept, and
+          # the build fails if one does, so no image is published from a tree that fails.
+          build-args: |
+            RUN_TESTS=1
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
+          tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-${{ matrix.arch }}
+          no-cache: true
+          cache-to: type=inline
+
+  docker_manifest:
+    needs: docker
+    # Nothing to join on pull requests, where the per-arch tags are never pushed.
+    if: ${{ !cancelled() && !failure() && github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }}
+    runs-on: ubuntu-26.04
+
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [humble-latest, jazzy-latest, kilted-latest, lyrical-latest]
+
+    steps:
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Create multi-arch manifest
+        run: |
+          docker buildx imagetools create \
+            -t introlab3it/rtabmap_ros:${{ matrix.docker_tag }} \
+            introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-amd64 \
+            introlab3it/rtabmap_ros:${{ matrix.docker_tag }}-arm64
+
+  # Images that install a released rtabmap_ros from apt (docker/<distro>): they hold
+  # nothing from the tree under review, so building them on a pull request would only
+  # report that the release still installs. Left to the pushes that publish them.
+  #
+  # This one is left on a single QEMU-emulated job, for simplicity: it only
+  # apt-installs a released rtabmap_ros, so nothing is compiled under emulation,
+  # and one build pushes the multi-arch tag straight away -- no per-arch tags and
+  # no manifest job to join them.
+  docker-released:
+    if: ${{ github.event_name != 'pull_request' && (github.event_name != 'workflow_dispatch' || github.ref == 'refs/heads/ros2') }}
+    runs-on: ubuntu-latest
+
+    strategy:
+      fail-fast: false
+      matrix:
+        docker_tag: [humble, jazzy, kilted, lyrical]
+        include:
+        - docker_tag: humble
+          docker_path: 'humble'
+          docker_platforms: |
+            linux/amd64
+        - docker_tag: jazzy
+          docker_path: 'jazzy'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+        - docker_tag: kilted
+          docker_path: 'kilted'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+        - docker_tag: lyrical
+          docker_path: 'lyrical'
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v4
+      -
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v3
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v3
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v6
+        with:
+          context: .
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
           file: ./docker/${{ matrix.docker_path }}/Dockerfile
           tags: introlab3it/rtabmap_ros:${{ matrix.docker_tag }}
           no-cache: true
           cache-to: type=inline
-
diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml
index ab73b32e..0222175d 100644
--- a/.github/workflows/docker.yml
+++ b/.github/workflows/docker.yml
@@ -4,9 +4,13 @@ on:
   push:
     branches:
       - 'master'
+  workflow_dispatch:
 
 jobs:
   docker:
+    # Built and pushed only from master (push or manual dispatch), since it
+    # pushes the introlab3it/rtabmap_ros tags to Docker Hub.
+    if: github.ref == 'refs/heads/master'
     runs-on: ubuntu-latest
     
     strategy:
diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml
index e374af80..5626e980 100644
--- a/.github/workflows/ros2.yml
+++ b/.github/workflows/ros2.yml
@@ -6,6 +6,7 @@ on:
     branches: [ lyrical-devel ]
   pull_request:
     branches: [ lyrical-devel ]
+  workflow_dispatch:
 
 env:
   BUILD_TYPE: Release
@@ -16,16 +17,16 @@ concurrency:
 
 jobs:
   build:
-    name: Build ros2 ${{ matrix.ros_distro }}
+    name: Build ros2 ${{ matrix.ros_distro }}${{ matrix.ros2_testing && ' (ros-testing)' || '' }}
     runs-on: ubuntu-latest
     strategy:
       matrix:
         ros_distro: [lyrical]
+        ros2_testing: [false, true] # build with main and ros-testing apt repositories
         include:
         - ros_distro: lyrical
           skip_keys: '' # skip keys shoudl be empty when release on ROSDISTRO_devel branch, comment these keys in the appriopriate package.
-          # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, commented from rtabmap_ros package. 
-          # Make sure rosdistro doesn't declare rtabmap_costmap_plugins.
+          # velodyne and grid_map_ros are not available on lyrical, they are commented in rtabmap_examples and rtabmap_util packages.
           packages: 'rtabmap_ros'
       fail-fast: false
     container:
@@ -35,6 +36,7 @@ jobs:
     - uses: ros-tooling/setup-ros@v0.7
       with:
         required-ros-distributions: ${{ matrix.ros_distro }}
+        use-ros2-testing: ${{ matrix.ros2_testing }}
     - run: |
         DEBIAN_FRONTEND=noninteractive
         sudo apt update
diff --git a/.gitignore b/.gitignore
index 5fb548e9..24f04474 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,12 @@
 .pydevproject
 .settings
+.vscode
 __pycache__
+# rosdoc2 build artifacts. docs_build/ holds a copy of each package manifest, so
+# 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
diff --git a/README.md b/README.md
index de5a2176..c0812e0a 100644
--- a/README.md
+++ b/README.md
@@ -1,89 +1,86 @@
 rtabmap_ros
 ===========
 
-RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Humble minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)). 
+ROS 2 wrapper for [RTAB-Map](https://github.com/introlab/rtabmap), a graph-based SLAM library with appearance-based loop closure detection. It builds and maintains a 3D map from RGB-D, stereo or lidar data, closes loops on revisited places and exports the result as an occupancy grid, a point cloud or an OctoMap.
+
+**ROS 2 Humble minimum required.** The interface matches ROS 1: parameters and topic names still follow the [ROS 1 documentation](http://wiki.ros.org/rtabmap_ros) for anything not yet covered by the package pages below.
 
 #### CI Latest
 
-  <table>
-    <tbody>
-        <tr>
-           <td>ROS 1</td>
-           <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
-        <tr>
-           <td>ROS 2</td>
-           <td><a href="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml"><img src="https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg" alt="Build Status"/>
-           </td>
-        </tr>
-     </tbody>
-  </table>
- 
- #### ROS Binaries
- 
- <table>
-    <tbody>
-        <tr>
-            <td rowspan="1">ROS 1</td>
-            <td>Noetic</td>
-            <td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td rowspan="4">ROS 2</td>
-            <td>Humble</td>
-            <td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Jazzy</td>
-            <td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-            <td>Rolling</td>
-            <td><a href="http://build.ros2.org/job/Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
-        </tr>
-        <tr>
-           <td>Docker</td>
-           <td>
-             <a href="https://hub.docker.com/r/introlab3it/rtabmap_ros">rtabmap_ros</a>
-           </td>
-           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls" alt="Docker Pulls"/></td>
-        </tr>
-    </tbody>
-</table>
+| | Build | Docker |
+|---|---|---|
+| ROS 1 | [![ROS 1](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros1.yml) | [![Docker](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker.yml) |
+| ROS 2 | [![ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/ros2.yml) | [![Docker ROS 2](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml/badge.svg)](https://github.com/introlab/rtabmap_ros/actions/workflows/docker-ros2.yml) |
 
-# Usage
+#### ROS Binaries
 
-* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
+| | Distro | Ubuntu | Released | In apt | Build |
+|---|---|---|---|---|---|
+| ROS 1 | Noetic (EOL) | 20.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fnoetic%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [![apt](https://img.shields.io/ros/v/noetic/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#noetic) |  |
+| ROS 2 | Humble | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fhumble%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [![apt](https://img.shields.io/ros/v/humble/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#humble) | [![build](http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary)](http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/) |
+| ROS 2 | Iron (EOL) | 22.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Firon%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [![apt](https://img.shields.io/ros/v/iron/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#iron) |  |
+| ROS 2 | Jazzy | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fjazzy%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [![apt](https://img.shields.io/ros/v/jazzy/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#jazzy) | [![build](http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) |
+| ROS 2 | Kilted | 24.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Fkilted%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [![apt](https://img.shields.io/ros/v/kilted/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#kilted) | [![build](http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary)](http://build.ros2.org/job/Kbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/) |
+| ROS 2 | Lyrical | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Flyrical%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [![apt](https://img.shields.io/ros/v/lyrical/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#lyrical) | [![build](http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary)](http://build.ros2.org/job/Lbin_uR64__rtabmap_ros__ubuntu_resolute_amd64__binary/) |
+| ROS 2 | Rolling | 26.04 | [![released](https://img.shields.io/badge/dynamic/yaml?url=https%3A%2F%2Fraw.githubusercontent.com%2Fros%2Frosdistro%2Fmaster%2Frolling%2Fdistribution.yaml&query=%24.repositories.rtabmap_ros.release.version&label=%20)](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [![apt](https://img.shields.io/ros/v/rolling/rtabmap_ros?label=%20)](https://index.ros.org/p/rtabmap_ros/#rolling) |  |
+| Docker | [rtabmap_ros](https://hub.docker.com/r/introlab3it/rtabmap_ros) | | | ![Docker Pulls](https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls) | |
 
-* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
+*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync.
 
-## Logging
-To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info):
-```bash
-export RCUTILS_LOGGING_USE_STDOUT=1
-export RCUTILS_LOGGING_BUFFERED_STREAM=1
-# Optional, but if you like colored logs:
-export RCUTILS_COLORIZED_OUTPUT=1
-```
+# Packages
 
-## Recommended DDS
-If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`):
-```bash
-export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
-# Cyclone prefers multicast by default, if your router got too much spammed, 
-# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489):
-export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
-```
+The stack is split into small packages so a pipeline only pulls in what it uses. Package names link to their documentation where it exists; the rest are being written and will be linked as they land.
 
-# Installation 
+### SLAM
+
+| Package | Description |
+|---|---|
+| [`rtabmap_slam`](rtabmap_slam/README.md) | The `rtabmap` node itself: appearance-based loop closure detection, graph optimization, memory management and map assembly. |
+| [`rtabmap_odom`](rtabmap_odom/README.md) | Odometry nodes — `rgbd_odometry`, `stereo_odometry` and `icp_odometry`. Any external odometry can be used instead. |
+| [`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
+
+| Package | Description |
+|---|---|
+| [`rtabmap_util`](rtabmap_util/README.md) | Utility nodes around the pipeline: format conversions, point cloud filtering and assembly, obstacle detection, map assembly, database replay. Most are useful on their own. |
+| `rtabmap_costmap_plugins` | A variant of nav2's voxel layer that follows the robot along z, keeping the voxel grid centered on the base frame. For robots that change altitude, e.g. drones. |
+
+### Interfaces and libraries
+
+| Package | Description |
+|---|---|
+| `rtabmap_msgs` | Message, service and action definitions used across the stack. |
+| [`rtabmap_conversions`](rtabmap_conversions/README.md) | C++ library converting between RTAB-Map library types and ROS 2 messages. |
+| [`rtabmap_python`](rtabmap_python/README.md) | Python helpers for RTAB-Map's own binary formats, currently the compressed matrices carried in `rtabmap_msgs` fields and database blobs. |
+
+### Visualization
+
+| Package | Description |
+|---|---|
+| `rtabmap_viz` | RTAB-Map's own GUI as a ROS 2 node: live graph, loop closures, feature matches and the parameter panel. |
+| `rtabmap_rviz_plugins` | RViz displays for the map graph, the assembled cloud and the SLAM info. |
+
+### Launch files
+
+| Package | Description |
+|---|---|
+| `rtabmap_launch` | `rtabmap.launch.py`, the one-line way to bring up the whole stack. |
+| [`rtabmap_examples`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) | Sensor integration examples: stereo and RGB-D cameras, 3D lidar. |
+| [`rtabmap_demos`](rtabmap_demos/README.md) | Full robot demos: turtlebot3 and turtlebot4, nav2 integration, multi-session mapping. |
+
+# Installation
+
+These instructions are for ROS 2. For ROS 1, follow the [installation instructions](https://github.com/introlab/rtabmap_ros/tree/master#installation) on the [`master`](https://github.com/introlab/rtabmap_ros/tree/master) branch, which also carries the latest version for Noetic.
 
 ### Binaries
+
 ```bash
 sudo apt install ros-$ROS_DISTRO-rtabmap-ros
 ```
 
 ### From Source
+
 * Make sure to uninstall any rtabmap binaries:
     ```
     sudo apt remove ros-$ROS_DISTRO-rtabmap*
@@ -103,3 +100,60 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros
     colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release
     ```
 
+### Testing
+
+```bash
+cd ~/ros2_ws
+colcon build --base-paths src/rtabmap_ros
+colcon test --base-paths src/rtabmap_ros
+colcon test-result --verbose
+```
+
+# Usage
+
+* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
+
+* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
+
+## Logging
+To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/jazzy/Concepts/Intermediate/About-Logging.html)" documentation for more info):
+```bash
+export RCUTILS_LOGGING_USE_STDOUT=1
+export RCUTILS_LOGGING_BUFFERED_STREAM=1
+# Optional, but if you like colored logs:
+export RCUTILS_COLORIZED_OUTPUT=1
+```
+
+## Recommended DDS
+If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/jazzy/Installation/RMW-Implementations/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files (or add to your `.bashrc`):
+```bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+# Cyclone prefers multicast by default, if your router got too much spammed, 
+# disable multicast with (https://github.com/ros2/rmw_cyclonedds/issues/489):
+export CYCLONEDDS_URI="<Disc><DefaultMulticastAddress>0.0.0.0</></>"
+```
+
+# Documentation
+
+* **Package documentation** — the tables above, and the [API reference on docs.ros.org](https://docs.ros.org/en/jazzy/p/rtabmap_ros/).
+* **Examples** — [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) for sensors, [rtabmap_demos](rtabmap_demos/README.md) for full robots.
+* **Parameters** — every `Rtabmap/*`, `Grid/*`, `Odom/*` and other core parameter is listed in the [RTAB-Map parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html).
+* **Library API** — [RTAB-Map's own API documentation](https://introlab.github.io/rtabmap/api/latest/).
+* **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.
diff --git a/codecov.yml b/codecov.yml
new file mode 100644
index 00000000..3ffcb55f
--- /dev/null
+++ b/codecov.yml
@@ -0,0 +1,91 @@
+# Codecov configuration -- https://docs.codecov.com/docs/codecov-yaml
+#
+# Coverage data is produced by .github/workflows/coverage.yml (colcon's
+# coverage-gcc mixin over the packages that have tests, aggregated by
+# colcon-lcov-result) and uploaded by codecov/codecov-action; this file only
+# controls what Codecov reports back on a pull request. Nothing is posted
+# unless the Codecov GitHub App has access to the repository.
+
+# action-ros-ci checks the repository out into a colcon workspace, so every
+# path in the lcov report is prefixed. Strip it, otherwise Codecov cannot match
+# a file to the one in the diff and reports no coverage at all.
+fixes:
+  - "ros_ws/src/rtabmap_ros/::"
+
+# Mark uncovered added lines inline in the "Files changed" tab.
+github_checks:
+  annotations: true
+
+coverage:
+  precision: 2
+  round: down
+  range: "10...90" # red/green scale: 10% is fully red, 90% fully green
+
+  status:
+    # Catch a slow slide down without pinning an absolute number.
+    project:
+      default:
+        target: auto
+        threshold: 1%
+
+    # Coverage of the lines this pull request touches. Advisory: reported, but
+    # does not block the merge -- drop "informational" to make it gate.
+    patch:
+      default:
+        informational: true
+
+# Per-package breakdown, computed from the same single upload -- no extra job
+# and no separate flag upload per package. Each component gets its own line in
+# the pull request comment and its own status check.
+component_management:
+  default_rules:
+    statuses:
+      - type: project
+        target: auto
+        threshold: 1%
+  individual_components:
+    - component_id: rtabmap_conversions
+      name: rtabmap_conversions
+      paths:
+        - rtabmap_conversions/**
+    - component_id: rtabmap_util
+      name: rtabmap_util
+      paths:
+        - rtabmap_util/**
+    - component_id: rtabmap_sync
+      name: rtabmap_sync
+      paths:
+        - rtabmap_sync/**
+    - component_id: rtabmap_odom
+      name: rtabmap_odom
+      paths:
+        - rtabmap_odom/**
+    - component_id: rtabmap_slam
+      name: rtabmap_slam
+      paths:
+        - rtabmap_slam/**
+    - component_id: rtabmap_python
+      name: rtabmap_python
+      paths:
+        - rtabmap_python/**
+
+comment:
+  layout: "condensed_header, diff, components, files"
+  behavior: default
+  require_changes: true # stay quiet when coverage doesn't move
+
+# Only rtabmap_conversions, rtabmap_util, rtabmap_sync, rtabmap_odom, rtabmap_slam and
+# rtabmap_python have tests today, so
+# everything else would report as 0% and drag the total down to a number that
+# says nothing. As a package gains tests, drop its line here and add it to
+# individual_components above.
+ignore:
+  - "rtabmap_costmap_plugins/**"
+  - "rtabmap_demos/**"
+  - "rtabmap_examples/**"
+  - "rtabmap_launch/**"
+  - "rtabmap_msgs/**"
+  - "rtabmap_rviz_plugins/**"
+  - "rtabmap_viz/**"
+  - "**/test/**"
+  - "**/setup.py" # packaging scaffolding, not code under test
diff --git a/docker/humble/latest/Dockerfile b/docker/humble/latest/Dockerfile
index fcbb8551..f14894c3 100644
--- a/docker/humble/latest/Dockerfile
+++ b/docker/humble/latest/Dockerfile
@@ -4,14 +4,26 @@ RUN mkdir -p ros2_ws/src
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
     colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/humble --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws
diff --git a/docker/iron/latest/Dockerfile b/docker/iron/latest/Dockerfile
index 3051956f..87b5919b 100644
--- a/docker/iron/latest/Dockerfile
+++ b/docker/iron/latest/Dockerfile
@@ -6,15 +6,21 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j1" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool && \
     apt remove ros-$ROS_DISTRO-rtabmap* -y && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j1" && \
     colcon build --event-handlers console_direct+ --install-base /opt/ros/iron --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
     cd && \
     rm -rf ros2_ws
diff --git a/docker/jazzy/latest/Dockerfile b/docker/jazzy/latest/Dockerfile
index 62259cc4..dafdfca4 100644
--- a/docker/jazzy/latest/Dockerfile
+++ b/docker/jazzy/latest/Dockerfile
@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws
diff --git a/docker/kilted/latest/Dockerfile b/docker/kilted/latest/Dockerfile
index 685cbec1..2f0db208 100644
--- a/docker/kilted/latest/Dockerfile
+++ b/docker/kilted/latest/Dockerfile
@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws
diff --git a/docker/lyrical/latest/Dockerfile b/docker/lyrical/latest/Dockerfile
index 6517694d..4a1bf22a 100644
--- a/docker/lyrical/latest/Dockerfile
+++ b/docker/lyrical/latest/Dockerfile
@@ -6,14 +6,26 @@ RUN source /ros_entrypoint.sh && \
 
 COPY . ros2_ws/src/rtabmap_ros
 
+# No rosdep "-r": a bad mirror must fail here.
 RUN source /ros_entrypoint.sh && \
     cd ros2_ws && \
-    export MAKEFLAGS="-j2" && \
+    echo 'Acquire::Retries "5";' > /etc/apt/apt.conf.d/80-retries && \
     rosdep init && \
     rosdep update && \
     apt-get update && \
-    rosdep install --from-paths src --ignore-src -r -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros nav2_costmap_2d" && \
-    apt-get clean && rm -rf /var/lib/apt/lists/ && \
-    colcon build --packages-skip rtabmap_costmap_plugins rtabmap_ros --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    rosdep install --from-paths src --ignore-src -y -t build_export -t test -t build -t buildtool_export -t buildtool --skip-keys="rtabmap nav2_bringup realsense2_camera nav2_msgs grid_map_ros" && \
+    bash src/rtabmap_ros/docker/verify_deps.sh && \
+    apt-get clean && rm -rf /var/lib/apt/lists/
+
+ARG RUN_TESTS=0
+
+RUN source /ros_entrypoint.sh && \
+    cd ros2_ws && \
+    export MAKEFLAGS="-j2" && \
+    colcon build --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release && \
+    if [ "$RUN_TESTS" = "1" ]; then \
+        colcon test --executor sequential --event-handlers console_direct+ --install-base /opt/ros/$ROS_DISTRO --merge-install && \
+        colcon test-result --verbose; \
+    fi && \
     cd && \
     rm -rf ros2_ws
diff --git a/docker/verify_deps.sh b/docker/verify_deps.sh
new file mode 100755
index 00000000..34a7bc66
--- /dev/null
+++ b/docker/verify_deps.sh
@@ -0,0 +1,76 @@
+#!/usr/bin/env bash
+#
+# Sanity-check the build sysroot right after "rosdep install".
+#
+# apt/dpkg can be left in a half-applied state, most often on the QEMU-emulated
+# arm64 CI leg (ports.ubuntu.com is a single, frequently desynced mirror). CMake
+# does not notice, because both PCL and VTK look their files up in ways that
+# degrade silently:
+#
+#   * PCLConfig.cmake resolves each component with
+#     find_library(... HINTS ${PCL_LIBRARY_DIRS} NO_DEFAULT_PATH), so a missing
+#     libpcl_*.so only yields "Could NOT find PCL_COMMON (missing:
+#     PCL_COMMON_LIBRARY)" on stderr and configuring still succeeds.
+#
+#   * VTK-targets.cmake creates every VTK::* imported target, then loads their
+#     IMPORTED_LOCATION from the per-configuration files it picks up with
+#     file(GLOB VTK-targets-*.cmake). An empty glob is not an error, so the
+#     targets survive with no location at all and the build only dies at the
+#     generate step with "IMPORTED_LOCATION not set for imported target
+#     VTK::CommonCore configuration Release".
+#
+# Both surface hours into the build, in whichever package first links those
+# targets (rtabmap_odom, via pcl_ros). Fail here instead, where the cause is
+# still readable.
+
+set -euo pipefail
+shopt -s nullglob
+
+status=0
+
+fail() {
+  echo "verify_deps: $*" >&2
+  status=1
+}
+
+# PCLConfig.cmake and the libpcl_*.so development symlinks both ship in
+# libpcl-dev, so finding the config without them means the package is not
+# fully installed.
+for config in /usr/lib/*/cmake/pcl/PCLConfig.cmake /usr/lib/cmake/pcl/PCLConfig.cmake; do
+  # nullglob only drops patterns, not wildcard-free words.
+  [ -e "${config}" ] || continue
+  libdir=${config%/cmake/pcl/PCLConfig.cmake}
+  for component in common io kdtree search surface filters registration \
+                   sample_consensus segmentation visualization; do
+    if [ ! -e "${libdir}/libpcl_${component}.so" ]; then
+      fail "${libdir}/libpcl_${component}.so is missing while ${config} is installed (libpcl-dev is incomplete)"
+    fi
+  done
+done
+
+for targets in /usr/lib/*/cmake/vtk-*/VTK-targets.cmake /usr/lib/cmake/vtk-*/VTK-targets.cmake; do
+  [ -e "${targets}" ] || continue
+  if ! compgen -G "${targets%.cmake}-*.cmake" > /dev/null; then
+    fail "no VTK-targets-<config>.cmake next to ${targets}, every VTK::* target would have no IMPORTED_LOCATION (libvtk-dev is incomplete)"
+  fi
+done
+
+# libssl-dev ships the libssl.so and libcrypto.so development symlinks beside the
+# headers FindOpenSSL reads its version from. Every find_package(rclcpp) reaches
+# find_package(OpenSSL REQUIRED) through fastrtps-config.cmake, so headers without
+# the symlinks fail the first package that configures rclcpp, several packages in.
+if [ -e /usr/include/openssl/opensslv.h ]; then
+  for lib in ssl crypto; do
+    if ! compgen -G "/usr/lib/*-linux-gnu/lib${lib}.so" > /dev/null \
+       && [ ! -e "/usr/lib/lib${lib}.so" ]; then
+      fail "no lib${lib}.so under /usr/lib while /usr/include/openssl is installed (libssl-dev is incomplete)"
+    fi
+  done
+fi
+
+if [ "${status}" -ne 0 ]; then
+  echo "verify_deps: dependency installation left an inconsistent sysroot, aborting before the build" >&2
+  exit 1
+fi
+
+echo "verify_deps: PCL, VTK and OpenSSL sysroot look consistent"
diff --git a/rtabmap_costmap_plugins/COLCON_IGNORE b/docs_build/COLCON_IGNORE
similarity index 100%
rename from rtabmap_costmap_plugins/COLCON_IGNORE
rename to docs_build/COLCON_IGNORE
diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt
index 4dc41907..af0a93d4 100644
--- a/rtabmap_conversions/CMakeLists.txt
+++ b/rtabmap_conversions/CMakeLists.txt
@@ -30,7 +30,7 @@ find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 
-find_package(RTABMap 0.23.5 REQUIRED)
+find_package(RTABMap 0.23.13 REQUIRED)
 
 # libraries
 SET(Libraries
@@ -122,5 +122,22 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_msg_conversion test/test_msg_conversion.cpp)
+  if(TARGET test_msg_conversion)
+    target_link_libraries(test_msg_conversion rtabmap_conversions)
+    if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+      ament_target_dependencies(test_msg_conversion ${AmentLibraries})
+    else()
+      target_link_libraries(test_msg_conversion ${Libraries} ${PublicLibraries})
+    endif()
+  endif()
+endif()
+
 ament_package()
 
diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md
new file mode 100644
index 00000000..2909600e
--- /dev/null
+++ b/rtabmap_conversions/README.md
@@ -0,0 +1,68 @@
+# rtabmap_conversions
+
+Conversions between [RTAB-Map](https://github.com/introlab/rtabmap) library types and ROS 2 messages.
+
+This package is a library only — it contains no nodes, no launch files and no parameters. Every other `rtabmap_ros` package that touches a message goes through it: [`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam), [`rtabmap_odom`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom), [`rtabmap_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync), [`rtabmap_util`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_util), [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) and [`rtabmap_rviz_plugins`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_rviz_plugins).
+
+You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`.
+
+## Contents
+
+- [Usage](#usage)
+- [What it covers](#what-it-covers)
+- [Conventions worth knowing](#conventions-worth-knowing)
+- [License](#license)
+
+## Usage
+
+Add the dependency to your `package.xml` and `CMakeLists.txt`:
+
+```xml
+<depend>rtabmap_conversions</depend>
+```
+
+```cmake
+find_package(rtabmap_conversions REQUIRED)
+target_link_libraries(my_node rtabmap_conversions::rtabmap_conversions)
+```
+
+Everything lives in a single header and the `rtabmap_conversions` namespace:
+
+```cpp
+#include <rtabmap_conversions/MsgConversion.h>
+
+// A pose message to an rtabmap::Transform and back.
+rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg.pose);
+
+geometry_msgs::msg::Pose out;
+rtabmap_conversions::transformToPoseMsg(pose, out);
+```
+
+The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type, `xxxToROS()` goes the other way. `ToROS()` functions write through a reference parameter so the message can be reused; `FromROS()` functions return by value.
+
+## What it covers
+
+| Group | Functions |
+|---|---|
+| Transforms | `transformFromTF`, `transformToTF`, `transformFromGeometryMsg`, `transformToGeometryMsg`, `transformFromPoseMsg`, `transformToPoseMsg` |
+| TF lookups | `getTransform`, `getMovingTransform` |
+| Camera models | `cameraModelFromROS`, `cameraModelToROS`, `stereoCameraModelFromROS` |
+| Images | `toCvCopy`, `toCvShare`, `rgbdImageFromROS`, `rgbdImageToROS`, `convertRGBDMsgs`, `convertStereoMsg` |
+| Laser scans | `convertScanMsg`, `convertScan3dMsg`, `deskew`, `transformPointCloud`, `sizeOfPointField` |
+| Features | `keypointFromROS`, `point2fFromROS`, `point3fFromROS`, `globalDescriptorFromROS` (+ vector and `ToROS` variants) |
+| Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) |
+| Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` |
+
+Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/jazzy/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h).
+
+## Conventions worth knowing
+
+These cut across the whole API and are not obvious from the signatures. Per-function caveats — object lifetimes, which fields a given `ToROS()` fills — are documented on the functions themselves.
+
+**Null transforms.** RTAB-Map distinguishes a *null* transform (unknown) from identity. Over the wire this is encoded as an all-zero quaternion, so `transformFromGeometryMsg()` and `transformFromPoseMsg()` return a null `rtabmap::Transform` for one. Always check `isNull()` before using a result. `tf2::Transform` cannot represent this — it stores rotation as a basis matrix — so `transformToTF()` returns a `bool` instead.
+
+**`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
index 4e82e388..666cdb38 100644
--- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
@@ -71,76 +71,374 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RCLCPP_QOS(queueSize, qos) rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos)
 #endif
 
+/**
+ * @namespace rtabmap_conversions
+ * @brief Conversions between RTAB-Map library types and ROS 2 messages.
+ *
+ * Naming is uniform throughout: `xxxFromROS()` converts a message into an RTAB-Map
+ * type and returns it by value, `xxxToROS()` writes an RTAB-Map type into a message
+ * passed by reference so the message can be reused.
+ *
+ * @note RTAB-Map distinguishes a *null* transform (unknown) from an identity one. On
+ *       the wire a null transform is encoded as an all-zero quaternion, so results of
+ *       the `transformFromXxx()` functions should be checked with
+ *       rtabmap::Transform::isNull() before use.
+ */
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+//============================================================================
+// Transforms
+// Conversions between rtabmap::Transform and the tf2 / geometry_msgs representations.
+//============================================================================
+
+/**
+ * @brief Convert a rtabmap::Transform into a tf2::Transform.
+ * @param[in]  transform   the transform to convert
+ * @param[out] tfTransform the converted transform, or filled with NaN if @p transform is null
+ * @return false if @p transform is null, true otherwise
+ *
+ * @note tf2::Transform stores its rotation as a basis matrix and so cannot represent
+ *       the all-zero quaternion used elsewhere to mean "null". The null case is
+ *       reported through the return value instead, and the output is poisoned with
+ *       NaN so that ignoring that return value fails loudly rather than silently
+ *       proceeding with a plausible-looking identity.
+ * @see transformFromTF()
+ */
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+
+/**
+ * @brief Convert a tf2::Transform into a rtabmap::Transform.
+ * @param transform the transform to convert
+ * @return the converted transform, or a null transform if @p transform contains NaN
+ *         (which is how transformToTF() reports a null transform)
+ * @see transformToTF()
+ */
 rtabmap::Transform transformFromTF(const tf2::Transform & transform);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Transform.
+ *
+ * The quaternion is normalized. A null @p transform is encoded as an all-zero
+ * quaternion, which transformFromGeometryMsg() decodes back to null.
+ *
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message
+ */
 void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Transform & msg);
+
+/**
+ * @brief Convert a geometry_msgs Transform into a rtabmap::Transform.
+ * @param msg the message to convert
+ * @return the converted transform, or a null transform if the quaternion is all zeros
+ */
 rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::msg::Transform & msg);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Pose.
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message; a null @p transform gives an all-zero orientation
+ */
 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Pose & msg);
+
+/**
+ * @brief Convert a geometry_msgs Pose into a rtabmap::Transform.
+ * @param msg                    the message to convert
+ * @param ignoreRotationIfNotSet if true, an all-zero orientation yields a
+ *                               translation-only transform instead of a null one
+ * @return the converted transform, or a null transform if the orientation is all zeros
+ *         and @p ignoreRotationIfNotSet is false
+ *
+ * @warning geometry_msgs::msg::Quaternion defaults to `w = 1`, not all zeros, so a
+ *          default-constructed Pose is a valid identity rotation rather than "unset".
+ */
 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::msg::Pose & msg, bool ignoreRotationIfNotSet = false);
 
+
+//============================================================================
+// Images
+// Extracting OpenCV images from RGBDImage messages, and building them back.
+//============================================================================
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message, copying the pixels.
+ *
+ * Handles both the raw (`rgb`, `depth`) and compressed (`rgb_compressed`,
+ * `depth_compressed`) fields. Both output pointers are always valid; they hold an
+ * empty image when the corresponding field is not set.
+ *
+ * @param[in]  image the message to read
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ * @see toCvShare() to avoid the copy
+ */
 void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ *
+ * The returned images alias the message's buffers, so @p image must outlive them. Both
+ * output pointers are always valid; they hold an empty image when the corresponding
+ * field is not set.
+ *
+ * @param[in]  image the message to read; its shared pointer keeps the buffers alive
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ *
+ * Both output pointers are always valid; they hold an empty image when the corresponding
+ * field is not set.
+ *
+ * @param[in]  image         the message to read
+ * @param[in]  trackedObject object whose lifetime keeps the message buffers alive
+ * @param[out] rgb           the RGB image
+ * @param[out] depth         the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Fill an RGBDImage message from a SensorData.
+ *
+ * Supports a single RGB-D camera or a single stereo pair; multi-camera data cannot be
+ * represented by this message and is rejected with an error.
+ *
+ * @param[in]  data          the sensor data to convert
+ * @param[out] msg           the converted message, stamped with @p data's stamp
+ * @param[in]  sensorFrameId frame id stamped on the message and its sub-messages
+ *
+ * @note rtabmap::SensorData holds its stamp as a double, so the stamp written here is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data originally came from. Callers that need
+ *       the exact original stamp assign `msg.header` after this call.
+ * @note Unlike infoToROS(), an already-stamped `msg.header` is overwritten rather than
+ *       kept: the same header is applied to every sub-message here, so preserving only
+ *       the top-level one would leave the message internally inconsistent.
+ */
 void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId);
+
+/**
+ * @brief Build a SensorData from an RGBDImage message.
+ *
+ * The stamp is taken from the top-level `image->header`, and the camera's local
+ * transform is not carried by the message (callers resolve it from TF).
+ *
+ * The depth image is optional: a message carrying only the color image and its camera
+ * info gives a SensorData with no depth, which is valid.
+ *
+ * @param image the message to convert
+ * @return the converted sensor data, empty (SensorData::isValid() false) if the message
+ *         carries no color image or an unsupported encoding
+ *
+ * @warning The returned SensorData does **not** copy the pixels: it points into the
+ *          message's own buffers. @p image must therefore outlive it and must not be
+ *          modified meanwhile. Deep-copy the images before letting the SensorData
+ *          escape a subscription callback, because the queue recycles the message as
+ *          soon as the callback returns.
+ */
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image);
 
-// copy data
+
+//============================================================================
+// Compressed data
+//============================================================================
+
+/**
+ * @brief Copy an already-compressed cv::Mat into a byte vector.
+ * @param[in]  compressed a 1xN CV_8UC1 matrix of compressed bytes, or an empty matrix
+ * @param[out] bytes      the bytes; cleared when @p compressed is empty
+ */
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
+
+/**
+ * @brief Wrap a byte vector as a 1xN CV_8UC1 cv::Mat of compressed data.
+ * @param bytes the bytes to wrap
+ * @param copy  if false, the returned matrix aliases @p bytes, which must then outlive it
+ * @return the matrix, empty when @p bytes is empty
+ */
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
 
+
+//============================================================================
+// Statistics
+//============================================================================
+
+/**
+ * @brief Read an Info message into RTAB-Map statistics.
+ * @param[in]  info the message to convert
+ * @param[out] stat the statistics, marked as extended
+ * @note The stamp comes from `info.header`, which infoToROS() does not set.
+ */
 void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & stat);
+
+/**
+ * @brief Fill an Info message from RTAB-Map statistics.
+ * @param[in]  stats the statistics to convert
+ * @param[out] info  the converted message
+ * @note If the caller left `info.header.stamp` unset it is filled from @p stats, so that
+ *       infoFromROS() recovers a stamp. An already-stamped header is never overwritten:
+ *       rtabmap::Statistics holds its stamp as a double, so the value derived from it is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data came from. Callers wanting the exact
+ *       input stamp — or a publication time unrelated to the data — stamp the header
+ *       themselves before or after this call.
+ * @warning The frame id is never set: rtabmap::Statistics does not carry one, so the
+ *          caller must always fill `info.header.frame_id` itself.
+ */
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info);
 
+
+//============================================================================
+// Features and landmarks
+// Keypoints, 2D/3D points, descriptors and environmental sensors.
+//============================================================================
+
+/** @brief Convert a Link message into a rtabmap::Link, including its 6x6 information matrix. */
 rtabmap::Link linkFromROS(const rtabmap_msgs::msg::Link & msg);
+/** @brief Fill a Link message from a rtabmap::Link. */
 void linkToROS(const rtabmap::Link & link, rtabmap_msgs::msg::Link & msg);
 
+/** @brief Convert a KeyPoint message into a cv::KeyPoint. */
 cv::KeyPoint keypointFromROS(const rtabmap_msgs::msg::KeyPoint & msg);
+/** @brief Fill a KeyPoint message from a cv::KeyPoint. */
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_msgs::msg::KeyPoint & msg);
 
+/** @brief Convert keypoint messages into a new vector of cv::KeyPoint. */
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
+
+/**
+ * @brief Append keypoint messages to an existing vector.
+ * @param[in]     msg    the messages to convert
+ * @param[in,out] kpts   vector the keypoints are appended to; existing content is kept
+ * @param[in]     xShift offset added to the x coordinate of every appended keypoint,
+ *                       used when several camera images are laid out side by side
+ */
 void keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift=0);
+
+/** @brief Fill keypoint messages from a vector of cv::KeyPoint. */
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
 
+/** @brief Convert a GlobalDescriptor message, decompressing its data and info matrices. */
 rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_msgs::msg::GlobalDescriptor & msg);
+/** @brief Fill a GlobalDescriptor message, compressing its data and info matrices. */
 void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_msgs::msg::GlobalDescriptor & msg);
 
+/** @brief Convert global descriptor messages into RTAB-Map descriptors. */
 std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
+/** @brief Fill global descriptor messages; @p msg is cleared first. */
 void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
 
+/** @brief Convert an EnvSensor message into a rtabmap::EnvSensor. */
 rtabmap::EnvSensor envSensorFromROS(const rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Fill an EnvSensor message from a rtabmap::EnvSensor. */
 void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Convert EnvSensor messages into a map keyed by sensor type. */
 rtabmap::EnvSensors envSensorsFromROS(const std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
+/** @brief Fill EnvSensor messages from a map of sensors; @p msg is cleared first. */
 void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
 
+/** @brief Convert a Point2f message into a cv::Point2f. */
 cv::Point2f point2fFromROS(const rtabmap_msgs::msg::Point2f & msg);
+/** @brief Fill a Point2f message from a cv::Point2f. */
 void point2fToROS(const cv::Point2f & kpt, rtabmap_msgs::msg::Point2f & msg);
 
+/** @brief Convert Point2f messages into a vector of cv::Point2f. */
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_msgs::msg::Point2f> & msg);
+/** @brief Fill Point2f messages from a vector of cv::Point2f. */
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_msgs::msg::Point2f> & msg);
 
+/** @brief Convert a Point3f message into a cv::Point3f. */
 cv::Point3f point3fFromROS(const rtabmap_msgs::msg::Point3f & msg);
+/** @brief Fill a Point3f message from a cv::Point3f. */
 void point3fToROS(const cv::Point3f & kpt, rtabmap_msgs::msg::Point3f & msg);
 
+/**
+ * @brief Convert Point3f messages into a vector of cv::Point3f.
+ * @param msg       the messages to convert
+ * @param transform applied to every point; ignored when null or identity
+ * @return the converted points
+ */
 std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Append Point3f messages to an existing vector.
+ * @param[in]     msg       the messages to convert
+ * @param[in,out] points3   vector the points are appended to; existing content is kept
+ * @param[in]     transform applied to every appended point; ignored when null or identity
+ */
 void points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Fill Point3f messages from a vector of cv::Point3f.
+ * @param[in]  kpts      the points to convert
+ * @param[out] msg       the converted messages
+ * @param[in]  transform applied to every point; ignored when null or identity
+ */
 void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
 
+
+//============================================================================
+// Camera models
+//============================================================================
+
+/**
+ * @brief Convert a CameraInfo message into a rtabmap::CameraModel.
+ *
+ * Fisheye/equidistant distortion (4 coefficients) is repacked into RTAB-Map's 1x6
+ * layout. A projection matrix means the model describes an already-rectified image.
+ *
+ * @param camInfo        the message to convert
+ * @param localTransform transform from the base frame to the optical frame
+ * @return the converted model
+ *
+ * @note `k`, `r` and `p` are fixed-size arrays and so are never empty. An unset matrix
+ *       is all zeros, which is detected through the focal length (`k[0]` / `p[0]`).
+ */
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill a CameraInfo message from a rtabmap::CameraModel.
+ *
+ * A model carrying a projection matrix describes a rectified image, so zero distortion
+ * is reported for it. Without one, `P` is synthesized as `[K | 0]` and the raw
+ * distortion coefficients are emitted (`equidistant` for a 1x6 fisheye matrix,
+ * `rational_polynomial` above 5 coefficients, `plumb_bob` otherwise).
+ *
+ * @param[in]  model   the model to convert
+ * @param[out] camInfo the converted message; the header is not set
+ */
 void cameraModelToROS(
 		const rtabmap::CameraModel & model,
 		sensor_msgs::msg::CameraInfo & camInfo);
 
+/**
+ * @brief Build a stereo model from a pair of CameraInfo messages.
+ * @param leftCamInfo     left camera info
+ * @param rightCamInfo    right camera info; the baseline is read from its `P(0,3)`
+ * @param localTransform  transform from the base frame to the left optical frame
+ * @param stereoTransform explicit left-to-right transform, when not encoded in `P`
+ * @return the converted model
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity(),
 		const rtabmap::Transform & stereoTransform = rtabmap::Transform());
+
+/**
+ * @brief Build a stereo model, resolving the local transform from TF.
+ * @param leftCamInfo      left camera info
+ * @param rightCamInfo     right camera info
+ * @param frameId          base frame the model's local transform is expressed in
+ * @param tfBuffer         must contain @p frameId -> the left camera info's frame at
+ *                         its stamp
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the converted model, invalid if the transform could not be resolved
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
@@ -148,12 +446,34 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Map graph
+// Poses, links, nodes and sensor data — the map serialization path.
+//============================================================================
+
+/**
+ * @brief Read a MapData message into poses, links and signatures.
+ * @param[in]  msg        the message to convert
+ * @param[out] poses      optimized poses by node id
+ * @param[out] links      constraints, keyed by their originating node id
+ * @param[out] signatures node data by node id
+ * @param[out] mapToOdom  transform from the map frame to the odometry frame
+ */
 void mapDataFromROS(
 		const rtabmap_msgs::msg::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		std::map<int, rtabmap::Signature> & signatures,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapData message from poses, links and signatures.
+ * @param[in]  poses      optimized poses by node id
+ * @param[in]  links      constraints
+ * @param[in]  signatures node data by node id
+ * @param[in]  mapToOdom  transform from the map frame to the odometry frame
+ * @param[out] msg        the converted message; the header is not set
+ */
 void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
@@ -161,40 +481,159 @@ void mapDataToROS(
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapData & msg);
 
+/**
+ * @brief Read a MapGraph message into poses and links.
+ * @param[in]  msg       the message to convert
+ * @param[out] poses     optimized poses by node id
+ * @param[out] links     constraints, keyed by their originating node id
+ * @param[out] mapToOdom transform from the map frame to the odometry frame
+ */
 void mapGraphFromROS(
 		const rtabmap_msgs::msg::MapGraph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapGraph message from poses and links.
+ * @param[in]  poses     optimized poses by node id
+ * @param[in]  links     constraints
+ * @param[in]  mapToOdom transform from the map frame to the odometry frame
+ * @param[out] msg       the converted message; the header is not set
+ */
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapGraph & msg);
 
+/**
+ * @brief Convert a SensorData message into a rtabmap::SensorData.
+ * @param msg the message to convert
+ * @return the converted sensor data
+ * @note `ground_truth_pose` is not read here; nodeFromROS() owns that field.
+ */
 rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg);
+
+/**
+ * @brief Fill a SensorData message from a rtabmap::SensorData.
+ * @param[in]  signature   the sensor data to convert
+ * @param[out] msg         the converted message
+ * @param[in]  frameId     frame id stamped on the message
+ * @param[in]  copyRawData also serialize the uncompressed images and laser scan, which
+ *                         is significantly larger on the wire
+ */
 void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false);
 
+/**
+ * @brief Convert a Node message into a rtabmap::Signature, with its data and visual words.
+ * @param msg the message to convert
+ * @return the converted signature
+ */
 rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg);
+
+/**
+ * @brief Fill a Node message from a rtabmap::Signature.
+ * @param[in]  signature the signature to convert
+ * @param[out] msg       the converted message
+ */
 void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
-// DEPRECATED
+/** @deprecated Use nodeFromROS() instead. */
 rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @deprecated Use nodeToROS() instead. */
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+/** @brief Convert only the node's metadata (id, map id, weight, stamp, label, pose). */
 rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @brief Fill only the node's metadata (id, map id, weight, stamp, label, pose). */
 void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+
+//============================================================================
+// Odometry
+//============================================================================
+
+/**
+ * @brief Format odometry info as the `Odometry/...` statistics published with the map.
+ * @param info the odometry info to summarize
+ * @return statistic name (with its unit) to value
+ * @note The covariance-derived entries are omitted when `info.reg.covariance` is not a
+ *       6x6 CV_64FC1 matrix, which is the case for a default-constructed OdometryInfo.
+ */
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
+
+/**
+ * @brief Convert an OdomInfo message into a rtabmap::OdometryInfo.
+ * @param msg        the message to convert
+ * @param ignoreData skip the heavy members (words, local map, correspondences)
+ * @return the converted odometry info
+ */
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
+
+/**
+ * @brief Fill an OdomInfo message from a rtabmap::OdometryInfo.
+ * @param[in]  info       the odometry info to convert
+ * @param[out] msg        the converted message
+ * @param[in]  ignoreData skip the heavy members (words, local map, correspondences)
+ */
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
 
+
+//============================================================================
+// User data, IMU and landmarks
+//============================================================================
+
+/**
+ * @brief Extract the payload of a UserData message.
+ * @param dataMsg the message to read
+ * @return the payload; still compressed when the message was written with compression,
+ *         in which case the caller applies rtabmap::uncompressData()
+ */
 cv::Mat userDataFromROS(const rtabmap_msgs::msg::UserData & dataMsg);
+
+/**
+ * @brief Fill a UserData message.
+ * @param[in]  data     the payload
+ * @param[out] dataMsg  the converted message
+ * @param[in]  compress compress the payload, which is then carried as a 1xN byte blob
+ */
 void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg, bool compress);
 
+/**
+ * @brief Convert an Imu message into a rtabmap::IMU.
+ * @param msg            the message to convert
+ * @param localTransform transform from the base frame to the IMU frame
+ * @return the converted IMU sample, with its three covariance matrices
+ */
 rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill an Imu message from a rtabmap::IMU.
+ * @param[in]  imu the IMU sample to convert
+ * @param[out] msg the converted message; the header is not set
+ */
 void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg);
 
+/**
+ * @brief Convert tag/landmark detections into RTAB-Map landmarks, expressed in @p frameId.
+ *
+ * Each detection is transformed from its own frame into @p frameId, then corrected for
+ * the odometry motion between @p odomStamp and the detection's stamp.
+ *
+ * @param tags               detections by landmark id, each paired with its tag size;
+ *                           ids must be > 0, others are dropped with an error
+ * @param frameId            base frame the landmarks are expressed in
+ * @param odomFrameId        fixed frame used for the odometry correction; when empty
+ *                           no correction is applied
+ * @param odomStamp          stamp the landmarks should be synchronized to
+ * @param tfBuffer           must contain @p frameId -> each detection's frame at that
+ *                           detection's stamp, and, when @p odomFrameId is set,
+ *                           @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform   seconds to wait for TF, 0 to not wait
+ * @param defaultLinVariance linear variance used when a detection carries no covariance
+ * @param defaultAngVariance angular variance used when a detection carries no covariance
+ * @return the landmarks, keyed by id
+ */
 rtabmap::Landmarks landmarksFromROS(
 		const std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,
@@ -205,10 +644,43 @@ rtabmap::Landmarks landmarksFromROS(
 		double defaultLinVariance,
 		double defaultAngVariance);
 
-inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
-inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));}
 
-// common stuff
+//============================================================================
+// Timestamps
+//============================================================================
+
+/**
+ * @brief Convert a ROS time into seconds.
+ * @note A double holds about 15-16 significant digits, so at current epoch times
+ *       (~1.7e9 s) it resolves to roughly 400 ns. Converting back with timestampToROS()
+ *       therefore does not reproduce the original stamp exactly, and the rounding can
+ *       carry into the seconds field. Compare converted stamps with a tolerance, and
+ *       keep the original rclcpp::Time whenever exactness matters.
+ */
+inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
+/**
+ * @brief Convert seconds into a ROS time.
+ * @note The result uses RCL_ROS_TIME, matching how message header stamps convert. The
+ *       rclcpp::Time(sec, nsec) constructor defaults to RCL_SYSTEM_TIME instead, and
+ *       comparing times of different clock types throws.
+ */
+inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9), RCL_ROS_TIME);}
+
+
+//============================================================================
+// TF lookups
+//============================================================================
+
+/**
+ * @brief Look a static relationship between two frames up in TF.
+ * @param fromFrameId      the reference frame
+ * @param toFrameId        the target frame
+ * @param stamp            time of the lookup
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the transform, or a null transform if the lookup failed (which is logged
+ *         rather than thrown)
+ */
 rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,
@@ -216,9 +688,19 @@ rtabmap::Transform getTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
-
-// get moving transform accordingly to a fixed frame. For example get
-// transform of /base_link between two stamps accordingly to /odom frame.
+/**
+ * @brief Measure how a frame moved between two stamps, relative to a fixed frame.
+ *
+ * For example, the motion of `base_link` between two stamps as seen from `odom`.
+ *
+ * @param movingFrame      the frame whose motion is measured
+ * @param fixedFrame       the frame the motion is measured against
+ * @param stampFrom        start of the interval
+ * @param stampTo          end of the interval
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the motion, or a null transform if the lookup failed
+ */
 rtabmap::Transform getMovingTransform(
 		const std::string & movingFrame,
 		const std::string & fixedFrame,
@@ -227,6 +709,54 @@ rtabmap::Transform getMovingTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Sensor message conversion
+// Assembling RGB-D, stereo and laser scan messages into RTAB-Map inputs.
+//============================================================================
+
+/**
+ * @brief Assemble one or more RGB-D (or RGB + right) camera streams into RTAB-Map inputs.
+ *
+ * With several cameras the images are concatenated horizontally into a single wide
+ * image and one model is produced per camera. Whether the second image is treated as a
+ * depth map or as the right image of a stereo pair is inferred from its encoding, and
+ * for `mono16` from whether the camera infos carry a baseline in `P(0,3)`.
+ *
+ * @param imageMsgs            RGB (or left) images, one per camera; may be empty
+ * @param depthMsgs            depth (or right) images, one per camera; may be empty
+ * @param cameraInfoMsgs       camera infos, one per camera; must not be empty
+ * @param depthCameraInfoMsgs  camera infos of the depth/right cameras; may be empty
+ * @param frameId              base frame the local transforms are expressed in
+ * @param odomFrameId          fixed frame the robot motion is measured against, used to
+ *                             re-express each camera pose relative to the base frame at
+ *                             @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp            stamp the data is synchronized to
+ * @param[out] rgb             the assembled RGB (or left) image
+ * @param[out] depth           the assembled depth (or right) image
+ * @param[out] cameraModels    one model per camera, when the input is RGB-D
+ * @param[out] stereoCameraModels one model per camera, when the input is stereo
+ * @param tfBuffer             must contain @p frameId -> each camera's optical frame at
+ *                             that camera's stamp, and, when @p odomFrameId is set,
+ *                             @p odomFrameId -> @p frameId covering both @p odomStamp
+ *                             and the camera stamps
+ * @param waitForTransform     seconds to wait for TF, 0 to not wait
+ * @param alreadRectifiedImages whether the images are already rectified
+ * @param localKeyPointsMsgs   optional per-camera keypoints to merge
+ * @param localPoints3dMsgs    optional per-camera 3D points to merge
+ * @param localDescriptorsMsgs optional per-camera descriptors to merge
+ * @param[out] localKeyPoints  merged keypoints, shifted to the concatenated image
+ * @param[out] localPoints3d   merged 3D points
+ * @param[out] localDescriptors merged descriptors
+ * @return false on an unsupported encoding or a missing camera local transform
+ *
+ * @note The odometry correction is applied per camera, using each camera's own stamp,
+ *       and only when it differs from @p odomStamp. If that lookup fails the function
+ *       warns and carries on with an uncorrected pose — unlike a missing camera local
+ *       transform, which is fatal and returns false.
+ * @note A camera's RGB and depth stamps are assumed to be equal. Should they differ,
+ *       the depth stamp is the one used, since the geometry is what gets synchronized.
+ */
 bool convertRGBDMsgs(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
@@ -249,6 +779,34 @@ bool convertRGBDMsgs(
 		std::vector<cv::Point3f> * localPoints3d = 0,
 		cv::Mat * localDescriptors = 0);
 
+/**
+ * @brief Convert a stereo pair into RTAB-Map inputs.
+ *
+ * The left image keeps its color; the right image is always reduced to mono.
+ *
+ * @param leftImageMsg     left image
+ * @param rightImageMsg    right image
+ * @param leftCamInfoMsg   left camera info
+ * @param rightCamInfoMsg  right camera info; the baseline is read from its `P(0,3)`
+ * @param frameId          base frame the local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the camera pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the data is synchronized to
+ * @param[out] left        the left image
+ * @param[out] right       the right image, as mono
+ * @param[out] stereoModel the stereo model
+ * @param tfBuffer         must contain @p frameId -> the left image's frame at the left
+ *                         image stamp, and, when @p odomFrameId is set,
+ *                         @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param alreadyRectified whether the images are already rectified
+ * @return false on an unsupported encoding or a missing local transform
+ *
+ * @note The odometry correction is applied only when the left image stamp differs from
+ *       @p odomStamp. A failed correction lookup warns and leaves the pose uncorrected;
+ *       a missing local transform is fatal and returns false.
+ */
 bool convertStereoMsg(
 		const cv_bridge::CvImageConstPtr& leftImageMsg,
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
@@ -264,6 +822,36 @@ bool convertStereoMsg(
 		double waitForTransform,
 		bool alreadyRectified);
 
+/**
+ * @brief Convert a 2D LaserScan into a rtabmap::LaserScan.
+ * @param scan2dMsg        the scan to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the laser frame at the scan stamp,
+ *                         and, to deskew, the laser frame relative to @p odomFrameId
+ *                         across the whole sweep, since the points are projected through
+ *                         it
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param outputInFrameId  express the points in @p frameId rather than the laser frame
+ * @return false if the scan is malformed (zero angle increment, inverted range or angle
+ *         bounds) or if a required transform is missing
+ *
+ * @note Unlike convertScan3dMsg(), this deskews the scan itself: the points are
+ *       projected with laser_geometry, which transforms each ray at its own time using
+ *       @p scan2dMsg.time_increment. That only corrects for motion if the projection
+ *       target is a fixed frame, i.e. if @p odomFrameId is set — with it empty the
+ *       target is @p frameId, which does not move relative to itself. This is also why
+ *       the laser frame must be known across the whole sweep, which the function checks
+ *       up front. When it is not known relative to @p odomFrameId -- odometry not
+ *       published on TF -- the scan is converted as with @p odomFrameId empty, neither
+ *       deskewed nor synchronized, with a warning shown once, rather than refused.
+ * @note The odometry correction is applied only when the scan stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ */
 bool convertScanMsg(
 		const sensor_msgs::msg::LaserScan & scan2dMsg,
 		const std::string & frameId,
@@ -274,6 +862,32 @@ bool convertScanMsg(
 		double waitForTransform,
 		bool outputInFrameId = false);
 
+/**
+ * @brief Convert a PointCloud2 into a rtabmap::LaserScan.
+ * @param scan3dMsg        the cloud to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the cloud's frame at the cloud
+ *                         stamp, and, when @p odomFrameId is set, @p odomFrameId ->
+ *                         @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param maxPoints        downsample to at most this many points, 0 for no limit
+ * @param maxRange         drop points beyond this range, 0 for no limit
+ * @param is2D             treat the cloud as planar
+ * @return false if the local transform could not be resolved
+ *
+ * @note The cloud is assumed to be already deskewed. A single rigid transform is applied
+ *       to the whole cloud, so any motion during the sweep is preserved as-is; call
+ *       deskew() on the message first if the sensor was moving. This is unlike
+ *       convertScanMsg(), which deskews 2D scans itself through laser_geometry.
+ * @note The odometry correction is applied only when the cloud stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ * @see deskew()
+ */
 bool convertScan3dMsg(
 		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 		const std::string & frameId,
@@ -286,6 +900,29 @@ bool convertScan3dMsg(
 		float maxRange = 0.0f,
 		bool is2D = false);
 
+
+//============================================================================
+// Point cloud utilities
+//============================================================================
+
+/**
+ * @brief Deskew a point cloud using TF.
+ *
+ * Corrects each point for the sensor motion during the sweep, using the per-point time
+ * channel (`t`, `time`, `stamps` or `timestamp`). See the other overload for how that
+ * channel is interpreted.
+ *
+ * @param input            the cloud to deskew
+ * @param[out] output      the deskewed cloud, expressed in the frame at input's header stamp
+ * @param fixedFrameId     frame the sensor motion is measured against
+ * @param tfBuffer         must contain the cloud's own frame relative to
+ *                         @p fixedFrameId across the whole sweep
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param slerp            interpolate between the sweep's two end poses instead of
+ *                         looking TF up for every point; one query instead of N, at the
+ *                         cost of linearizing the motion across the sweep
+ * @return false if the cloud has no usable time channel or a lookup failed
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
@@ -294,21 +931,49 @@ bool deskew(
 		double waitForTransform,
 		bool slerp = false);
 
+/**
+ * @brief Deskew a point cloud using a constant velocity model.
+ *
+ * The per-point time channel may be named `t`, `time`, `stamps` or `timestamp`. Its
+ * datatype decides how it is read: `UINT32` (nanoseconds) and `FLOAT32` (seconds) are
+ * *offsets from the message header stamp*, while `FLOAT64` carries *absolute* stamps,
+ * with milliseconds/microseconds/nanoseconds detected automatically by magnitude.
+ *
+ * On success the channel is zeroed to mark the cloud as deskewed, so calling this again
+ * on the same cloud is a no-op that returns true rather than an error.
+ *
+ * @param input        cloud with a per-point time channel
+ * @param[out] output  deskewed cloud, expressed in the frame at input's header stamp
+ * @param velocity     twist of the sensor frame (m/s and rad/s)
+ * @return false if the cloud has no usable time channel or @p velocity is null
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity);
 
-// Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Apply a rigid transform to the XYZ fields of a point cloud.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param transform the transform to apply
+ * @param in        the cloud to transform
+ * @param[out] out  the transformed cloud; all other fields are copied unchanged
+ */
 void transformPointCloud (
 		const Eigen::Matrix4f &transform,
 		const sensor_msgs::msg::PointCloud2 &in,
         sensor_msgs::msg::PointCloud2 &out);
 
-/** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
- * @param datatype one of the enums of sensor_msgs::PointField::
- * Note: Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Return the size in bytes of a PointField datatype.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param datatype one of the sensor_msgs::msg::PointField enums
+ * @return the size in bytes
+ * @throws std::runtime_error if @p datatype is not a known PointField type
  */
 inline int sizeOfPointField(int datatype)
 {
@@ -330,6 +995,13 @@ inline int sizeOfPointField(int datatype)
   return -1;
 }
 
+/**
+ * @brief Find the entry of a map whose key is closest to @p key.
+ * @param buffer the map to search; must not be empty
+ * @param key    the key to look for
+ * @return iterator to the closest entry, clamped to the first or last one when @p key
+ *         falls outside the map's range
+ */
 template <typename K, typename V>
 typename std::map<K, V>::const_iterator getClosestIterator(
 	const std::map<K, V> & buffer,
@@ -365,6 +1037,7 @@ typename std::map<K, V>::const_iterator getClosestIterator(
 	return iterB;
 }
 
+
 }
 
 #endif /* MSGCONVERSION_H_ */
diff --git a/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h b/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h
new file mode 100644
index 00000000..95d898a5
--- /dev/null
+++ b/rtabmap_conversions/include/rtabmap_conversions/PointCloudConversion.h
@@ -0,0 +1,75 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
+#define RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+/**
+ * @file
+ * @brief pcl::toROSMsg and pcl::fromROSMsg, minus their empty-cloud crash.
+ *
+ * Both take the address of the first point, and of the first byte of the output, before
+ * checking that there is one (see pcl/conversions.h): for an empty cloud that indexes
+ * past the end of an empty vector. Nothing notices while the standard library does not
+ * check, which is why it went unseen for years -- Ubuntu enables those checks from
+ * resolute on, and then the process aborts outright.
+ *
+ * An empty cloud is ordinary here rather than exceptional: a scan whose points were all
+ * filtered out, a frame with no obstacles in it, an occupancy grid with nothing new. Each
+ * of those still has to be published, so the conversions are used through this.
+ */
+
+namespace rtabmap_conversions {
+
+/**
+ * @brief @p cloud as a PointCloud2 message.
+ *
+ * An empty cloud is converted as a single point and emptied afterwards, so the message
+ * still carries the field layout the installed PCL would have given it.
+ */
+template<typename PointT>
+void toPointCloud2Msg(
+		const pcl::PointCloud<PointT> & cloud, sensor_msgs::msg::PointCloud2 & msg)
+{
+	if(!cloud.empty())
+	{
+		pcl::toROSMsg(cloud, msg);
+		return;
+	}
+
+	pcl::PointCloud<PointT> onePoint;
+	onePoint.header = cloud.header;
+	onePoint.is_dense = cloud.is_dense;
+	onePoint.push_back(PointT());
+	pcl::toROSMsg(onePoint, msg);
+	msg.width = 0;
+	msg.height = 1;
+	msg.row_step = 0;
+	msg.data.clear();
+}
+
+/// @brief @p msg as a point cloud, an empty message included.
+template<typename PointT>
+void fromPointCloud2Msg(
+		const sensor_msgs::msg::PointCloud2 & msg, pcl::PointCloud<PointT> & cloud)
+{
+	if(msg.data.empty())
+	{
+		cloud.clear();
+		cloud.is_dense = msg.is_dense;
+		pcl_conversions::toPCL(msg.header, cloud.header);
+		return;
+	}
+	pcl::fromROSMsg(msg, cloud);
+}
+
+}  // namespace rtabmap_conversions
+
+#endif /* RTABMAP_CONVERSIONS_POINTCLOUDCONVERSION_H_ */
diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml
index 9ad6cadd..19627ae1 100644
--- a/rtabmap_conversions/package.xml
+++ b/rtabmap_conversions/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_conversions</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -30,7 +30,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>
diff --git a/rtabmap_conversions/rosdoc2.yaml b/rtabmap_conversions/rosdoc2.yaml
new file mode 100644
index 00000000..bcd0f339
--- /dev/null
+++ b/rtabmap_conversions/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_conversions
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_conversions --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_conversions 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_conversions',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }
diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp
index 39dabb6e..6da75420 100644
--- a/rtabmap_conversions/src/MsgConversion.cpp
+++ b/rtabmap_conversions/src/MsgConversion.cpp
@@ -25,8 +25,12 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_conversions/MsgConversion.h"
 
+#include <cmath>
+#include <limits>
+
 #include <opencv2/highgui/highgui.hpp>
 #include <zlib.h>
 #include "rclcpp/rclcpp.hpp"
@@ -60,21 +64,46 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
 {
-	if(!transform.isNull())
+	if(transform.isNull())
 	{
-		geometry_msgs::msg::TransformStamped gm = tf2::eigenToTransform(transform.toEigen3d());
-		//tf2::fromMsg(gm, tfTransform);
-	}
-	else
-	{
-		tfTransform = tf2::Transform(tf2::Quaternion(0,0,0,0));
+		// tf2::Transform cannot represent a null transform: it stores its rotation as a
+		// basis matrix, so there is no equivalent of the all-zero quaternion used by the
+		// geometry_msgs conversions. Fill it with NaN so that a caller ignoring the
+		// return value corrupts its results loudly instead of silently carrying on with
+		// an identity that looks legitimate.
+		const tf2Scalar nan = std::numeric_limits<tf2Scalar>::quiet_NaN();
+		tfTransform = tf2::Transform(
+				tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan),
+				tf2::Vector3(nan, nan, nan));
+		return false;
 	}
+
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(transform, msg);
+	tf2::fromMsg(msg, tfTransform);
+	return true;
 }
 
 rtabmap::Transform transformFromTF(const tf2::Transform & transform)
 {
+	// transformToTF() poisons its output with NaN for a null transform, as tf2::Transform
+	// has no null representation of its own. Map that back to a null transform here so the
+	// two functions round-trip, and so a NaN coming from anywhere else does not silently
+	// propagate into the rest of the pipeline.
+	const tf2::Vector3 & origin = transform.getOrigin();
+	const tf2::Matrix3x3 & basis = transform.getBasis();
+	bool nan = std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z());
+	for(int i=0; !nan && i<3; ++i)
+	{
+		nan = std::isnan(basis[i].x()) || std::isnan(basis[i].y()) || std::isnan(basis[i].z());
+	}
+	if(nan)
+	{
+		return rtabmap::Transform();
+	}
+
 	Eigen::Isometry3d eigenTf;
 	geometry_msgs::msg::Transform gm = tf2::toMsg(transform);
 	eigenTf = tf2::transformToEigen(gm);
@@ -227,6 +256,11 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr
 				depth = ptr;
 			}
 		}
+		else
+		{
+			// empty
+			depth = std::make_shared<cv_bridge::CvImage>();
+		}
 	}
 	catch(cv::Exception& e) {
 		UFATAL("Fatal error while converting rgbd image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what());
@@ -244,6 +278,7 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma
 		UERROR("Cannot convert multi-camera data to rgbd image");
 		return;
 	}
+	msg.header = header;
 	if(data.cameraModels().size() == 1)
 	{
 		//rgb+depth
@@ -403,7 +438,11 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 		int depthWidth = depthMsg->image.cols;
 		int depthHeight = depthMsg->image.rows;
 
+		// The depth image is optional: a message can legitimately carry only the color
+		// image and its camera info. Compare the resolutions only when there is a depth
+		// image, otherwise the ratios divide by zero.
 		UASSERT_MSG(
+			depthMsg->image.empty() ||
 			imageWidth/depthWidth == imageHeight/depthHeight,
 			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
 
@@ -419,7 +458,8 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
-			!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			!(depthMsg->image.empty() ||
+			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 			 depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
 		{
@@ -558,6 +598,13 @@ void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & sta
 
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info)
 {
+	// Fall back to the statistics' own stamp when the caller left the header unstamped.
+	// Callers that already stamped it keep their value, which may be a publication time
+	// unrelated to the data, or the exact input stamp rather than this double-derived one.
+	if(info.header.stamp.sec == 0 && info.header.stamp.nanosec == 0)
+	{
+		info.header.stamp = timestampToROS(stats.stamp());
+	}
 	info.ref_id = stats.refImageId();
 	info.loop_closure_id = stats.loopClosureId();
 	info.proximity_detection_id = stats.proximityDetectionId();
@@ -829,9 +876,12 @@ rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
 {
+	// Note: k, r and p are fixed-size arrays in the ROS message, so they are never
+	// empty and their size is always right. An unset matrix is signalled by all-zero
+	// content instead: k[0] and p[0] hold the focal length, which is always non-zero
+	// for a valid calibration, and an unset rectification matrix is all zeros.
 	cv:: Mat K;
-	UASSERT(camInfo.k.empty() || camInfo.k.size() == 9);
-	if(!camInfo.k.empty())
+	if(camInfo.k[0] != 0.0)
 	{
 		K = cv::Mat(3, 3, CV_64FC1);
 		memcpy(K.data, camInfo.k.data(), 9*sizeof(double));
@@ -858,17 +908,22 @@ rtabmap::CameraModel cameraModelFromROS(
 		}
 	}
 
+	// R is a rotation matrix, so any of its elements can legitimately be zero: only
+	// an entirely zero matrix means "not set".
 	cv:: Mat R;
-	UASSERT(camInfo.r.empty() || camInfo.r.size() == 9);
-	if(!camInfo.r.empty())
+	bool rIsSet = false;
+	for(size_t i=0; !rIsSet && i<camInfo.r.size(); ++i)
+	{
+		rIsSet = camInfo.r[i] != 0.0;
+	}
+	if(rIsSet)
 	{
 		R = cv::Mat(3, 3, CV_64FC1);
 		memcpy(R.data, camInfo.r.data(), 9*sizeof(double));
 	}
 
 	cv:: Mat P;
-	UASSERT(camInfo.p.empty() || camInfo.p.size() == 12);
-	if(!camInfo.p.empty())
+	if(camInfo.p[0] != 0.0)
 	{
 		P = cv::Mat(3, 4, CV_64FC1);
 		memcpy(P.data, camInfo.p.data(), 12*sizeof(double));
@@ -942,8 +997,9 @@ void cameraModelToROS(
 	{
 		memset(camInfo.p.data(), 0.0, 12*sizeof(double));
 		if(!model.K_raw().empty()) {
+			// P = [K | 0]: copying K already sets the homogeneous P(2,2)=1, and the
+			// fourth column (the Tx/Ty/Tz translation) stays zero for a single camera.
 			model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3));
-			camInfo.p.back() = 1.0;
 		}
 	}
 	else
@@ -1249,7 +1305,8 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg)
 		pcl::PCLPointCloud2 cloud;
 		pcl_conversions::toPCL(msg.laser_scan, cloud);
 		s.setLaserScan(rtabmap::LaserScan(
-			rtabmap::util3d::laserScanFromPointCloud(cloud),
+			rtabmap::util3d::laserScanFromPointCloud(cloud, true,
+					rtabmap::LaserScan::isScan2d((rtabmap::LaserScan::Format)msg.laser_scan_format)),
 			msg.laser_scan_max_pts,
 			msg.laser_scan_max_range,
 			transformFromGeometryMsg(msg.laser_scan_local_transform)),
@@ -1352,10 +1409,13 @@ void sensorDataToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::Sensor
 	{
 		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw());
 		pcl_conversions::moveFromPCL(*cloud, msg.laser_scan);
-		msg.laser_scan_max_pts = data.laserScanCompressed().maxPoints();
-		msg.laser_scan_max_range = data.laserScanCompressed().rangeMax();
-		msg.laser_scan_format = data.laserScanCompressed().format();
-		transformToGeometryMsg(data.laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
+		// Describe the scan we just serialized: reading these from laserScanCompressed()
+		// zeroes them whenever only the raw scan is set, and sensorDataFromROS() then
+		// fails its format assertion.
+		msg.laser_scan_max_pts = data.laserScanRaw().maxPoints();
+		msg.laser_scan_max_range = data.laserScanRaw().rangeMax();
+		msg.laser_scan_format = data.laserScanRaw().format();
+		transformToGeometryMsg(data.laserScanRaw().localTransform(), msg.laser_scan_local_transform);
 	}
 	if(!data.laserScanCompressed().empty())
 	{
@@ -1624,10 +1684,17 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
 	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", info.reg.icpStructuralDistribution));
 	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", info.reg.icpCorrespondences));
-	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
-	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
-	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
-	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	// RegistrationInfo leaves covariance empty by default, so only read it when the
+	// expected 6x6 matrix is actually there.
+	if(info.reg.covariance.type() == CV_64FC1 &&
+	   info.reg.covariance.rows == 6 &&
+	   info.reg.covariance.cols == 6)
+	{
+		stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
+		stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
+		stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
+		stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	}
 	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
 	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
 	stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
@@ -2648,13 +2715,41 @@ bool convertScanMsg(
 	}
 
 	// make sure the frame of the laser is updated during the whole scan time
-	rtabmap::Transform tmpT = getMovingTransform(
-			scan2dMsg.header.frame_id,
-			odomFrameId.empty()?frameId:odomFrameId,
-			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec),
-			rclcpp::Time(scan2dMsg.header.stamp.sec, scan2dMsg.header.stamp.nanosec) + rclcpp::Duration::from_seconds(scan2dMsg.ranges.size()*scan2dMsg.time_increment),
-			tfBuffer,
-			waitForTransform);
+	const rclcpp::Time scanStart(scan2dMsg.header.stamp);
+	const rclcpp::Time scanEnd = scanStart + rclcpp::Duration::from_seconds((scan2dMsg.ranges.empty()?0:scan2dMsg.ranges.size()-1)*scan2dMsg.time_increment);
+	std::string fixedFrameId = odomFrameId.empty()?frameId:odomFrameId;
+	rtabmap::Transform tmpT;
+	if(fixedFrameId == frameId || tfBuffer._frameExists(fixedFrameId)) // don't wait for a frame never published
+	{
+		tmpT = getMovingTransform(
+				scan2dMsg.header.frame_id,
+				fixedFrameId,
+				scanStart,
+				scanEnd,
+				tfBuffer,
+				waitForTransform);
+	}
+	if(tmpT.isNull() && fixedFrameId != frameId)
+	{
+		// Odometry not in TF: use the scan as it is rather than dropping it.
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("Could not get laser frame \"%s\" relative to odometry frame \"%s\" over the scan "
+				  "(%fs to %fs). Laser scans are used without deskewing nor synchronization with "
+				  "odometry. Publish odometry on TF to have them deskewed. This message is only shown once.",
+				  scan2dMsg.header.frame_id.c_str(), odomFrameId.c_str(), scanStart.seconds(), scanEnd.seconds());
+			warned = true;
+		}
+		fixedFrameId = frameId;
+		tmpT = getMovingTransform(
+				scan2dMsg.header.frame_id,
+				fixedFrameId,
+				scanStart,
+				scanEnd,
+				tfBuffer,
+				waitForTransform);
+	}
 	if(tmpT.isNull())
 	{
 		return false;
@@ -2674,12 +2769,12 @@ bool convertScanMsg(
 	//transform in frameId_ frame
 	sensor_msgs::msg::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
-	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, scan2dMsg, scanOut, tfBuffer);
+	projection.transformLaserScanToPointCloud(fixedFrameId, scan2dMsg, scanOut, tfBuffer);
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
 			scan2dMsg.header.frame_id,
-			odomFrameId.empty()?frameId:odomFrameId,
+			fixedFrameId,
 			scan2dMsg.header.stamp,
 			tfBuffer,
 			waitForTransform);
@@ -2689,7 +2784,7 @@ bool convertScanMsg(
 	}
 
 	// sync with odometry stamp
-	if(!odomFrameId.empty() && odomStamp != scan2dMsg.header.stamp)
+	if(fixedFrameId != frameId && odomStamp != scan2dMsg.header.stamp)
 	{
 		rtabmap::Transform sensorT = getMovingTransform(
 				frameId,
@@ -2743,7 +2838,7 @@ bool convertScanMsg(
 	if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
 		format = rtabmap::LaserScan::kXYI;
@@ -2751,7 +2846,7 @@ bool convertScanMsg(
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
 		format = rtabmap::LaserScan::kXY;
@@ -2832,8 +2927,7 @@ bool deskew_impl(
 		tf2_ros::Buffer * tfBuffer,
 		double waitForTransform,
 		bool slerp,
-		const rtabmap::Transform & velocity,
-		double previousStamp)
+		const rtabmap::Transform & velocity)
 {
 	if(tfBuffer != 0)
 	{
@@ -2857,12 +2951,6 @@ bool deskew_impl(
 			return false;
 		}
 
-		if(previousStamp <= 0.0)
-		{
-			UERROR("previousStamp should be >0 when constant velocity model is used!");
-			return false;
-		}
-
 		if(velocity.isNull())
 		{
 			UERROR("velocity should be valid when constant velocity model is used!");
@@ -3133,8 +3221,23 @@ bool deskew_impl(
 	}
 	else if(lastStamp == firstStamp)
 	{
-		UERROR("First and last stamps in the scan are the same (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
-		return false;
+		// There is no time spread across the scan, so there is nothing to correct. This
+		// happens when the driver doesn't fill the per-point time channel, and also when
+		// the cloud has already been deskewed: deskewing zeroes that channel to mark it.
+		// Pass the cloud through unchanged so that deskewing twice is a no-op rather than
+		// a failure that makes the caller drop the frame.
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("First and last stamps in the scan are the same (%f) (header=%f), the "
+				  "cloud is returned unchanged. Either the time channel is not filled by "
+				  "the driver, or the cloud has already been deskewed. This warning is "
+				  "only shown once.",
+				  timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
+			warned = true;
+		}
+		output = input;
+		return true;
 	}
 	std::string errorMsg;
 	if(tfBuffer != 0 &&
@@ -3184,23 +3287,19 @@ bool deskew_impl(
 			float vx,vy,vz, vroll,vpitch,vyaw;
 			velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 
-			// We need three poses:
-			//  1- The pose of base frame in odom frame at first stamp
-			//  2- The pose of base frame in odom frame at msg stamp
-			//  3- The pose of base frame in odom frame at last stamp
-			UASSERT(timestampFromROS(firstStamp) >= previousStamp);
-			UASSERT(timestampFromROS(lastStamp) > previousStamp);
-			double dt1 = timestampFromROS(firstStamp) - previousStamp;
-			double dt2 = timestampFromROS(input.header.stamp) - previousStamp;
-			double dt3 = timestampFromROS(lastStamp) - previousStamp;
-
-			rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
-			rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
-			rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
+			// Integrate the velocity directly from the stamp of the msg, which is the
+			// frame the deskewed cloud is expressed in. Going through a third, earlier
+			// reference pose and composing it away would give the same answer for a pure
+			// translation, but not for a rotation: Transform() scales roll/pitch/yaw
+			// linearly instead of using the twist exponential, so the composition only
+			// cancels in the small-angle limit. Keeping dt bounded by the scan duration
+			// is where that approximation is at its best.
+			double dt1 = timestampFromROS(firstStamp) - timestampFromROS(input.header.stamp);
+			double dt3 = timestampFromROS(lastStamp) - timestampFromROS(input.header.stamp);
 
 			// First and last poses are relative to stamp of the msg
-			firstPose = p2.inverse() * p1;
-			lastPose = p2.inverse() * p3;
+			firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+			lastPose = rtabmap::Transform(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
 		}
 
 		if(firstPose.isNull())
@@ -3227,6 +3326,7 @@ bool deskew_impl(
 
 	output = input;
 	rclcpp::Time stamp;
+	bool clampWarned = false;   // reported once per cloud, see the clamp below
 	UTimer processingTime;
 	if(timeOnColumns)
 	{
@@ -3272,7 +3372,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3366,7 +3486,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3428,16 +3568,15 @@ bool deskew(
 		double waitForTransform,
 		bool slerp)
 {
-	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0);
+	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform());
 }
 
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity)
 {
-	return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
+	return deskew_impl(input, output, "", 0, 0, true, velocity);
 }
 
 
@@ -3493,8 +3632,15 @@ transformPointCloud (
     Eigen::Vector4f pt_out;
 
     bool max_range_point = false;
-    int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
-    float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
+    // Only touch in.fields[dist_idx] when the "distance" field actually exists:
+    // indexing with -1 is out of bounds and aborts on a hardened libstdc++.
+    int distance_ptr_offset = 0;
+    float* distance_ptr = NULL;
+    if (dist_idx >= 0)
+    {
+      distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
+      distance_ptr = (float*)(&in.data[distance_ptr_offset]);
+    }
     if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
     {
       if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point
diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp
new file mode 100644
index 00000000..8cc12760
--- /dev/null
+++ b/rtabmap_conversions/test/test_msg_conversion.cpp
@@ -0,0 +1,3646 @@
+/*
+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.
+*/
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <functional>
+#include <limits>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/Compression.h>
+
+using namespace rtabmap_conversions;
+
+namespace {
+
+// A transform with translation and rotation on all three axes, so that a
+// round-trip that drops or swaps a component cannot pass by accident.
+rtabmap::Transform sampleTransform()
+{
+	return rtabmap::Transform(1.0f, -2.0f, 3.0f, 0.1f, -0.2f, 0.3f);
+}
+
+void expectTransformNear(
+		const rtabmap::Transform & actual,
+		const rtabmap::Transform & expected,
+		float epsilon = 1e-5f)
+{
+	ASSERT_FALSE(actual.isNull()) << "expected " << expected.prettyPrint();
+	for(int i=0; i<12; ++i)
+	{
+		EXPECT_NEAR(actual.data()[i], expected.data()[i], epsilon)
+			<< "at index " << i
+			<< "\n  actual:   " << actual.prettyPrint()
+			<< "\n  expected: " << expected.prettyPrint();
+	}
+}
+
+}  // namespace
+
+/////////////////////////
+// Transform <-> geometry_msgs
+/////////////////////////
+
+TEST(MsgConversion, transformGeometryMsgRoundTrip)
+{
+	const rtabmap::Transform in = sampleTransform();
+
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(in, msg);
+
+	expectTransformNear(transformFromGeometryMsg(msg), in);
+}
+
+TEST(MsgConversion, transformGeometryMsgQuaternionIsNormalized)
+{
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(sampleTransform(), msg);
+
+	const double norm = std::sqrt(
+			msg.rotation.x * msg.rotation.x +
+			msg.rotation.y * msg.rotation.y +
+			msg.rotation.z * msg.rotation.z +
+			msg.rotation.w * msg.rotation.w);
+	EXPECT_NEAR(norm, 1.0, 1e-9);
+}
+
+TEST(MsgConversion, transformGeometryMsgNullRoundTrip)
+{
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(rtabmap::Transform(), msg);
+
+	// A null transform is encoded as an all-zero quaternion.
+	EXPECT_EQ(msg.rotation.x, 0.0);
+	EXPECT_EQ(msg.rotation.y, 0.0);
+	EXPECT_EQ(msg.rotation.z, 0.0);
+	EXPECT_EQ(msg.rotation.w, 0.0);
+	EXPECT_TRUE(transformFromGeometryMsg(msg).isNull());
+}
+
+TEST(MsgConversion, transformGeometryMsgIdentityIsNotNull)
+{
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(rtabmap::Transform::getIdentity(), msg);
+
+	const rtabmap::Transform out = transformFromGeometryMsg(msg);
+	EXPECT_FALSE(out.isNull());
+	EXPECT_TRUE(out.isIdentity());
+}
+
+/////////////////////////
+// Transform <-> tf2
+/////////////////////////
+
+TEST(MsgConversion, transformTFRoundTrip)
+{
+	const rtabmap::Transform in = sampleTransform();
+
+	tf2::Transform tf;
+	EXPECT_TRUE(transformToTF(in, tf));
+
+	expectTransformNear(transformFromTF(tf), in);
+}
+
+TEST(MsgConversion, transformTFIdentityRoundTrip)
+{
+	tf2::Transform tf;
+	EXPECT_TRUE(transformToTF(rtabmap::Transform::getIdentity(), tf))
+		<< "an identity transform is not a null transform";
+
+	const rtabmap::Transform out = transformFromTF(tf);
+	EXPECT_FALSE(out.isNull());
+	EXPECT_TRUE(out.isIdentity());
+}
+
+TEST(MsgConversion, transformToTFWritesTranslationAndRotation)
+{
+	// Guards against the output being left untouched: seed it with a value that
+	// differs from the expected result, then check it was actually overwritten.
+	tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99));
+	EXPECT_TRUE(transformToTF(sampleTransform(), tf));
+
+	EXPECT_NEAR(tf.getOrigin().x(), 1.0, 1e-5);
+	EXPECT_NEAR(tf.getOrigin().y(), -2.0, 1e-5);
+	EXPECT_NEAR(tf.getOrigin().z(), 3.0, 1e-5);
+
+	geometry_msgs::msg::Transform expected;
+	transformToGeometryMsg(sampleTransform(), expected);
+	EXPECT_NEAR(tf.getRotation().x(), expected.rotation.x, 1e-5);
+	EXPECT_NEAR(tf.getRotation().y(), expected.rotation.y, 1e-5);
+	EXPECT_NEAR(tf.getRotation().z(), expected.rotation.z, 1e-5);
+	EXPECT_NEAR(tf.getRotation().w(), expected.rotation.w, 1e-5);
+}
+
+TEST(MsgConversion, transformToTFNullReturnsFalseAndNaN)
+{
+	tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99));
+
+	EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
+
+	// tf2::Transform cannot carry a null sentinel, so the output is poisoned with NaN
+	// on purpose: a caller that ignores the return value must fail loudly rather than
+	// silently proceed with a plausible-looking identity.
+	for(int i=0; i<3; ++i)
+	{
+		EXPECT_TRUE(std::isnan(tf.getBasis()[i].x())) << "basis row " << i;
+		EXPECT_TRUE(std::isnan(tf.getBasis()[i].y())) << "basis row " << i;
+		EXPECT_TRUE(std::isnan(tf.getBasis()[i].z())) << "basis row " << i;
+	}
+	EXPECT_TRUE(std::isnan(tf.getOrigin().x()));
+	EXPECT_TRUE(std::isnan(tf.getOrigin().y()));
+	EXPECT_TRUE(std::isnan(tf.getOrigin().z()));
+
+	const tf2::Quaternion q = tf.getRotation();
+	EXPECT_TRUE(std::isnan(q.x()));
+	EXPECT_TRUE(std::isnan(q.y()));
+	EXPECT_TRUE(std::isnan(q.z()));
+	EXPECT_TRUE(std::isnan(q.w()));
+}
+
+TEST(MsgConversion, transformToTFNullPoisonsComposition)
+{
+	// The point of the NaN: it propagates through downstream math instead of
+	// quietly producing a wrong-but-finite answer.
+	tf2::Transform tf;
+	EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
+
+	const tf2::Transform composed =
+			tf * tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(1, 2, 3));
+
+	EXPECT_TRUE(std::isnan(composed.getOrigin().x()));
+	EXPECT_TRUE(std::isnan(composed.getOrigin().y()));
+	EXPECT_TRUE(std::isnan(composed.getOrigin().z()));
+}
+
+TEST(MsgConversion, transformFromTFDetectsNaN)
+{
+	const tf2Scalar nan = std::numeric_limits<tf2Scalar>::quiet_NaN();
+
+	// NaN anywhere in the rotation basis...
+	EXPECT_TRUE(transformFromTF(tf2::Transform(
+			tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan),
+			tf2::Vector3(0, 0, 0))).isNull()) << "NaN basis";
+
+	// ...or in the translation alone must yield a null transform.
+	EXPECT_TRUE(transformFromTF(tf2::Transform(
+			tf2::Quaternion(0, 0, 0, 1),
+			tf2::Vector3(nan, 0, 0))).isNull()) << "NaN origin";
+}
+
+TEST(MsgConversion, transformTFNullRoundTrip)
+{
+	// The pair round-trips a null transform: toTF poisons with NaN and reports
+	// false, fromTF maps that back to null.
+	tf2::Transform tf;
+	EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf));
+	EXPECT_TRUE(transformFromTF(tf).isNull());
+}
+
+TEST(MsgConversion, transformFromTFAcceptsValidTransforms)
+{
+	// The NaN guard must not reject legitimate values, including zeros.
+	EXPECT_FALSE(transformFromTF(tf2::Transform(
+			tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0))).isNull());
+	EXPECT_FALSE(transformFromTF(tf2::Transform(
+			tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-1, 2, -3))).isNull());
+}
+
+/////////////////////////
+// Transform <-> Pose
+/////////////////////////
+
+TEST(MsgConversion, transformPoseMsgRoundTrip)
+{
+	const rtabmap::Transform in = sampleTransform();
+
+	geometry_msgs::msg::Pose msg;
+	transformToPoseMsg(in, msg);
+
+	expectTransformNear(transformFromPoseMsg(msg), in);
+}
+
+TEST(MsgConversion, transformPoseMsgNullIsNull)
+{
+	geometry_msgs::msg::Pose msg;
+	transformToPoseMsg(rtabmap::Transform(), msg);
+
+	EXPECT_TRUE(transformFromPoseMsg(msg).isNull());
+}
+
+TEST(MsgConversion, transformPoseMsgIgnoreRotationIfNotSet)
+{
+	// Note: geometry_msgs::msg::Quaternion defaults to w=1.0, so an "unset"
+	// orientation has to be zeroed explicitly to reach the branch under test.
+	geometry_msgs::msg::Pose msg;
+	msg.position.x = 1.0;
+	msg.position.y = 2.0;
+	msg.position.z = 3.0;
+	msg.orientation.w = 0.0;
+
+	// An all-zero orientation normally yields a null transform...
+
+	EXPECT_TRUE(transformFromPoseMsg(msg, false).isNull());
+
+	// ...but with ignoreRotationIfNotSet the translation is kept with no rotation.
+	expectTransformNear(
+			transformFromPoseMsg(msg, true),
+			rtabmap::Transform(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f));
+}
+
+/////////////////////////
+// Points and keypoints
+/////////////////////////
+
+TEST(MsgConversion, point2fRoundTrip)
+{
+	const cv::Point2f in(1.5f, -2.5f);
+
+	rtabmap_msgs::msg::Point2f msg;
+	point2fToROS(in, msg);
+	const cv::Point2f out = point2fFromROS(msg);
+
+	EXPECT_FLOAT_EQ(out.x, in.x);
+	EXPECT_FLOAT_EQ(out.y, in.y);
+}
+
+TEST(MsgConversion, points2fVectorRoundTrip)
+{
+	const std::vector<cv::Point2f> in = {{1.0f, 2.0f}, {-3.0f, 4.5f}};
+
+	std::vector<rtabmap_msgs::msg::Point2f> msg;
+	points2fToROS(in, msg);
+	const std::vector<cv::Point2f> out = points2fFromROS(msg);
+
+	ASSERT_EQ(out.size(), in.size());
+	for(size_t i=0; i<in.size(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out[i].x, in[i].x) << "at " << i;
+		EXPECT_FLOAT_EQ(out[i].y, in[i].y) << "at " << i;
+	}
+}
+
+TEST(MsgConversion, point3fRoundTrip)
+{
+	const cv::Point3f in(1.5f, -2.5f, 3.5f);
+
+	rtabmap_msgs::msg::Point3f msg;
+	point3fToROS(in, msg);
+	const cv::Point3f out = point3fFromROS(msg);
+
+	EXPECT_FLOAT_EQ(out.x, in.x);
+	EXPECT_FLOAT_EQ(out.y, in.y);
+	EXPECT_FLOAT_EQ(out.z, in.z);
+}
+
+TEST(MsgConversion, points3fVectorRoundTripWithoutTransform)
+{
+	const std::vector<cv::Point3f> in = {{1.0f, 2.0f, 3.0f}, {-4.0f, 5.0f, -6.0f}};
+
+	std::vector<rtabmap_msgs::msg::Point3f> msg;
+	points3fToROS(in, msg);
+	const std::vector<cv::Point3f> out = points3fFromROS(msg);
+
+	ASSERT_EQ(out.size(), in.size());
+	for(size_t i=0; i<in.size(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out[i].x, in[i].x) << "at " << i;
+		EXPECT_FLOAT_EQ(out[i].y, in[i].y) << "at " << i;
+		EXPECT_FLOAT_EQ(out[i].z, in[i].z) << "at " << i;
+	}
+}
+
+TEST(MsgConversion, points3fTransformIsAppliedOnBothDirections)
+{
+	const std::vector<cv::Point3f> in = {{1.0f, 2.0f, 3.0f}};
+	const rtabmap::Transform t = sampleTransform();
+
+	// Applying t on the way out and t.inverse() on the way in must cancel.
+	std::vector<rtabmap_msgs::msg::Point3f> msg;
+	points3fToROS(in, msg, t);
+	const std::vector<cv::Point3f> out = points3fFromROS(msg, t.inverse());
+
+	ASSERT_EQ(out.size(), in.size());
+	EXPECT_NEAR(out[0].x, in[0].x, 1e-4);
+	EXPECT_NEAR(out[0].y, in[0].y, 1e-4);
+	EXPECT_NEAR(out[0].z, in[0].z, 1e-4);
+
+	// ...and the intermediate message really is the transformed point.
+	const cv::Point3f expected = rtabmap::util3d::transformPoint(in[0], t);
+	EXPECT_NEAR(msg[0].x, expected.x, 1e-4);
+	EXPECT_NEAR(msg[0].y, expected.y, 1e-4);
+	EXPECT_NEAR(msg[0].z, expected.z, 1e-4);
+}
+
+TEST(MsgConversion, points3fFromROSAppendsToExistingVector)
+{
+	std::vector<rtabmap_msgs::msg::Point3f> msg(2);
+	msg[0].x = 1.0f;
+	msg[1].x = 2.0f;
+
+	std::vector<cv::Point3f> points = {{9.0f, 9.0f, 9.0f}};
+	points3fFromROS(msg, points);
+
+	ASSERT_EQ(points.size(), 3u);
+	EXPECT_FLOAT_EQ(points[0].x, 9.0f) << "existing content must be preserved";
+	EXPECT_FLOAT_EQ(points[1].x, 1.0f);
+	EXPECT_FLOAT_EQ(points[2].x, 2.0f);
+}
+
+TEST(MsgConversion, keypointRoundTrip)
+{
+	const cv::KeyPoint in(cv::Point2f(10.0f, 20.0f), 7.0f, 45.0f, 0.5f, 2, 3);
+
+	rtabmap_msgs::msg::KeyPoint msg;
+	keypointToROS(in, msg);
+	const cv::KeyPoint out = keypointFromROS(msg);
+
+	EXPECT_FLOAT_EQ(out.pt.x, in.pt.x);
+	EXPECT_FLOAT_EQ(out.pt.y, in.pt.y);
+	EXPECT_FLOAT_EQ(out.size, in.size);
+	EXPECT_FLOAT_EQ(out.angle, in.angle);
+	EXPECT_FLOAT_EQ(out.response, in.response);
+	EXPECT_EQ(out.octave, in.octave);
+	EXPECT_EQ(out.class_id, in.class_id);
+}
+
+TEST(MsgConversion, keypointsFromROSAppendsAndAppliesXShift)
+{
+	const std::vector<cv::KeyPoint> in = {
+		cv::KeyPoint(cv::Point2f(10.0f, 20.0f), 7.0f),
+		cv::KeyPoint(cv::Point2f(30.0f, 40.0f), 7.0f)};
+
+	std::vector<rtabmap_msgs::msg::KeyPoint> msg;
+	keypointsToROS(in, msg);
+	ASSERT_EQ(msg.size(), in.size());
+
+	std::vector<cv::KeyPoint> kpts = {cv::KeyPoint(cv::Point2f(1.0f, 1.0f), 1.0f)};
+	keypointsFromROS(msg, kpts, /*xShift=*/100);
+
+	ASSERT_EQ(kpts.size(), 3u);
+	EXPECT_FLOAT_EQ(kpts[0].pt.x, 1.0f) << "existing content must be preserved";
+	EXPECT_FLOAT_EQ(kpts[1].pt.x, 110.0f);
+	EXPECT_FLOAT_EQ(kpts[2].pt.x, 130.0f);
+	EXPECT_FLOAT_EQ(kpts[1].pt.y, 20.0f) << "xShift must not touch y";
+}
+
+/////////////////////////
+// Timestamps
+/////////////////////////
+
+TEST(MsgConversion, timestampRoundTrip)
+{
+	// Not exact on purpose: a double resolves to a few hundred nanoseconds at this
+	// magnitude, so the round trip is only good to about a microsecond.
+	const double in = 1234567890.123456;
+	EXPECT_NEAR(timestampFromROS(timestampToROS(in)), in, 1e-6);
+}
+
+TEST(MsgConversion, timestampToROSUsesRosClock)
+{
+	// Message header stamps convert to RCL_ROS_TIME, while rclcpp::Time(sec, nsec)
+	// defaults to RCL_SYSTEM_TIME. Comparing two different clock types throws, so a
+	// timestamp built here must be comparable with one taken from a message -- several
+	// conversions do exactly that when syncing to an odometry stamp.
+	const rclcpp::Time built = timestampToROS(1000.0);
+	EXPECT_EQ(built.get_clock_type(), RCL_ROS_TIME);
+
+	builtin_interfaces::msg::Time asMsg = timestampToROS(1000.5);
+	const rclcpp::Time fromMsg(asMsg);
+	EXPECT_EQ(fromMsg.get_clock_type(), RCL_ROS_TIME);
+
+	EXPECT_NO_THROW({ volatile bool differ = (built != fromMsg); (void)differ; })
+		<< "a built stamp must be comparable with a message-derived one";
+}
+
+TEST(MsgConversion, timestampZeroRoundTrip)
+{
+	EXPECT_EQ(timestampFromROS(timestampToROS(0.0)), 0.0);
+}
+
+/////////////////////////
+// sizeOfPointField
+/////////////////////////
+
+TEST(MsgConversion, sizeOfPointField)
+{
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT8), 1);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT8), 1);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT16), 2);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT16), 2);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT32), 4);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT32), 4);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32), 4);
+	EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT64), 8);
+}
+
+TEST(MsgConversion, sizeOfPointFieldThrowsOnUnknownType)
+{
+	EXPECT_THROW(sizeOfPointField(42), std::runtime_error);
+}
+
+/////////////////////////
+// getClosestIterator
+/////////////////////////
+
+TEST(MsgConversion, getClosestIterator)
+{
+	const std::map<double, int> buffer = {{1.0, 10}, {2.0, 20}, {3.0, 30}};
+
+	EXPECT_EQ(getClosestIterator(buffer, 1.0)->second, 10) << "exact match";
+	EXPECT_EQ(getClosestIterator(buffer, 2.0)->second, 20) << "exact match";
+	EXPECT_EQ(getClosestIterator(buffer, 1.4)->second, 10) << "closer to lower";
+	EXPECT_EQ(getClosestIterator(buffer, 1.6)->second, 20) << "closer to upper";
+	EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 10) << "clamped below range";
+	EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 30) << "clamped above range";
+}
+
+TEST(MsgConversion, getClosestIteratorSingleEntry)
+{
+	const std::map<double, int> buffer = {{5.0, 50}};
+
+	EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 50);
+	EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 50);
+}
+
+/////////////////////////
+// compressedMat <-> bytes
+/////////////////////////
+
+TEST(MsgConversion, compressedMatRoundTrip)
+{
+	const cv::Mat in = (cv::Mat_<unsigned char>(1, 5) << 1, 2, 3, 250, 255);
+
+	std::vector<unsigned char> bytes;
+	compressedMatToBytes(in, bytes);
+	ASSERT_EQ(bytes.size(), 5u);
+
+	const cv::Mat out = compressedMatFromBytes(bytes);
+	ASSERT_EQ(out.type(), CV_8UC1);
+	ASSERT_EQ(out.total(), in.total());
+	EXPECT_EQ(cv::countNonZero(out.reshape(1, 1) != in.reshape(1, 1)), 0);
+}
+
+TEST(MsgConversion, compressedMatEmptyRoundTrip)
+{
+	std::vector<unsigned char> bytes = {1, 2, 3};
+	compressedMatToBytes(cv::Mat(), bytes);
+
+	EXPECT_TRUE(bytes.empty()) << "output must be cleared";
+	EXPECT_TRUE(compressedMatFromBytes(bytes).empty());
+}
+
+TEST(MsgConversion, compressedMatFromBytesCopyFlag)
+{
+	std::vector<unsigned char> bytes = {1, 2, 3};
+
+	const cv::Mat shared = compressedMatFromBytes(bytes, /*copy=*/false);
+	const cv::Mat copied = compressedMatFromBytes(bytes, /*copy=*/true);
+
+	bytes[0] = 99;
+	EXPECT_EQ(shared.at<unsigned char>(0, 0), 99) << "copy=false must alias the input";
+	EXPECT_EQ(copied.at<unsigned char>(0, 0), 1) << "copy=true must be independent";
+}
+
+/////////////////////////
+// EnvSensor
+/////////////////////////
+
+TEST(MsgConversion, envSensorRoundTrip)
+{
+	const rtabmap::EnvSensor in(
+			rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1234567890.5);
+
+	rtabmap_msgs::msg::EnvSensor msg;
+	envSensorToROS(in, msg);
+	const rtabmap::EnvSensor out = envSensorFromROS(msg);
+
+	EXPECT_EQ(out.type(), in.type());
+	EXPECT_DOUBLE_EQ(out.value(), in.value());
+	EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
+}
+
+TEST(MsgConversion, envSensorsRoundTripKeyedByType)
+{
+	rtabmap::EnvSensors in;
+	in.insert(std::make_pair(
+			rtabmap::EnvSensor::kAmbientTemperature,
+			rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1.0)));
+	in.insert(std::make_pair(
+			rtabmap::EnvSensor::kAmbientLight,
+			rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientLight, 300.0, 2.0)));
+
+	std::vector<rtabmap_msgs::msg::EnvSensor> msg;
+	envSensorsToROS(in, msg);
+	ASSERT_EQ(msg.size(), in.size());
+
+	const rtabmap::EnvSensors out = envSensorsFromROS(msg);
+	ASSERT_EQ(out.size(), in.size());
+	for(rtabmap::EnvSensors::const_iterator iter=in.begin(); iter!=in.end(); ++iter)
+	{
+		rtabmap::EnvSensors::const_iterator found = out.find(iter->first);
+		ASSERT_NE(found, out.end()) << "missing type " << iter->first;
+		EXPECT_DOUBLE_EQ(found->second.value(), iter->second.value());
+	}
+}
+
+/////////////////////////
+// Link
+/////////////////////////
+
+TEST(MsgConversion, linkRoundTrip)
+{
+	cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1) * 3.0;
+	const rtabmap::Link in(
+			1, 2, rtabmap::Link::kGlobalClosure, sampleTransform(), information);
+
+	rtabmap_msgs::msg::Link msg;
+	linkToROS(in, msg);
+	const rtabmap::Link out = linkFromROS(msg);
+
+	EXPECT_EQ(out.from(), in.from());
+	EXPECT_EQ(out.to(), in.to());
+	EXPECT_EQ(out.type(), in.type());
+	expectTransformNear(out.transform(), in.transform());
+
+	ASSERT_EQ(out.infMatrix().rows, 6);
+	ASSERT_EQ(out.infMatrix().cols, 6);
+	for(int i=0; i<6; ++i)
+	{
+		for(int j=0; j<6; ++j)
+		{
+			EXPECT_DOUBLE_EQ(
+					out.infMatrix().at<double>(i, j),
+					in.infMatrix().at<double>(i, j)) << "at " << i << "," << j;
+		}
+	}
+}
+
+/////////////////////////
+// CameraModel
+/////////////////////////
+
+TEST(MsgConversion, cameraModelFromROSReadsIntrinsics)
+{
+	sensor_msgs::msg::CameraInfo in;
+	in.width = 640;
+	in.height = 480;
+	in.distortion_model = "plumb_bob";
+	in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
+	in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
+	in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+	in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
+
+	const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
+	const rtabmap::CameraModel model = cameraModelFromROS(in, localTransform);
+
+	EXPECT_EQ(model.imageWidth(), 640);
+	EXPECT_EQ(model.imageHeight(), 480);
+	EXPECT_NEAR(model.fx(), 525.0, 1e-9);
+	EXPECT_NEAR(model.fy(), 525.0, 1e-9);
+	EXPECT_NEAR(model.cx(), 320.0, 1e-9);
+	EXPECT_NEAR(model.cy(), 240.0, 1e-9);
+	expectTransformNear(model.localTransform(), localTransform);
+
+	// The raw distortion coefficients are kept verbatim.
+	ASSERT_EQ(model.D_raw().cols, 5);
+	for(size_t i=0; i<in.d.size(); ++i)
+	{
+		EXPECT_NEAR(model.D_raw().at<double>(0, i), in.d[i], 1e-9) << "d at " << i;
+	}
+}
+
+TEST(MsgConversion, cameraModelToROSRectifiedHasNoDistortion)
+{
+	sensor_msgs::msg::CameraInfo in;
+	in.width = 640;
+	in.height = 480;
+	in.distortion_model = "plumb_bob";
+	in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
+	in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
+	in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+	in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
+
+	sensor_msgs::msg::CameraInfo out;
+	cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out);
+
+	EXPECT_EQ(out.width, in.width);
+	EXPECT_EQ(out.height, in.height);
+
+	// A model carrying a projection matrix describes an already-rectified image,
+	// so cameraModelToROS deliberately emits zero distortion rather than echoing
+	// back the raw coefficients. K and P do round-trip unchanged.
+	EXPECT_EQ(out.distortion_model, "plumb_bob");
+	ASSERT_EQ(out.d.size(), 5u);
+	for(size_t i=0; i<out.d.size(); ++i)
+	{
+		EXPECT_DOUBLE_EQ(out.d[i], 0.0) << "d at " << i;
+	}
+	for(size_t i=0; i<in.k.size(); ++i)
+	{
+		EXPECT_NEAR(out.k[i], in.k[i], 1e-9) << "k at " << i;
+	}
+	for(size_t i=0; i<in.p.size(); ++i)
+	{
+		EXPECT_NEAR(out.p[i], in.p[i], 1e-9) << "p at " << i;
+	}
+}
+
+TEST(MsgConversion, cameraModelFromROSPacksFisheyeDistortion)
+{
+	// Fisheye/equidistant models carry 4 coefficients, which rtabmap stores in a
+	// 1x6 matrix at positions 0, 1, 4 and 5.
+	for(const std::string & model : {"fisheye", "equidistant", "Kannala Brandt4"})
+	{
+		sensor_msgs::msg::CameraInfo in;
+		in.width = 640;
+		in.height = 480;
+		in.distortion_model = model;
+		in.d = {0.1, 0.2, 0.3, 0.4};
+		in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
+
+		const cv::Mat D = cameraModelFromROS(in, rtabmap::Transform::getIdentity()).D_raw();
+
+		ASSERT_EQ(D.total(), 6u) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 0), 0.1, 1e-9) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 1), 0.2, 1e-9) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 2), 0.0, 1e-9) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 3), 0.0, 1e-9) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 4), 0.3, 1e-9) << "distortion_model=" << model;
+		EXPECT_NEAR(D.at<double>(0, 5), 0.4, 1e-9) << "distortion_model=" << model;
+	}
+}
+
+TEST(MsgConversion, cameraModelToROSUnpacksFisheyeDistortion)
+{
+	// Built with an empty P: cameraModelToROS only reports "equidistant" for a
+	// raw (unrectified) model. Note this cannot be produced by cameraModelFromROS,
+	// whose P is a fixed-size array and therefore never empty.
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
+	cv::Mat D = cv::Mat::zeros(1, 6, CV_64FC1);
+	D.at<double>(0, 0) = 0.1;
+	D.at<double>(0, 1) = 0.2;
+	D.at<double>(0, 4) = 0.3;
+	D.at<double>(0, 5) = 0.4;
+
+	const rtabmap::CameraModel model(
+			"fisheye", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(),
+			rtabmap::Transform::getIdentity());
+
+	sensor_msgs::msg::CameraInfo out;
+	cameraModelToROS(model, out);
+
+	EXPECT_EQ(out.distortion_model, "equidistant");
+	ASSERT_EQ(out.d.size(), 4u);
+	EXPECT_NEAR(out.d[0], 0.1, 1e-9);
+	EXPECT_NEAR(out.d[1], 0.2, 1e-9);
+	EXPECT_NEAR(out.d[2], 0.3, 1e-9);
+	EXPECT_NEAR(out.d[3], 0.4, 1e-9);
+}
+
+TEST(MsgConversion, cameraModelToROSRationalPolynomialDistortion)
+{
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
+	cv::Mat D = (cv::Mat_<double>(1, 8) <<
+			0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8);
+
+	const rtabmap::CameraModel model(
+			"rational", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(),
+			rtabmap::Transform::getIdentity());
+
+	sensor_msgs::msg::CameraInfo out;
+	cameraModelToROS(model, out);
+
+	EXPECT_EQ(out.distortion_model, "rational_polynomial");
+	ASSERT_EQ(out.d.size(), 8u);
+	for(size_t i=0; i<out.d.size(); ++i)
+	{
+		EXPECT_NEAR(out.d[i], 0.1 * (i + 1), 1e-9) << "d at " << i;
+	}
+}
+
+TEST(MsgConversion, cameraModelFromROSTreatsUnsetMatricesAsAbsent)
+{
+	// k, r and p are fixed-size arrays, so "unset" means all-zero rather than empty.
+	sensor_msgs::msg::CameraInfo in;
+	in.width = 640;
+	in.height = 480;
+	in.distortion_model = "plumb_bob";
+	in.d = {0.1, 0.2, 0.3, 0.4, 0.5};
+	in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
+	// r and p deliberately left at their all-zero defaults.
+
+	const rtabmap::CameraModel model =
+			cameraModelFromROS(in, rtabmap::Transform::getIdentity());
+
+	EXPECT_TRUE(model.P().empty()) << "an all-zero P must not become a 3x4 of zeros";
+	EXPECT_TRUE(model.R().empty()) << "an all-zero R must not become a 3x3 of zeros";
+	EXPECT_FALSE(model.K_raw().empty());
+
+	// With no P, the intrinsics must come from K instead of an all-zero P.
+	EXPECT_NEAR(model.fx(), 525.0, 1e-9);
+	EXPECT_NEAR(model.fy(), 525.0, 1e-9);
+	EXPECT_NEAR(model.cx(), 320.0, 1e-9);
+	EXPECT_NEAR(model.cy(), 240.0, 1e-9);
+}
+
+TEST(MsgConversion, cameraModelUnrectifiedFisheyeRoundTrip)
+{
+	// Without a projection matrix the model stays raw, so the fisheye coefficients
+	// survive the full message -> model -> message round trip.
+	sensor_msgs::msg::CameraInfo in;
+	in.width = 640;
+	in.height = 480;
+	in.distortion_model = "equidistant";
+	in.d = {0.1, 0.2, 0.3, 0.4};
+	in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0};
+
+	sensor_msgs::msg::CameraInfo out;
+	cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out);
+
+	EXPECT_EQ(out.distortion_model, "equidistant");
+	ASSERT_EQ(out.d.size(), 4u);
+	for(size_t i=0; i<in.d.size(); ++i)
+	{
+		EXPECT_NEAR(out.d[i], in.d[i], 1e-9) << "d at " << i;
+	}
+	for(size_t i=0; i<in.k.size(); ++i)
+	{
+		EXPECT_NEAR(out.k[i], in.k[i], 1e-9) << "k at " << i;
+	}
+
+	// An unset R is reported as identity.
+	const std::array<double, 9> identity = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
+	for(size_t i=0; i<identity.size(); ++i)
+	{
+		EXPECT_NEAR(out.r[i], identity[i], 1e-9) << "r at " << i;
+	}
+}
+
+TEST(MsgConversion, cameraModelToROSSynthesizesPFromK)
+{
+	// With no P of its own, cameraModelToROS builds P = [K | 0].
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
+
+	const rtabmap::CameraModel model(
+			"raw", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(),
+			rtabmap::Transform::getIdentity());
+
+	sensor_msgs::msg::CameraInfo out;
+	cameraModelToROS(model, out);
+
+	const std::array<double, 12> expected = {
+			525.0, 0.0, 320.0, 0.0,
+			0.0, 525.0, 240.0, 0.0,
+			0.0, 0.0, 1.0, 0.0};
+	for(size_t i=0; i<expected.size(); ++i)
+	{
+		EXPECT_NEAR(out.p[i], expected[i], 1e-9) << "p at " << i;
+	}
+	// P(2,3) is a translation term and must stay zero for a single camera.
+	EXPECT_DOUBLE_EQ(out.p[11], 0.0);
+}
+
+/////////////////////////
+// GlobalDescriptor
+/////////////////////////
+
+TEST(MsgConversion, globalDescriptorRoundTrip)
+{
+	cv::Mat data = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
+	cv::Mat info = (cv::Mat_<float>(1, 2) << 9.0f, 8.0f);
+	const rtabmap::GlobalDescriptor in(7, data, info);
+
+	rtabmap_msgs::msg::GlobalDescriptor msg;
+	globalDescriptorToROS(in, msg);
+	const rtabmap::GlobalDescriptor out = globalDescriptorFromROS(msg);
+
+	EXPECT_EQ(out.type(), in.type());
+	ASSERT_EQ(out.data().total(), in.data().total());
+	for(size_t i=0; i<in.data().total(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out.data().at<float>(0, i), in.data().at<float>(0, i)) << "data at " << i;
+	}
+	ASSERT_EQ(out.info().total(), in.info().total());
+	for(size_t i=0; i<in.info().total(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out.info().at<float>(0, i), in.info().at<float>(0, i)) << "info at " << i;
+	}
+}
+
+TEST(MsgConversion, globalDescriptorsVectorRoundTrip)
+{
+	std::vector<rtabmap::GlobalDescriptor> in;
+	in.push_back(rtabmap::GlobalDescriptor(1, (cv::Mat_<float>(1, 2) << 1.0f, 2.0f)));
+	in.push_back(rtabmap::GlobalDescriptor(2, (cv::Mat_<float>(1, 2) << 3.0f, 4.0f)));
+
+	std::vector<rtabmap_msgs::msg::GlobalDescriptor> msg;
+	globalDescriptorsToROS(in, msg);
+	ASSERT_EQ(msg.size(), in.size());
+
+	const std::vector<rtabmap::GlobalDescriptor> out = globalDescriptorsFromROS(msg);
+	ASSERT_EQ(out.size(), in.size());
+	for(size_t i=0; i<in.size(); ++i)
+	{
+		EXPECT_EQ(out[i].type(), in[i].type()) << "at " << i;
+		EXPECT_FLOAT_EQ(out[i].data().at<float>(0, 0), in[i].data().at<float>(0, 0)) << "at " << i;
+	}
+}
+
+TEST(MsgConversion, globalDescriptorsEmptyRoundTrip)
+{
+	std::vector<rtabmap_msgs::msg::GlobalDescriptor> msg(3);
+	globalDescriptorsToROS(std::vector<rtabmap::GlobalDescriptor>(), msg);
+
+	EXPECT_TRUE(msg.empty()) << "output must be cleared";
+	EXPECT_TRUE(globalDescriptorsFromROS(msg).empty());
+}
+
+/////////////////////////
+// UserData
+/////////////////////////
+
+TEST(MsgConversion, userDataUncompressedRoundTrip)
+{
+	const cv::Mat in = (cv::Mat_<int>(2, 3) << 1, 2, 3, 4, 5, 6);
+
+	rtabmap_msgs::msg::UserData msg;
+	userDataToROS(in, msg, /*compress=*/false);
+
+	EXPECT_EQ(msg.rows, in.rows);
+	EXPECT_EQ(msg.cols, in.cols);
+	EXPECT_EQ(msg.type, in.type());
+
+	const cv::Mat out = userDataFromROS(msg);
+	ASSERT_EQ(out.rows, in.rows);
+	ASSERT_EQ(out.cols, in.cols);
+	ASSERT_EQ(out.type(), in.type());
+	EXPECT_EQ(cv::countNonZero(out != in), 0);
+}
+
+TEST(MsgConversion, userDataCompressedRoundTrip)
+{
+	const cv::Mat in = (cv::Mat_<int>(2, 3) << 1, 2, 3, 4, 5, 6);
+
+	rtabmap_msgs::msg::UserData msg;
+	userDataToROS(in, msg, /*compress=*/true);
+
+	// Compressed payloads travel as a 1xN byte blob.
+	EXPECT_EQ(msg.rows, 1);
+	EXPECT_EQ(msg.type, CV_8UC1);
+	EXPECT_EQ((size_t)msg.cols, msg.data.size());
+
+	// userDataFromROS hands back the still-compressed blob; the caller uncompresses.
+	const cv::Mat blob = userDataFromROS(msg);
+	ASSERT_FALSE(blob.empty());
+	const cv::Mat out = rtabmap::uncompressData(blob);
+
+	ASSERT_EQ(out.rows, in.rows);
+	ASSERT_EQ(out.cols, in.cols);
+	ASSERT_EQ(out.type(), in.type());
+	EXPECT_EQ(cv::countNonZero(out != in), 0);
+}
+
+TEST(MsgConversion, userDataEmpty)
+{
+	rtabmap_msgs::msg::UserData msg;
+	userDataToROS(cv::Mat(), msg, /*compress=*/false);
+	EXPECT_TRUE(msg.data.empty());
+	EXPECT_TRUE(userDataFromROS(msg).empty());
+}
+
+/////////////////////////
+// StereoCameraModel
+/////////////////////////
+
+TEST(MsgConversion, stereoCameraModelFromROS)
+{
+	const double fx = 525.0;
+	const double baseline = 0.12;
+
+	sensor_msgs::msg::CameraInfo left;
+	left.width = 640;
+	left.height = 480;
+	left.k = {fx, 0.0, 320.0, 0.0, fx, 240.0, 0.0, 0.0, 1.0};
+	left.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+	left.p = {fx, 0.0, 320.0, 0.0, 0.0, fx, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0};
+
+	// The right camera carries the baseline in P(0,3) = -fx * baseline.
+	sensor_msgs::msg::CameraInfo right = left;
+	right.p[3] = -fx * baseline;
+
+	const rtabmap::StereoCameraModel model = stereoCameraModelFromROS(
+			left, right, rtabmap::Transform::getIdentity());
+
+	EXPECT_NEAR(model.left().fx(), fx, 1e-9);
+	EXPECT_NEAR(model.right().fx(), fx, 1e-9);
+	EXPECT_NEAR(model.baseline(), baseline, 1e-9);
+	EXPECT_TRUE(model.isValidForProjection());
+}
+
+/////////////////////////
+// OdometryInfo
+/////////////////////////
+
+TEST(MsgConversion, odomInfoRoundTrip)
+{
+	rtabmap::OdometryInfo in;
+	in.lost = false;
+	in.features = 500;
+	in.localMapSize = 1000;
+	in.localScanMapSize = 2000;
+	in.localKeyFrames = 5;
+	in.keyFrameAdded = true;
+	in.timeEstimation = 0.02f;
+	in.interval = 0.033;
+	in.distanceTravelled = 12.5f;
+	in.reg.matches = 300;
+	in.reg.inliers = 250;
+	in.transform = sampleTransform();
+
+	rtabmap_msgs::msg::OdomInfo msg;
+	odomInfoToROS(in, msg);
+	const rtabmap::OdometryInfo out = odomInfoFromROS(msg);
+
+	EXPECT_EQ(out.lost, in.lost);
+	EXPECT_EQ(out.features, in.features);
+	EXPECT_EQ(out.localMapSize, in.localMapSize);
+	EXPECT_EQ(out.localScanMapSize, in.localScanMapSize);
+	EXPECT_EQ(out.localKeyFrames, in.localKeyFrames);
+	EXPECT_EQ(out.keyFrameAdded, in.keyFrameAdded);
+	EXPECT_FLOAT_EQ(out.timeEstimation, in.timeEstimation);
+	EXPECT_NEAR(out.interval, in.interval, 1e-6);
+	EXPECT_FLOAT_EQ(out.distanceTravelled, in.distanceTravelled);
+	EXPECT_EQ(out.reg.matches, in.reg.matches);
+	EXPECT_EQ(out.reg.inliers, in.reg.inliers);
+	expectTransformNear(out.transform, in.transform);
+}
+
+TEST(MsgConversion, odomInfoIgnoreDataDropsHeavyMembers)
+{
+	rtabmap::OdometryInfo in;
+	in.features = 500;
+	in.reg.inliers = 250;
+	in.words.insert(std::make_pair(1, cv::KeyPoint(cv::Point2f(1, 2), 3)));
+	in.localMap.insert(std::make_pair(1, cv::Point3f(1, 2, 3)));
+
+	rtabmap_msgs::msg::OdomInfo full;
+	odomInfoToROS(in, full, /*ignoreData=*/false);
+	EXPECT_FALSE(full.words_keys.empty());
+
+	rtabmap_msgs::msg::OdomInfo light;
+	odomInfoToROS(in, light, /*ignoreData=*/true);
+	EXPECT_TRUE(light.words_keys.empty()) << "heavy members must be dropped";
+
+	// The scalar statistics survive either way.
+	EXPECT_EQ(odomInfoFromROS(light).features, in.features);
+	EXPECT_EQ(odomInfoFromROS(light).reg.inliers, in.reg.inliers);
+}
+
+TEST(MsgConversion, odomInfoToStatistics)
+{
+	rtabmap::OdometryInfo info;
+	info.features = 400;
+	info.reg.inliers = 100;
+	info.reg.matches = 200;
+	info.localMapSize = 1234;
+
+	const std::map<std::string, float> stats = odomInfoToStatistics(info);
+
+	ASSERT_TRUE(stats.find("Odometry/Features/") != stats.end());
+	EXPECT_FLOAT_EQ(stats.at("Odometry/Features/"), 400.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/Matches/"), 200.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/Inliers/"), 100.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/LocalMapSize/"), 1234.0f);
+	// MatchesRatio is inliers/features, and must not divide by zero.
+	EXPECT_FLOAT_EQ(stats.at("Odometry/MatchesRatio/"), 100.0f/400.0f);
+}
+
+TEST(MsgConversion, odomInfoToStatisticsEmptyCovariance)
+{
+	// RegistrationInfo does not initialize covariance, so a plain OdometryInfo has
+	// an empty matrix. Reading it must not be attempted.
+	rtabmap::OdometryInfo info;
+	ASSERT_TRUE(info.reg.covariance.empty()) << "precondition";
+
+	const std::map<std::string, float> stats = odomInfoToStatistics(info);
+
+	EXPECT_TRUE(stats.find("Odometry/StdDevLin/") == stats.end())
+		<< "covariance-derived stats must be omitted, not read out of bounds";
+	EXPECT_TRUE(stats.find("Odometry/VarianceAng/") == stats.end());
+	// The rest of the statistics are still produced.
+	EXPECT_TRUE(stats.find("Odometry/Features/") != stats.end());
+}
+
+TEST(MsgConversion, odomInfoToStatisticsWithCovariance)
+{
+	rtabmap::OdometryInfo info;
+	info.reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 4.0;
+
+	const std::map<std::string, float> stats = odomInfoToStatistics(info);
+
+	ASSERT_TRUE(stats.find("Odometry/VarianceLin/") != stats.end());
+	EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceLin/"), 4.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevLin/"), 2.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceAng/"), 4.0f);
+	EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevAng/"), 2.0f);
+}
+
+TEST(MsgConversion, odomInfoToStatisticsNoFeatures)
+{
+	rtabmap::OdometryInfo info;
+	info.features = 0;
+	info.reg.inliers = 10;
+
+	EXPECT_FLOAT_EQ(odomInfoToStatistics(info).at("Odometry/MatchesRatio/"), 0.0f)
+		<< "must not divide by zero";
+}
+
+/////////////////////////
+// MapGraph / MapData
+/////////////////////////
+
+TEST(MsgConversion, mapGraphRoundTrip)
+{
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, rtabmap::Transform(1, 0, 0, 0, 0, 0)));
+	poses.insert(std::make_pair(2, sampleTransform()));
+
+	std::multimap<int, rtabmap::Link> links;
+	links.insert(std::make_pair(1, rtabmap::Link(
+			1, 2, rtabmap::Link::kNeighbor, sampleTransform(),
+			cv::Mat::eye(6, 6, CV_64FC1) * 2.0)));
+	links.insert(std::make_pair(2, rtabmap::Link(
+			2, 1, rtabmap::Link::kGlobalClosure, rtabmap::Transform::getIdentity(),
+			cv::Mat::eye(6, 6, CV_64FC1))));
+
+	const rtabmap::Transform mapToOdom(0.5f, -0.5f, 0.0f, 0.0f, 0.0f, 0.1f);
+
+	rtabmap_msgs::msg::MapGraph msg;
+	mapGraphToROS(poses, links, mapToOdom, msg);
+	ASSERT_EQ(msg.poses.size(), poses.size());
+	ASSERT_EQ(msg.poses_id.size(), poses.size());
+	ASSERT_EQ(msg.links.size(), links.size());
+
+	std::map<int, rtabmap::Transform> outPoses;
+	std::multimap<int, rtabmap::Link> outLinks;
+	rtabmap::Transform outMapToOdom;
+	mapGraphFromROS(msg, outPoses, outLinks, outMapToOdom);
+
+	ASSERT_EQ(outPoses.size(), poses.size());
+	for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		ASSERT_TRUE(outPoses.find(iter->first) != outPoses.end()) << "missing pose " << iter->first;
+		expectTransformNear(outPoses.at(iter->first), iter->second);
+	}
+
+	ASSERT_EQ(outLinks.size(), links.size());
+	for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		std::multimap<int, rtabmap::Link>::const_iterator found = outLinks.find(iter->first);
+		ASSERT_TRUE(found != outLinks.end()) << "missing link from " << iter->first;
+		EXPECT_EQ(found->second.from(), iter->second.from());
+		EXPECT_EQ(found->second.to(), iter->second.to());
+		EXPECT_EQ(found->second.type(), iter->second.type());
+	}
+
+	expectTransformNear(outMapToOdom, mapToOdom);
+}
+
+TEST(MsgConversion, mapGraphEmptyRoundTrip)
+{
+	rtabmap_msgs::msg::MapGraph msg;
+	mapGraphToROS(std::map<int, rtabmap::Transform>(), std::multimap<int, rtabmap::Link>(),
+			rtabmap::Transform(), msg);
+
+	EXPECT_TRUE(msg.poses.empty());
+	EXPECT_TRUE(msg.links.empty());
+
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> links;
+	rtabmap::Transform mapToOdom;
+	mapGraphFromROS(msg, poses, links, mapToOdom);
+
+	EXPECT_TRUE(poses.empty());
+	EXPECT_TRUE(links.empty());
+	EXPECT_TRUE(mapToOdom.isNull()) << "a null map_to_odom must survive as null";
+}
+
+TEST(MsgConversion, mapDataRoundTrip)
+{
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, sampleTransform()));
+
+	std::multimap<int, rtabmap::Link> links;
+	links.insert(std::make_pair(1, rtabmap::Link(
+			1, 2, rtabmap::Link::kNeighbor, sampleTransform())));
+
+	std::map<int, rtabmap::Signature> signatures;
+	rtabmap::Signature sig(1, 0, 3, 1234.5, "my_label", sampleTransform());
+	signatures.insert(std::make_pair(1, sig));
+
+	const rtabmap::Transform mapToOdom = rtabmap::Transform::getIdentity();
+
+	rtabmap_msgs::msg::MapData msg;
+	mapDataToROS(poses, links, signatures, mapToOdom, msg);
+	ASSERT_EQ(msg.nodes.size(), signatures.size());
+	ASSERT_EQ(msg.graph.poses.size(), poses.size());
+
+	std::map<int, rtabmap::Transform> outPoses;
+	std::multimap<int, rtabmap::Link> outLinks;
+	std::map<int, rtabmap::Signature> outSignatures;
+	rtabmap::Transform outMapToOdom;
+	mapDataFromROS(msg, outPoses, outLinks, outSignatures, outMapToOdom);
+
+	EXPECT_EQ(outPoses.size(), poses.size());
+	EXPECT_EQ(outLinks.size(), links.size());
+	ASSERT_EQ(outSignatures.size(), signatures.size());
+	ASSERT_TRUE(outSignatures.find(1) != outSignatures.end());
+	EXPECT_EQ(outSignatures.at(1).id(), sig.id());
+	EXPECT_EQ(outSignatures.at(1).getLabel(), sig.getLabel());
+	EXPECT_EQ(outSignatures.at(1).getWeight(), sig.getWeight());
+	EXPECT_NEAR(outSignatures.at(1).getStamp(), sig.getStamp(), 1e-6);
+}
+
+/////////////////////////
+// Node / Signature
+/////////////////////////
+
+namespace {
+
+rtabmap::Signature sampleSignature()
+{
+	rtabmap::Signature s(7, 2, 3, 1234.5, "node_label", sampleTransform());
+
+	std::multimap<int, int> words;
+	std::vector<cv::KeyPoint> kpts;
+	std::vector<cv::Point3f> pts3;
+	cv::Mat descriptors(2, 4, CV_32FC1);
+	for(int i=0; i<2; ++i)
+	{
+		words.insert(std::make_pair(100 + i, i));
+		kpts.push_back(cv::KeyPoint(cv::Point2f(10.0f * i, 20.0f * i), 7.0f));
+		pts3.push_back(cv::Point3f(1.0f * i, 2.0f * i, 3.0f * i));
+		for(int j=0; j<4; ++j)
+		{
+			descriptors.at<float>(i, j) = float(i * 4 + j);
+		}
+	}
+	s.setWords(words, kpts, pts3, descriptors);
+	return s;
+}
+
+}  // namespace
+
+TEST(MsgConversion, nodeRoundTrip)
+{
+	const rtabmap::Signature in = sampleSignature();
+
+	rtabmap_msgs::msg::Node msg;
+	nodeToROS(in, msg);
+	const rtabmap::Signature out = nodeFromROS(msg);
+
+	EXPECT_EQ(out.id(), in.id());
+	EXPECT_EQ(out.mapId(), in.mapId());
+	EXPECT_EQ(out.getWeight(), in.getWeight());
+	EXPECT_NEAR(out.getStamp(), in.getStamp(), 1e-6);
+	EXPECT_EQ(out.getLabel(), in.getLabel());
+	expectTransformNear(out.getPose(), in.getPose());
+
+	// Visual words: ids, keypoints, 3D points and descriptors.
+	ASSERT_EQ(out.getWords().size(), in.getWords().size());
+	EXPECT_TRUE(std::equal(out.getWords().begin(), out.getWords().end(), in.getWords().begin()));
+
+	ASSERT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size());
+	for(size_t i=0; i<in.getWordsKpts().size(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out.getWordsKpts()[i].pt.x, in.getWordsKpts()[i].pt.x) << "kpt " << i;
+		EXPECT_FLOAT_EQ(out.getWordsKpts()[i].pt.y, in.getWordsKpts()[i].pt.y) << "kpt " << i;
+	}
+
+	ASSERT_EQ(out.getWords3().size(), in.getWords3().size());
+	for(size_t i=0; i<in.getWords3().size(); ++i)
+	{
+		EXPECT_FLOAT_EQ(out.getWords3()[i].x, in.getWords3()[i].x) << "pt3 " << i;
+		EXPECT_FLOAT_EQ(out.getWords3()[i].z, in.getWords3()[i].z) << "pt3 " << i;
+	}
+
+	ASSERT_EQ(out.getWordsDescriptors().rows, in.getWordsDescriptors().rows);
+	ASSERT_EQ(out.getWordsDescriptors().cols, in.getWordsDescriptors().cols);
+	EXPECT_EQ(cv::countNonZero(out.getWordsDescriptors() != in.getWordsDescriptors()), 0);
+}
+
+TEST(MsgConversion, nodeGroundTruthRoundTrip)
+{
+	// The ground truth travels in the Node's SensorData sub-message but is written and
+	// read by the Node conversion itself.
+	rtabmap::Signature in(7, 2, 3, 1234.5, "node_label",
+			sampleTransform(), rtabmap::Transform(9.0f, 8.0f, 7.0f, 0.0f, 0.0f, 0.0f));
+
+	rtabmap_msgs::msg::Node msg;
+	nodeToROS(in, msg);
+	const rtabmap::Signature out = nodeFromROS(msg);
+
+	expectTransformNear(out.getGroundTruthPose(), in.getGroundTruthPose());
+}
+
+TEST(MsgConversion, nodeInfoRoundTripCarriesNoSensorData)
+{
+	const rtabmap::Signature in = sampleSignature();
+
+	rtabmap_msgs::msg::Node msg;
+	nodeInfoToROS(in, msg);
+	const rtabmap::Signature out = nodeInfoFromROS(msg);
+
+	EXPECT_EQ(out.id(), in.id());
+	EXPECT_EQ(out.mapId(), in.mapId());
+	EXPECT_EQ(out.getWeight(), in.getWeight());
+	EXPECT_EQ(out.getLabel(), in.getLabel());
+	expectTransformNear(out.getPose(), in.getPose());
+}
+
+TEST(MsgConversion, nodeDataRoundTripCarriesWords)
+{
+	const rtabmap::Signature in = sampleSignature();
+
+	rtabmap_msgs::msg::Node msg;
+	nodeDataToROS(in, msg);
+	const rtabmap::Signature out = nodeDataFromROS(msg);
+
+	EXPECT_EQ(out.getWords().size(), in.getWords().size());
+	EXPECT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size());
+	EXPECT_EQ(out.getWords3().size(), in.getWords3().size());
+}
+
+TEST(MsgConversion, nodeEmptyRoundTrip)
+{
+	rtabmap::Signature in(1);
+
+	rtabmap_msgs::msg::Node msg;
+	nodeToROS(in, msg);
+	const rtabmap::Signature out = nodeFromROS(msg);
+
+	EXPECT_EQ(out.id(), 1);
+	EXPECT_TRUE(out.getWords().empty());
+	EXPECT_TRUE(out.getWordsKpts().empty());
+	EXPECT_TRUE(out.getWordsDescriptors().empty());
+}
+
+/////////////////////////
+// SensorData
+/////////////////////////
+
+TEST(MsgConversion, sensorDataRoundTrip)
+{
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
+	const rtabmap::CameraModel model(
+			"cam", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(),
+			rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f));
+
+	rtabmap::SensorData in(cv::Mat(), cv::Mat(), model, 42, 1234.5);
+	in.setGroundTruth(sampleTransform());
+	in.setGPS(rtabmap::GPS(1234.5, -71.9, 45.4, 100.0, 5.0, 90.0));
+
+	rtabmap_msgs::msg::SensorData msg;
+	sensorDataToROS(in, msg, "base_link");
+	EXPECT_EQ(msg.header.frame_id, "base_link");
+
+	const rtabmap::SensorData out = sensorDataFromROS(msg);
+
+	EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
+
+	// sensorDataToROS writes ground_truth_pose into the message, but sensorDataFromROS
+	// deliberately does not read it back: the ground truth is owned by the enclosing
+	// Node conversion (nodeFromROS feeds it to the Signature constructor). See
+	// nodeGroundTruthRoundTrip for the round trip that does preserve it.
+	EXPECT_FALSE(transformFromPoseMsg(msg.ground_truth_pose).isNull())
+		<< "the message must still carry the ground truth for nodeFromROS";
+	EXPECT_TRUE(out.groundTruth().isNull())
+		<< "sensorDataFromROS does not restore the ground truth";
+
+	ASSERT_EQ(out.cameraModels().size(), 1u);
+	EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9);
+	EXPECT_NEAR(out.cameraModels()[0].cx(), 320.0, 1e-9);
+	expectTransformNear(
+			out.cameraModels()[0].localTransform(), model.localTransform());
+
+	EXPECT_NEAR(out.gps().longitude(), in.gps().longitude(), 1e-9);
+	EXPECT_NEAR(out.gps().latitude(), in.gps().latitude(), 1e-9);
+	EXPECT_NEAR(out.gps().altitude(), in.gps().altitude(), 1e-9);
+	EXPECT_NEAR(out.gps().bearing(), in.gps().bearing(), 1e-9);
+}
+
+TEST(MsgConversion, sensorDataUserDataRoundTrip)
+{
+	rtabmap::SensorData in;
+	in.setStamp(10.0);
+	in.setUserData((cv::Mat_<int>(1, 3) << 7, 8, 9));
+
+	rtabmap_msgs::msg::SensorData msg;
+	sensorDataToROS(in, msg);
+	const rtabmap::SensorData out = sensorDataFromROS(msg);
+
+	const cv::Mat data = out.userDataRaw().empty()
+			? rtabmap::uncompressData(out.userDataCompressed())
+			: out.userDataRaw();
+	ASSERT_FALSE(data.empty());
+	ASSERT_EQ(data.cols, 3);
+	EXPECT_EQ(data.at<int>(0, 0), 7);
+	EXPECT_EQ(data.at<int>(0, 2), 9);
+}
+
+/////////////////////////
+// Statistics / Info
+/////////////////////////
+
+TEST(MsgConversion, infoRoundTrip)
+{
+	rtabmap::Statistics in;
+	in.setExtended(true);
+	in.setRefImageId(5);
+	in.setLoopClosureId(9);
+	in.setProximityDetectionId(11);
+	in.setStamp(1234.5);
+	in.setLoopClosureTransform(sampleTransform());
+	in.setWmState(std::vector<int>{1, 2, 3});
+
+	std::map<int, float> posterior;
+	posterior.insert(std::make_pair(1, 0.25f));
+	posterior.insert(std::make_pair(2, 0.75f));
+	in.setPosterior(posterior);
+
+	std::map<int, int> weights;
+	weights.insert(std::make_pair(1, 10));
+	in.setWeights(weights);
+
+	std::map<int, std::string> labels;
+	labels.insert(std::make_pair(1, "kitchen"));
+	in.setLabels(labels);
+
+	in.addStatistic("Some/Stat/", 3.5f);
+
+	rtabmap_msgs::msg::Info msg;
+	infoToROS(in, msg);
+
+	// An unstamped header is filled from the statistics, so infoFromROS recovers the
+	// stamp without the caller doing anything. Only to double precision, though.
+	EXPECT_NEAR(timestampFromROS(msg.header.stamp), in.stamp(), 1e-6);
+	EXPECT_TRUE(msg.header.frame_id.empty()) << "the frame id is always the caller's job";
+
+	rtabmap::Statistics out;
+	infoFromROS(msg, out);
+
+	EXPECT_EQ(out.refImageId(), in.refImageId());
+	EXPECT_EQ(out.loopClosureId(), in.loopClosureId());
+	EXPECT_EQ(out.proximityDetectionId(), in.proximityDetectionId());
+	EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
+	expectTransformNear(out.loopClosureTransform(), in.loopClosureTransform());
+	EXPECT_EQ(out.wmState(), in.wmState());
+
+	ASSERT_EQ(out.posterior().size(), in.posterior().size());
+	EXPECT_FLOAT_EQ(out.posterior().at(1), 0.25f);
+	EXPECT_FLOAT_EQ(out.posterior().at(2), 0.75f);
+
+	ASSERT_EQ(out.weights().size(), in.weights().size());
+	EXPECT_EQ(out.weights().at(1), 10);
+
+	ASSERT_EQ(out.labels().size(), in.labels().size());
+	EXPECT_EQ(out.labels().at(1), "kitchen");
+
+	ASSERT_TRUE(out.data().find("Some/Stat/") != out.data().end());
+	EXPECT_FLOAT_EQ(out.data().at("Some/Stat/"), 3.5f);
+}
+
+/////////////////////////
+// PointCloud2 helpers
+/////////////////////////
+
+namespace {
+
+/// Builds a dense, unorganized XYZ float cloud from the given points.
+sensor_msgs::msg::PointCloud2 makeXYZCloud(const std::vector<cv::Point3f> & points)
+{
+	sensor_msgs::msg::PointCloud2 cloud;
+	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;
+}
+
+cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	const float * p = reinterpret_cast<const float *>(&cloud.data[index * cloud.point_step]);
+	return cv::Point3f(p[0], p[1], p[2]);
+}
+
+}  // namespace
+
+TEST(MsgConversion, transformPointCloudTranslation)
+{
+	const std::vector<cv::Point3f> points = {{1.0f, 2.0f, 3.0f}, {-1.0f, 0.0f, 1.0f}};
+	const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points);
+
+	Eigen::Matrix4f t = Eigen::Matrix4f::Identity();
+	t(0, 3) = 10.0f;
+	t(1, 3) = 20.0f;
+	t(2, 3) = 30.0f;
+
+	sensor_msgs::msg::PointCloud2 out;
+	transformPointCloud(t, in, out);
+
+	ASSERT_EQ(out.width, in.width);
+	ASSERT_EQ(out.point_step, in.point_step);
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		const cv::Point3f p = readXYZ(out, i);
+		EXPECT_NEAR(p.x, points[i].x + 10.0f, 1e-4) << "point " << i;
+		EXPECT_NEAR(p.y, points[i].y + 20.0f, 1e-4) << "point " << i;
+		EXPECT_NEAR(p.z, points[i].z + 30.0f, 1e-4) << "point " << i;
+	}
+}
+
+TEST(MsgConversion, transformPointCloudRotation)
+{
+	// 90 degrees about z maps (1,0,0) to (0,1,0).
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points);
+
+	const Eigen::Matrix4f t =
+			rtabmap::Transform(0, 0, 0, 0, 0, M_PI/2.0).toEigen4f();
+
+	sensor_msgs::msg::PointCloud2 out;
+	transformPointCloud(t, in, out);
+
+	const cv::Point3f p = readXYZ(out, 0);
+	EXPECT_NEAR(p.x, 0.0f, 1e-5);
+	EXPECT_NEAR(p.y, 1.0f, 1e-5);
+	EXPECT_NEAR(p.z, 0.0f, 1e-5);
+}
+
+TEST(MsgConversion, transformPointCloudIdentityPreservesMetadata)
+{
+	const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 2.0f, 3.0f}});
+
+	sensor_msgs::msg::PointCloud2 out;
+	transformPointCloud(Eigen::Matrix4f::Identity(), in, out);
+
+	EXPECT_EQ(out.height, in.height);
+	EXPECT_EQ(out.width, in.width);
+	EXPECT_EQ(out.point_step, in.point_step);
+	EXPECT_EQ(out.row_step, in.row_step);
+	EXPECT_EQ(out.is_dense, in.is_dense);
+	ASSERT_EQ(out.fields.size(), in.fields.size());
+	for(size_t i=0; i<in.fields.size(); ++i)
+	{
+		EXPECT_EQ(out.fields[i].name, in.fields[i].name) << "field " << i;
+	}
+
+	const cv::Point3f p = readXYZ(out, 0);
+	EXPECT_NEAR(p.x, 1.0f, 1e-5);
+	EXPECT_NEAR(p.y, 2.0f, 1e-5);
+	EXPECT_NEAR(p.z, 3.0f, 1e-5);
+}
+
+namespace {
+
+// A robot driving straight at a wall while the lidar sweeps.
+// A 10 Hz lidar: 100 samples 1 ms apart, so the sweep spans 99 ms and scans repeat
+// every 100 ms. Scan N covers [1000.000, 1000.099], scan N+1 covers [1000.100, 1000.199].
+constexpr size_t kScanPoints = 100;      // samples per sweep
+constexpr double kScanStep = 0.001;      // s between consecutive samples
+constexpr double kScanSpan = kScanStep * (kScanPoints - 1);   // 0.099 s, first -> last
+constexpr float kWallDistance = 5.0f;    // m, distance to the wall at the first point
+constexpr float kSpeed = 1.0f;           // m/s forward (+x)
+
+/// How the per-point time channel is encoded. deskew() accepts three datatypes, and
+/// FLOAT64 differs from the other two: it carries ABSOLUTE stamps (with an automatic
+/// ms/us/ns unit guess), while UINT32 and FLOAT32 carry offsets from the header stamp.
+enum TimeEncoding
+{
+	kOffsetSecFloat32,   ///< FLOAT32 seconds, relative to header.stamp
+	kOffsetNsecUint32,   ///< UINT32 nanoseconds, relative to header.stamp
+	kAbsoluteSecFloat64, ///< FLOAT64 absolute seconds
+	kAbsoluteMsecFloat64 ///< FLOAT64 absolute milliseconds (auto-scaled by deskew)
+};
+
+/// Organized-cloud layout. deskew() picks its traversal from width>height, so the two
+/// orderings exercise different loops.
+enum ScanLayout
+{
+	kTimeOnColumns,  ///< Ouster style: width=time samples, height=rings
+	kTimeOnRows      ///< Velodyne style: height=time samples, width=rings
+};
+
+/**
+ * Builds the raw (skewed) scan of a flat wall captured while moving forward.
+ *
+ * Each time sample is taken 1 ms after the previous one, by which time the robot has
+ * closed in on the wall by kSpeed * elapsed. Expressed in the sensor frame at capture
+ * time, the wall therefore appears to slide towards the robot: a straight wall is
+ * recorded as a slanted line. Deskewing must undo exactly that.
+ *
+ * @param headerStamp       absolute stamp put in the message header
+ * @param firstPointOffset  time of the first sample relative to the header stamp
+ * @param encoding          how to write the time channel
+ * @param layout            whether time runs along columns or rows
+ * @param rings             number of rings (the non-time dimension)
+ * @param fieldName         name of the time channel
+ * @param descendingTime    emit the samples newest-first, which deskew has to detect
+ * @param displacement      distance travelled as a function of time since the first
+ *                          sample; defaults to the constant-velocity kSpeed * elapsed
+ */
+sensor_msgs::msg::PointCloud2 makeSkewedWallScan(
+		double headerStamp,
+		double firstPointOffset,
+		TimeEncoding encoding = kOffsetSecFloat32,
+		ScanLayout layout = kTimeOnColumns,
+		size_t rings = 1,
+		const std::string & fieldName = "t",
+		bool descendingTime = false,
+		const std::function<double(double)> & displacement = nullptr)
+{
+	const bool timeIs64Bit =
+			encoding == kAbsoluteSecFloat64 || encoding == kAbsoluteMsecFloat64;
+	// Keep the 8-byte time channel aligned: x,y,z then 4 bytes of padding.
+	const uint32_t timeOffset = timeIs64Bit ? 16 : 12;
+	const uint32_t pointStep = timeIs64Bit ? 24 : 16;
+
+	sensor_msgs::msg::PointCloud2 cloud;
+	cloud.header.stamp = timestampToROS(headerStamp);
+	cloud.header.frame_id = "base_link";
+	cloud.is_bigendian = false;
+	cloud.is_dense = true;
+	if(layout == kTimeOnColumns)
+	{
+		cloud.width = kScanPoints;
+		cloud.height = rings;
+	}
+	else
+	{
+		cloud.width = rings;
+		cloud.height = kScanPoints;
+	}
+
+	cloud.fields.resize(4);
+	const char * xyz[3] = {"x", "y", "z"};
+	for(int i=0; i<3; ++i)
+	{
+		cloud.fields[i].name = xyz[i];
+		cloud.fields[i].offset = 4 * i;
+		cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+		cloud.fields[i].count = 1;
+	}
+	cloud.fields[3].name = fieldName;
+	cloud.fields[3].offset = timeOffset;
+	cloud.fields[3].datatype =
+			encoding == kOffsetNsecUint32 ? sensor_msgs::msg::PointField::UINT32 :
+			timeIs64Bit                   ? sensor_msgs::msg::PointField::FLOAT64 :
+			                                sensor_msgs::msg::PointField::FLOAT32;
+	cloud.fields[3].count = 1;
+
+	cloud.point_step = pointStep;
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(cloud.row_step * cloud.height);
+
+	for(size_t i=0; i<kScanPoints; ++i)          // position in the message
+	{
+		// When descending, the point stored first is the one captured last.
+		const size_t sample = descendingTime ? (kScanPoints - 1 - i) : i;
+		const double elapsed = double(sample) * kScanStep;    // since the first capture
+		const double offset = firstPointOffset + elapsed;     // relative to the header
+		const double absolute = headerStamp + offset;
+
+		for(size_t r=0; r<rings; ++r)
+		{
+			const size_t row = (layout == kTimeOnColumns) ? r : i;
+			const size_t col = (layout == kTimeOnColumns) ? i : r;
+			unsigned char * base = &cloud.data[row * cloud.row_step + col * cloud.point_step];
+
+			float * p = reinterpret_cast<float *>(base);
+			// The robot has closed in on the wall by this much when the sample was taken.
+			const double travelled = displacement ? displacement(elapsed) : kSpeed * elapsed;
+			p[0] = kWallDistance - float(travelled);          // the skew
+			p[1] = -1.0f + 2.0f * float(sample) / float(kScanPoints - 1);
+			p[2] = 0.1f * float(r);                            // one plane per ring
+
+			switch(encoding)
+			{
+				case kOffsetSecFloat32:
+					*reinterpret_cast<float *>(base + timeOffset) = float(offset);
+					break;
+				case kOffsetNsecUint32:
+					*reinterpret_cast<uint32_t *>(base + timeOffset) =
+							uint32_t(std::llround(offset * 1e9));
+					break;
+				case kAbsoluteSecFloat64:
+					*reinterpret_cast<double *>(base + timeOffset) = absolute;
+					break;
+				case kAbsoluteMsecFloat64:
+					*reinterpret_cast<double *>(base + timeOffset) = absolute * 1e3;
+					break;
+			}
+		}
+	}
+	return cloud;
+}
+
+/// Reads x of the point at (time sample, ring) for the given layout.
+float readWallX(const sensor_msgs::msg::PointCloud2 & cloud, size_t sample, size_t ring,
+		ScanLayout layout)
+{
+	const size_t row = (layout == kTimeOnColumns) ? ring : sample;
+	const size_t col = (layout == kTimeOnColumns) ? sample : ring;
+	return *reinterpret_cast<const float *>(
+			&cloud.data[row * cloud.row_step + col * cloud.point_step]);
+}
+
+float readField(const sensor_msgs::msg::PointCloud2 & cloud, size_t index, size_t field)
+{
+	return *reinterpret_cast<const float *>(
+			&cloud.data[index * cloud.point_step + cloud.fields[field].offset]);
+}
+
+}  // namespace
+
+TEST(MsgConversion, deskewConstantVelocityHeaderAtFirstPoint)
+{
+	const double firstPointStamp = 1000.0;
+
+	// Header stamped at the first point, so "t" runs 0 .. +0.100 s.
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0);
+	ASSERT_NEAR(readField(in, 0, 0), kWallDistance, 1e-4) << "first point is unskewed";
+	ASSERT_NEAR(readField(in, kScanPoints-1, 0), kWallDistance - float(kScanSpan), 1e-4)
+		<< "last point is skewed by v*0.099s = 9.9 cm";
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	// Everything collapses back onto the wall at its original distance.
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readField(out, i, 0), kWallDistance, 1e-4) << "x of point " << i;
+		EXPECT_NEAR(readField(out, i, 1), readField(in, i, 1), 1e-6) << "y of point " << i;
+		EXPECT_NEAR(readField(out, i, 2), 0.0f, 1e-6) << "z of point " << i;
+		EXPECT_FLOAT_EQ(readField(out, i, 3), 0.0f)
+			<< "t must be zeroed to mark the cloud as deskewed, point " << i;
+	}
+}
+
+TEST(MsgConversion, deskewConstantVelocityHeaderAtLastPoint)
+{
+	const double firstPointStamp = 1000.0;
+	const double lastPointStamp = firstPointStamp + kScanSpan;
+
+	// Same physical scan, but stamped at the last point: "t" runs -0.100 .. 0 s.
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(lastPointStamp, -kScanSpan);
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	// Still a straight line, but now expressed in the frame at the END of the scan,
+	// by which point the robot has advanced kSpeed * kScanSpan = 9.9 cm.
+	const float expected = kWallDistance - kSpeed * float(kScanSpan);
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readField(out, i, 0), expected, 1e-4) << "x of point " << i;
+	}
+
+	// The line is flat to well under a millimeter: that is the deskewing working,
+	// independently of which end of the scan the frame is anchored to.
+	float minX = readField(out, 0, 0);
+	float maxX = minX;
+	for(size_t i=1; i<kScanPoints; ++i)
+	{
+		minX = std::min(minX, readField(out, i, 0));
+		maxX = std::max(maxX, readField(out, i, 0));
+	}
+	EXPECT_LT(maxX - minX, 1e-3f) << "deskewed scan must be flat";
+}
+
+TEST(MsgConversion, deskewConstantVelocityStationaryRobotIsNoOp)
+{
+	// With no motion there is nothing to correct, so the skewed input must come back
+	// unchanged -- this pins that the correction is driven by the velocity and not by
+	// something incidental to the time field.
+	const double firstPointStamp = 1000.0;
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0);
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(0, 0, 0, 0, 0, 0)));
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readField(out, i, 0), readField(in, i, 0), 1e-6) << "point " << i;
+	}
+}
+
+/// Deskews a wall scan built with the given encoding/layout and asserts it comes out
+/// flat at the expected distance.
+void expectDeskewRecoversWall(
+		TimeEncoding encoding,
+		ScanLayout layout,
+		size_t rings,
+		double headerStamp,
+		const std::string & fieldName = "t")
+{
+	SCOPED_TRACE("encoding=" + std::to_string(int(encoding)) +
+			" layout=" + std::to_string(int(layout)) +
+			" rings=" + std::to_string(rings) +
+			" field=" + fieldName);
+
+	const sensor_msgs::msg::PointCloud2 in =
+			makeSkewedWallScan(headerStamp, 0.0, encoding, layout, rings, fieldName);
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		for(size_t r=0; r<rings; ++r)
+		{
+			EXPECT_NEAR(readWallX(out, i, r, layout), kWallDistance, 1e-3)
+				<< "sample " << i << " ring " << r;
+		}
+	}
+}
+
+TEST(MsgConversion, deskewTimeAsFloat32SecondsOffset)
+{
+	expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 1, 1000.0);
+}
+
+TEST(MsgConversion, deskewTimeAsUint32NanosecondsOffset)
+{
+	// UINT32 offsets are unsigned, so the header can only sit at or before the scan.
+	expectDeskewRecoversWall(kOffsetNsecUint32, kTimeOnColumns, 1, 1000.0);
+}
+
+TEST(MsgConversion, deskewTimeAsFloat64AbsoluteSeconds)
+{
+	// FLOAT64 carries absolute stamps rather than offsets.
+	expectDeskewRecoversWall(kAbsoluteSecFloat64, kTimeOnColumns, 1, 1000.0);
+}
+
+TEST(MsgConversion, deskewTimeAsFloat64AbsoluteMilliseconds)
+{
+	// Above 1e12 deskew treats FLOAT64 stamps as milliseconds and rescales them, so
+	// this needs a realistic epoch: 1.7e9 s is 1.7e12 ms.
+	expectDeskewRecoversWall(kAbsoluteMsecFloat64, kTimeOnColumns, 1, 1.7e9);
+}
+
+TEST(MsgConversion, deskewTimeOnColumnsWithMultipleRings)
+{
+	// Ouster layout: width=101 samples > height=4 rings.
+	expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 4, 1000.0);
+}
+
+TEST(MsgConversion, deskewTimeOnRowsWithMultipleRings)
+{
+	// Velodyne layout: height=101 samples > width=4 rings, which takes the other loop.
+	expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnRows, 4, 1000.0);
+}
+
+TEST(MsgConversion, deskewLayoutsAgree)
+{
+	// The same scan expressed in either layout must deskew to the same geometry.
+	const double headerStamp = 1000.0;
+	const size_t rings = 4;
+
+	sensor_msgs::msg::PointCloud2 byColumns, byRows;
+	ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, rings),
+			byColumns, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+	ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnRows, rings),
+			byRows, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		for(size_t r=0; r<rings; ++r)
+		{
+			EXPECT_NEAR(readWallX(byColumns, i, r, kTimeOnColumns),
+					readWallX(byRows, i, r, kTimeOnRows), 1e-6)
+				<< "sample " << i << " ring " << r;
+		}
+	}
+}
+
+TEST(MsgConversion, deskewAcceptsEveryTimeFieldName)
+{
+	for(const std::string & name : {"t", "time", "stamps", "timestamp"})
+	{
+		expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 1, 1000.0, name);
+	}
+}
+
+TEST(MsgConversion, deskewHandlesDescendingTimestamps)
+{
+	// Some drivers emit the sweep newest-first. deskew detects that the channel is not
+	// ascending and rescans it for the true min/max before interpolating.
+	const double headerStamp = 1000.0;
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
+			headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t",
+			/*descendingTime=*/true);
+
+	// Sanity: the stored order really is newest-first.
+	ASSERT_NEAR(readWallX(in, 0, 0, kTimeOnColumns), kWallDistance - float(kScanSpan), 1e-4);
+	ASSERT_NEAR(readWallX(in, kScanPoints-1, 0, kTimeOnColumns), kWallDistance, 1e-4);
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
+			<< "sample " << i;
+	}
+}
+
+TEST(MsgConversion, deskewRejectsUnknownTimeFieldName)
+{
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
+			1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "elapsed");
+
+	sensor_msgs::msg::PointCloud2 out;
+	EXPECT_FALSE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+}
+
+TEST(MsgConversion, deskewRejectsUnsupportedTimeDatatype)
+{
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
+	in.fields[3].datatype = sensor_msgs::msg::PointField::INT32;   // 4 bytes, but not 6/7/8
+
+	sensor_msgs::msg::PointCloud2 out;
+	EXPECT_FALSE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+}
+
+TEST(MsgConversion, deskewWithRotationIsAnchoredAtTheHeaderStamp)
+{
+	// A pure yaw rate makes the correction a pure rotation about z, so it can be checked
+	// exactly: each sample must be rotated by yawRate * (its time - the header stamp),
+	// with its distance from the origin unchanged. This is what pins the correction to
+	// the header stamp -- there is no other reference time involved.
+	const double headerStamp = 1000.0;
+	const double yawRate = 0.5;   // rad/s
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0);
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(0, 0, 0, 0, 0, yawRate)));
+
+	auto xy = [](const sensor_msgs::msg::PointCloud2 & c, size_t i) {
+		const float * p = reinterpret_cast<const float *>(&c.data[i * c.point_step]);
+		return std::make_pair(p[0], p[1]);
+	};
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		const std::pair<float, float> a = xy(in, i);
+		const std::pair<float, float> b = xy(out, i);
+		const double dt = double(i) * kScanStep;   // sample 0 sits at the header stamp
+
+		EXPECT_NEAR(std::hypot(b.first, b.second), std::hypot(a.first, a.second), 1e-4)
+			<< "a rotation must preserve the range of sample " << i;
+		EXPECT_NEAR(std::atan2(b.second, b.first) - std::atan2(a.second, a.first),
+				yawRate * dt, 1e-4)
+			<< "sample " << i << " must be rotated by yawRate*dt";
+	}
+
+	// Spelling out the i=0 case: dt is zero there, so that sample is untouched.
+	EXPECT_FLOAT_EQ(xy(out, 0).first, xy(in, 0).first);
+	EXPECT_FLOAT_EQ(xy(out, 0).second, xy(in, 0).second);
+}
+
+TEST(MsgConversion, deskewPassesThroughWhenThereIsNoTimeSpread)
+{
+	// A driver that leaves the time channel at zero gives a scan with no time spread.
+	// There is nothing to correct, so the cloud must come back unchanged rather than
+	// being reported as a failure -- callers abort the frame on false.
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		*reinterpret_cast<float *>(&in.data[i * in.point_step + in.fields[3].offset]) = 0.0f;
+	}
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+	EXPECT_EQ(out.data, in.data) << "the cloud must be returned untouched";
+}
+
+TEST(MsgConversion, deskewIsIdempotent)
+{
+	// Deskewing zeroes the time channel to mark the cloud as done, so running deskew a
+	// second time (e.g. lidar_deskewing feeding icp_odometry) must be a silent no-op.
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
+	const rtabmap::Transform velocity(kSpeed, 0, 0, 0, 0, 0);
+
+	sensor_msgs::msg::PointCloud2 once;
+	ASSERT_TRUE(deskew(in, once, velocity));
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		ASSERT_FLOAT_EQ(*reinterpret_cast<const float *>(
+				&once.data[i * once.point_step + once.fields[3].offset]), 0.0f)
+			<< "deskewing must zero the time channel, sample " << i;
+	}
+
+	sensor_msgs::msg::PointCloud2 twice;
+	ASSERT_TRUE(deskew(once, twice, velocity)) << "a second pass must not fail";
+	EXPECT_EQ(twice.data, once.data) << "a second pass must change nothing";
+}
+
+TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep)
+{
+	// The ordering check only inspects the first and last samples, so a corrupt stamp in
+	// the middle is not detected. It must be clamped to the end of the sweep rather than
+	// extrapolated, which would fling the point far past the wall.
+	const double headerStamp = 1000.0;
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0);
+	const size_t corrupt = kScanPoints / 2;
+	*reinterpret_cast<float *>(
+			&in.data[corrupt * in.point_step + in.fields[3].offset]) = 0.5f;  // 5x the sweep
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0)));
+
+	// Clamped to the last sample's correction, so it lands within the sweep's own range
+	// rather than meters away. Every other sample is unaffected.
+	const float x = readWallX(out, corrupt, 0, kTimeOnColumns);
+	EXPECT_GE(x, kWallDistance - 1e-3f);
+	EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f)
+		<< "an unclamped ratio of ~5 would put this point ~0.45 m past the wall";
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		if(i == corrupt) continue;
+		EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
+			<< "uncorrupted sample " << i << " must be unaffected";
+	}
+}
+
+TEST(MsgConversion, deskewWithoutTimeFieldFails)
+{
+	// Deskewing needs a per-point time field; a plain XYZ cloud cannot be deskewed.
+	const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
+
+	sensor_msgs::msg::PointCloud2 out;
+	EXPECT_FALSE(deskew(in, out, rtabmap::Transform(1, 0, 0, 0, 0, 0)));
+}
+
+TEST(MsgConversion, deskewNullVelocityFails)
+{
+	const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
+
+	sensor_msgs::msg::PointCloud2 out;
+	EXPECT_FALSE(deskew(in, out, rtabmap::Transform()))
+		<< "a null velocity cannot deskew";
+}
+
+/////////////////////////
+// RGBDImage
+/////////////////////////
+
+TEST(MsgConversion, rgbdImageRoundTrip)
+{
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0);
+	const rtabmap::CameraModel model(
+			"cam", cv::Size(4, 4), K, cv::Mat(), cv::Mat(), cv::Mat(),
+			rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f));
+
+	cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
+	cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
+	rtabmap::SensorData in(rgb, depth, model, 1, 1234.5);
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	rgbdImageToROS(in, msg, "camera_link");
+
+	EXPECT_EQ(msg.rgb_camera_info.header.frame_id, "camera_link");
+	EXPECT_NEAR(timestampFromROS(msg.rgb_camera_info.header.stamp), 1234.5, 1e-6);
+
+	// The top-level header is stamped too, so rgbdImageFromROS recovers the stamp
+	// without the caller having to fill it in.
+	EXPECT_EQ(msg.header.frame_id, "camera_link");
+	EXPECT_NEAR(timestampFromROS(msg.header.stamp), 1234.5, 1e-6);
+
+	// The returned SensorData shallow-references the message buffers, so the message
+	// must outlive it -- see rgbdImageFromROSAliasesTheMessage.
+	const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr held =
+			std::make_shared<const rtabmap_msgs::msg::RGBDImage>(msg);
+	const rtabmap::SensorData out = rgbdImageFromROS(held);
+
+	EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6);
+	ASSERT_EQ(out.cameraModels().size(), 1u);
+	EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9);
+
+	// The local transform is not carried by the message (CameraInfo has no such
+	// field); callers resolve it from TF, so it comes back as the default identity.
+	EXPECT_TRUE(out.cameraModels()[0].localTransform().isIdentity())
+		<< out.cameraModels()[0].localTransform().prettyPrint();
+
+	ASSERT_FALSE(out.imageRaw().empty());
+	EXPECT_EQ(out.imageRaw().type(), CV_8UC3);
+	EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0);
+
+	ASSERT_FALSE(out.depthRaw().empty());
+	EXPECT_EQ(out.depthRaw().type(), CV_16UC1);
+	EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0);
+}
+
+TEST(MsgConversion, rgbdImageFromROSAliasesTheMessage)
+{
+	// rgbdImageFromROS deliberately avoids copying the pixels: the SensorData it returns
+	// points into the message's own buffers. Mutating the message is visible through the
+	// SensorData. Callers must therefore keep the message alive and unchanged for as long
+	// as they use the result -- and must deep-copy before letting the SensorData outlive
+	// the subscription callback, since the ROS queue recycles the message once it
+	// returns.
+	cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
+	cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
+
+	auto msg = std::make_shared<rtabmap_msgs::msg::RGBDImage>();
+	msg->rgb_camera_info.width = 4;
+	msg->rgb_camera_info.height = 4;
+	msg->rgb_camera_info.k = {525.0, 0.0, 2.0, 0.0, 525.0, 2.0, 0.0, 0.0, 1.0};
+	cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb);
+	cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg->depth);
+
+	const rtabmap::SensorData data = rgbdImageFromROS(msg);
+	ASSERT_FALSE(data.imageRaw().empty());
+	ASSERT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
+
+	// Writing through the message is observable in the SensorData: no copy was made.
+	msg->rgb.data[0] = 99;
+	EXPECT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0)[0], 99)
+		<< "SensorData is expected to alias the message buffer";
+}
+
+TEST(MsgConversion, toCvCopyReadsRawImages)
+{
+	cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
+	cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb);
+	cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth);
+
+	cv_bridge::CvImagePtr rgbPtr, depthPtr;
+	toCvCopy(msg, rgbPtr, depthPtr);
+
+	ASSERT_TRUE(rgbPtr && depthPtr);
+	EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
+	EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
+
+	// The copy must be independent of the message buffer.
+	rgbPtr->image.at<cv::Vec3b>(0, 0) = cv::Vec3b(0, 0, 0);
+	EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
+}
+
+TEST(MsgConversion, toCvCopyEmptyImageYieldsEmptyPtr)
+{
+	rtabmap_msgs::msg::RGBDImage msg;
+
+	cv_bridge::CvImagePtr rgbPtr, depthPtr;
+	toCvCopy(msg, rgbPtr, depthPtr);
+
+	ASSERT_TRUE(rgbPtr && depthPtr) << "pointers must be valid even with no image";
+	EXPECT_TRUE(rgbPtr->image.empty());
+	EXPECT_TRUE(depthPtr->image.empty());
+}
+
+TEST(MsgConversion, toCvShareAliasesRawImages)
+{
+	cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30));
+	cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb);
+	cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth);
+
+	cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
+	toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
+
+	ASSERT_TRUE(rgbPtr && depthPtr);
+	ASSERT_FALSE(rgbPtr->image.empty());
+	EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
+	EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
+}
+
+/////////////////////////
+// Compressed images
+/////////////////////////
+
+namespace {
+
+/// Builds a depth image compressed the way rtabmap does it (not a jpg/png CompressedImage).
+sensor_msgs::msg::CompressedImage makeRtabmapCompressedDepth(const cv::Mat & depth)
+{
+	sensor_msgs::msg::CompressedImage msg;
+	msg.format = "";   // anything but "jpg" takes the rtabmap::uncompressImage path
+	msg.data = rtabmap::compressImage(depth, ".png");
+	return msg;
+}
+
+}  // namespace
+
+TEST(MsgConversion, toCvCopyReadsCompressedDepth)
+{
+	const cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1234));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.depth_compressed = makeRtabmapCompressedDepth(depth);
+
+	cv_bridge::CvImagePtr rgbPtr, depthPtr;
+	toCvCopy(msg, rgbPtr, depthPtr);
+
+	ASSERT_TRUE(depthPtr);
+	ASSERT_FALSE(depthPtr->image.empty());
+	EXPECT_EQ(depthPtr->image.type(), CV_16UC1);
+	EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
+}
+
+TEST(MsgConversion, toCvShareReadsCompressedDepth)
+{
+	const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.5f));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.depth_compressed = makeRtabmapCompressedDepth(depth);
+
+	cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
+	toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
+
+	ASSERT_TRUE(depthPtr);
+	ASSERT_FALSE(depthPtr->image.empty());
+	EXPECT_EQ(depthPtr->image.type(), CV_32FC1);
+	EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+	EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0);
+}
+
+TEST(MsgConversion, toCvShareOnAnEmptyMessageGivesEmptyImages)
+{
+	// Both pointers must be valid even when the message carries nothing: callers such as
+	// rgbdImageFromROS() dereference them unconditionally.
+	const rtabmap_msgs::msg::RGBDImage msg;
+
+	cv_bridge::CvImageConstPtr rgbPtr, depthPtr;
+	toCvShare(msg, std::shared_ptr<void const>(), rgbPtr, depthPtr);
+
+	ASSERT_TRUE(rgbPtr);
+	ASSERT_TRUE(depthPtr);
+	EXPECT_TRUE(rgbPtr->image.empty());
+	EXPECT_TRUE(depthPtr->image.empty());
+}
+
+TEST(MsgConversion, rgbdImageFromROSOnAnEmptyMessageIsInvalid)
+{
+	rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
+			std::make_shared<rtabmap_msgs::msg::RGBDImage>();
+	msg->header.frame_id = "camera_link";
+	msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
+
+	const rtabmap::SensorData data = rgbdImageFromROS(msg);
+
+	EXPECT_FALSE(data.isValid()) << "an empty message must give empty data, not a crash";
+}
+
+TEST(MsgConversion, rgbdImageFromROSWithoutDepthKeepsTheColorImage)
+{
+	// The depth image is optional: color plus camera info is a valid message, and the
+	// resolution check must not divide by the zero depth width.
+	rtabmap_msgs::msg::RGBDImage::SharedPtr msg =
+			std::make_shared<rtabmap_msgs::msg::RGBDImage>();
+	msg->header.frame_id = "camera_link";
+	msg->header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
+	cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb);
+	msg->rgb_camera_info.width = 8;
+	msg->rgb_camera_info.height = 8;
+	msg->rgb_camera_info.k = {525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0};
+
+	const rtabmap::SensorData data = rgbdImageFromROS(msg);
+
+	EXPECT_TRUE(data.isValid());
+	ASSERT_FALSE(data.imageRaw().empty());
+	EXPECT_EQ(data.imageRaw().at<cv::Vec3b>(0, 0), cv::Vec3b(10, 20, 30));
+	EXPECT_TRUE(data.depthRaw().empty());
+	ASSERT_EQ(data.cameraModels().size(), 1u);
+	EXPECT_NEAR(data.cameraModels()[0].fx(), 525.0, 1e-9);
+}
+
+TEST(MsgConversion, toCvCopyReadsCompressedRgb)
+{
+	const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.rgb_compressed.format = "png";
+	msg.rgb_compressed.data = rtabmap::compressImage(rgb, ".png");
+
+	cv_bridge::CvImagePtr rgbPtr, depthPtr;
+	toCvCopy(msg, rgbPtr, depthPtr);
+
+	ASSERT_TRUE(rgbPtr);
+	ASSERT_FALSE(rgbPtr->image.empty());
+	EXPECT_EQ(rgbPtr->image.type(), CV_8UC3);
+	EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0);
+}
+
+/////////////////////////
+// SensorData: raw copies, laser scans, stereo
+/////////////////////////
+
+TEST(MsgConversion, sensorDataToROSCopyRawDataCarriesImages)
+{
+	cv::Mat K = (cv::Mat_<double>(3, 3) <<
+			525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0);
+	const rtabmap::CameraModel model("cam", cv::Size(8, 8), K, cv::Mat(), cv::Mat(), cv::Mat());
+
+	const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(2000));
+	rtabmap::SensorData in(rgb, depth, model, 1, 1000.0);
+
+	// Without copyRawData the raw images are not serialized...
+	rtabmap_msgs::msg::SensorData without;
+	sensorDataToROS(in, without, "base_link", /*copyRawData=*/false);
+	EXPECT_TRUE(without.left.data.empty());
+	EXPECT_TRUE(without.right.data.empty());
+
+	// ...with it, they are.
+	rtabmap_msgs::msg::SensorData with;
+	sensorDataToROS(in, with, "base_link", /*copyRawData=*/true);
+	ASSERT_FALSE(with.left.data.empty());
+	ASSERT_FALSE(with.right.data.empty());
+	EXPECT_EQ(with.left.encoding, sensor_msgs::image_encodings::BGR8);
+	EXPECT_EQ(with.right.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+
+	const rtabmap::SensorData out = sensorDataFromROS(with);
+	ASSERT_FALSE(out.imageRaw().empty());
+	EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0);
+	ASSERT_FALSE(out.depthRaw().empty());
+	EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0);
+}
+
+TEST(MsgConversion, sensorDataLaserScanRoundTrip)
+{
+	cv::Mat points(1, 3, CV_32FC3);
+	points.at<cv::Vec3f>(0, 0) = cv::Vec3f(1.0f, 0.0f, 0.0f);
+	points.at<cv::Vec3f>(0, 1) = cv::Vec3f(0.0f, 2.0f, 0.0f);
+	points.at<cv::Vec3f>(0, 2) = cv::Vec3f(0.0f, 0.0f, 3.0f);
+
+	const rtabmap::Transform localTransform(0.0f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f);
+	const rtabmap::LaserScan scan(points, /*maxPoints=*/100, /*maxRange=*/40.0f,
+			rtabmap::LaserScan::kXYZ, localTransform);
+
+	rtabmap::SensorData in;
+	in.setStamp(1000.0);
+	in.setLaserScan(scan);
+
+	rtabmap_msgs::msg::SensorData msg;
+	sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true);
+
+	EXPECT_EQ(msg.laser_scan_max_pts, 100);
+	EXPECT_FLOAT_EQ(msg.laser_scan_max_range, 40.0f);
+	EXPECT_EQ(msg.laser_scan_format, (int)rtabmap::LaserScan::kXYZ);
+	expectTransformNear(transformFromGeometryMsg(msg.laser_scan_local_transform),
+			localTransform, 1e-4f);
+
+	const rtabmap::SensorData out = sensorDataFromROS(msg);
+	const rtabmap::LaserScan & outScan = out.laserScanRaw().empty()
+			? out.laserScanCompressed() : out.laserScanRaw();
+	EXPECT_EQ(outScan.size(), scan.size());
+	EXPECT_EQ(outScan.maxPoints(), scan.maxPoints());
+	EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax());
+	expectTransformNear(outScan.localTransform(), localTransform, 1e-4f);
+}
+
+/**
+ * A raw scan survives the round trip through a SensorData message in every format: it
+ * goes out as a cloud with one field per channel, and comes back in laser_scan_format
+ * with the same values. A 2D scan in particular goes out as an x/y/z cloud with z at 0,
+ * which alone cannot tell it was 2D -- the odometry nodes' odom_sensor_data/raw does this
+ * for a 2D lidar -- and must come back 2D rather than as a 3D scan that fails the format
+ * check.
+ */
+TEST(MsgConversion, sensorDataLaserScanRoundTripEveryFormat)
+{
+	for(int f = rtabmap::LaserScan::kXY; f <= rtabmap::LaserScan::kXYZIRT; ++f)
+	{
+		const rtabmap::LaserScan::Format format = (rtabmap::LaserScan::Format)f;
+		SCOPED_TRACE(rtabmap::LaserScan::formatName(format));
+		const int channels = rtabmap::LaserScan::channels(format);
+		ASSERT_GT(channels, 0);
+
+		// Small integers in every channel: exact through float and through the integer
+		// fields some formats use, like the ring.
+		cv::Mat points(1, 3, CV_32FC(channels));
+		for(int i = 0; i < points.cols; ++i)
+		{
+			float * p = points.ptr<float>(0, i);
+			for(int c = 0; c < channels; ++c)
+			{
+				p[c] = float(1 + i + c);
+			}
+		}
+		const rtabmap::Transform localTransform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+		const rtabmap::LaserScan scan(points, /*maxPoints=*/360, /*maxRange=*/10.0f, format, localTransform);
+		if(scan.hasRGB())
+		{
+			// A packed 0x00RRGGBB color, as PCL stores it.
+			for(int i = 0; i < points.cols; ++i)
+			{
+				const uint32_t rgb = 0x00102030u + i;
+				memcpy(points.ptr<float>(0, i) + scan.getRGBOffset(), &rgb, sizeof(float));
+			}
+		}
+
+		rtabmap::SensorData in;
+		in.setStamp(1000.0);
+		in.setLaserScan(scan);
+
+		rtabmap_msgs::msg::SensorData msg;
+		sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true);
+		ASSERT_FALSE(msg.laser_scan.data.empty());
+		EXPECT_EQ(msg.laser_scan_format, (int)format);
+
+		rtabmap::SensorData out;
+		bool converted = false;
+		EXPECT_NO_THROW({ out = sensorDataFromROS(msg); converted = true; });
+		if(!converted)
+		{
+			continue; // reported above; carry on so every failing format is listed
+		}
+		const rtabmap::LaserScan & outScan = out.laserScanRaw();
+		ASSERT_FALSE(outScan.isEmpty());
+		EXPECT_EQ(outScan.format(), format);
+		EXPECT_EQ(outScan.is2d(), scan.is2d());
+		EXPECT_EQ(outScan.maxPoints(), scan.maxPoints());
+		EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax());
+		expectTransformNear(outScan.localTransform(), localTransform, 1e-4f);
+		ASSERT_EQ(outScan.data().size(), scan.data().size());
+		ASSERT_EQ(outScan.data().type(), scan.data().type());
+		EXPECT_EQ(0, memcmp(outScan.data().data, scan.data().data,
+				scan.data().total() * scan.data().elemSize())) << "the values changed";
+	}
+}
+
+TEST(MsgConversion, sensorDataStereoModelRoundTrip)
+{
+	const double fx = 525.0;
+	const double baseline = 0.12;
+	const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
+
+	const rtabmap::StereoCameraModel stereo(
+			fx, fx, 320.0, 240.0, baseline, localTransform, cv::Size(640, 480));
+	ASSERT_TRUE(stereo.isValidForProjection()) << "precondition";
+
+	rtabmap::SensorData in;
+	in.setStamp(1000.0);
+	in.setStereoImage(cv::Mat(), cv::Mat(), stereo);
+
+	rtabmap_msgs::msg::SensorData msg;
+	sensorDataToROS(in, msg, "base_link");
+
+	// The stereo branch fills BOTH camera infos, unlike the monocular one.
+	ASSERT_EQ(msg.left_camera_info.size(), 1u);
+	ASSERT_EQ(msg.right_camera_info.size(), 1u);
+
+	const rtabmap::SensorData out = sensorDataFromROS(msg);
+	ASSERT_EQ(out.stereoCameraModels().size(), 1u);
+	EXPECT_TRUE(out.cameraModels().empty()) << "must not be read back as monocular";
+	EXPECT_NEAR(out.stereoCameraModels()[0].left().fx(), fx, 1e-9);
+	EXPECT_NEAR(out.stereoCameraModels()[0].baseline(), baseline, 1e-6);
+	expectTransformNear(out.stereoCameraModels()[0].localTransform(), localTransform, 1e-4f);
+}
+
+TEST(MsgConversion, nodeWithStereoModelRoundTrip)
+{
+	const rtabmap::StereoCameraModel stereo(
+			525.0, 525.0, 320.0, 240.0, 0.12,
+			rtabmap::Transform::getIdentity(), cv::Size(640, 480));
+
+	rtabmap::Signature in(3, 0, 1, 1000.0, "stereo_node", sampleTransform());
+	in.sensorData().setStereoImage(cv::Mat(), cv::Mat(), stereo);
+
+	rtabmap_msgs::msg::Node msg;
+	nodeToROS(in, msg);
+	const rtabmap::Signature out = nodeFromROS(msg);
+
+	EXPECT_EQ(out.id(), in.id());
+	ASSERT_EQ(out.sensorData().stereoCameraModels().size(), 1u);
+	EXPECT_NEAR(out.sensorData().stereoCameraModels()[0].baseline(), 0.12, 1e-6);
+}
+
+TEST(MsgConversion, infoToROSKeepsACallerSuppliedStamp)
+{
+	// CoreWrapper stamps the message before calling infoToROS, sometimes with a
+	// publication time unrelated to the data. That must not be overwritten.
+	rtabmap::Statistics in;
+	in.setExtended(true);
+	in.setStamp(1234.5);
+
+	rtabmap_msgs::msg::Info msg;
+	msg.header.stamp = timestampToROS(9999.0);
+	msg.header.frame_id = "map";
+	infoToROS(in, msg);
+
+	EXPECT_NEAR(timestampFromROS(msg.header.stamp), 9999.0, 1e-6)
+		<< "a caller-supplied stamp must win over the statistics stamp";
+	EXPECT_EQ(msg.header.frame_id, "map");
+}
+
+TEST(MsgConversion, infoOdomCacheRoundTrip)
+{
+	// Statistics carries a whole MapGraph for the odometry cache in localization mode.
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, sampleTransform()));
+	poses.insert(std::make_pair(2, rtabmap::Transform(1, 2, 3, 0, 0, 0)));
+
+	std::multimap<int, rtabmap::Link> links;
+	links.insert(std::make_pair(1, rtabmap::Link(
+			1, 2, rtabmap::Link::kNeighbor, sampleTransform())));
+
+	rtabmap::Statistics in;
+	in.setExtended(true);
+	in.setOdomCachePoses(poses);
+	in.setOdomCacheConstraints(links);
+
+	rtabmap_msgs::msg::Info msg;
+	infoToROS(in, msg);
+	ASSERT_EQ(msg.odom_cache.poses.size(), poses.size());
+	ASSERT_EQ(msg.odom_cache.links.size(), links.size());
+
+	rtabmap::Statistics out;
+	infoFromROS(msg, out);
+
+	ASSERT_EQ(out.odomCachePoses().size(), poses.size());
+	expectTransformNear(out.odomCachePoses().at(1), poses.at(1));
+	expectTransformNear(out.odomCachePoses().at(2), poses.at(2));
+	EXPECT_EQ(out.odomCacheConstraints().size(), links.size());
+}
+
+/////////////////////////
+// TF-based conversions
+/////////////////////////
+
+namespace {
+
+/// A tf2 buffer needs a clock, but neither a node nor a listener: transforms can be
+/// injected directly, which makes every TF-based conversion an ordinary unit test.
+std::shared_ptr<tf2_ros::Buffer> makeTfBuffer()
+{
+	std::shared_ptr<tf2_ros::Buffer> buffer =
+			std::make_shared<tf2_ros::Buffer>(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
+	// Transforms are injected synchronously before the lookups, so tell tf2 not to warn
+	// about waiting for a listener thread that will never exist.
+	buffer->setUsingDedicatedThread(true);
+	return buffer;
+}
+
+void addTf(tf2_ros::Buffer & buffer,
+		const std::string & parent, const std::string & child,
+		const rtabmap::Transform & t, double stamp, bool isStatic = true)
+{
+	geometry_msgs::msg::TransformStamped msg;
+	msg.header.stamp = timestampToROS(stamp);
+	msg.header.frame_id = parent;
+	msg.child_frame_id = child;
+	transformToGeometryMsg(t, msg.transform);
+	ASSERT_TRUE(buffer.setTransform(msg, "unit_test", isStatic));
+}
+
+}  // namespace
+
+TEST(MsgConversion, getTransformReadsTheBuffer)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+
+	const rtabmap::Transform out =
+			getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0);
+
+	expectTransformNear(out, baseToCamera);
+}
+
+TEST(MsgConversion, getTransformReturnsNullWhenUnknown)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	// An unrelated frame must not throw; it must come back as a null transform.
+	EXPECT_TRUE(getTransform("base_link", "lidar_link", timestampToROS(1000.0), *buffer, 0.0)
+			.isNull());
+}
+
+TEST(MsgConversion, getTransformIsInvertedByFrameOrder)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.5f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+
+	const rtabmap::Transform forward =
+			getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0);
+	const rtabmap::Transform backward =
+			getTransform("camera_link", "base_link", timestampToROS(1000.0), *buffer, 0.0);
+
+	expectTransformNear(backward, forward.inverse(), 1e-4f);
+}
+
+TEST(MsgConversion, getMovingTransformMeasuresMotionBetweenStamps)
+{
+	// base_link drives 1 m along x of odom between t=1000 and t=1001.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
+
+	// Motion of base_link from t=1000 to t=1001, seen in the fixed odom frame.
+	const rtabmap::Transform motion = getMovingTransform(
+			"base_link", "odom", timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0);
+
+	ASSERT_FALSE(motion.isNull());
+	EXPECT_NEAR(motion.x(), 1.0, 1e-4);
+	EXPECT_NEAR(motion.y(), 0.0, 1e-4);
+	EXPECT_NEAR(motion.z(), 0.0, 1e-4);
+}
+
+TEST(MsgConversion, getMovingTransformInterpolatesBetweenStamps)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
+
+	// Halfway through, so half the motion.
+	const rtabmap::Transform half = getMovingTransform(
+			"base_link", "odom", timestampToROS(1000.0), timestampToROS(1000.5), *buffer, 0.0);
+
+	ASSERT_FALSE(half.isNull());
+	EXPECT_NEAR(half.x(), 0.5, 1e-4);
+}
+
+TEST(MsgConversion, getMovingTransformIsNullWithoutAFixedFrame)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform::getIdentity(), 1000.0, false);
+
+	EXPECT_TRUE(getMovingTransform("base_link", "map",
+			timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0).isNull());
+}
+
+TEST(MsgConversion, convertScanMsgProducesALaserScan)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0);
+
+	sensor_msgs::msg::LaserScan msg;
+	msg.header.stamp = timestampToROS(1000.0);
+	msg.header.frame_id = "laser";
+	msg.angle_min = -1.0f;
+	msg.angle_max = 1.0f;
+	msg.angle_increment = 0.1f;
+	msg.time_increment = 0.0f;
+	msg.range_min = 0.1f;
+	msg.range_max = 30.0f;
+	msg.ranges.assign(21, 5.0f);
+
+	rtabmap::LaserScan scan;
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+
+	EXPECT_FALSE(scan.empty());
+	EXPECT_EQ(scan.size(), (int)msg.ranges.size());
+	expectTransformNear(scan.localTransform(), baseToLaser, 1e-4f);
+}
+
+TEST(MsgConversion, convertScanMsgRejectsMalformedScans)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0);
+
+	sensor_msgs::msg::LaserScan base;
+	base.header.stamp = timestampToROS(1000.0);
+	base.header.frame_id = "laser";
+	base.angle_min = -1.0f;
+	base.angle_max = 1.0f;
+	base.angle_increment = 0.1f;
+	base.range_min = 0.1f;
+	base.range_max = 30.0f;
+	base.ranges.assign(21, 5.0f);
+
+	rtabmap::LaserScan scan;
+
+	sensor_msgs::msg::LaserScan zeroIncrement = base;
+	zeroIncrement.angle_increment = 0.0f;
+	EXPECT_FALSE(convertScanMsg(zeroIncrement, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0)) << "angle_increment of 0 would divide by zero";
+
+	sensor_msgs::msg::LaserScan invertedRange = base;
+	invertedRange.range_min = 40.0f;
+	EXPECT_FALSE(convertScanMsg(invertedRange, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0)) << "range_min > range_max";
+
+	sensor_msgs::msg::LaserScan invertedAngle = base;
+	invertedAngle.angle_min = 1.0f;
+	invertedAngle.angle_max = -1.0f;
+	EXPECT_FALSE(convertScanMsg(invertedAngle, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0)) << "positive increment with angle_max < angle_min";
+}
+
+TEST(MsgConversion, convertScanMsgFailsWithoutTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();   // empty
+
+	sensor_msgs::msg::LaserScan msg;
+	msg.header.stamp = timestampToROS(1000.0);
+	msg.header.frame_id = "laser";
+	msg.angle_min = -1.0f;
+	msg.angle_max = 1.0f;
+	msg.angle_increment = 0.1f;
+	msg.range_min = 0.1f;
+	msg.range_max = 30.0f;
+	msg.ranges.assign(21, 5.0f);
+
+	rtabmap::LaserScan scan;
+	EXPECT_FALSE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+}
+
+TEST(MsgConversion, convertScan3dMsgKeepsLocalTransformAndLimits)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToLidar(0.0f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "lidar", baseToLidar, 1000.0);
+
+	sensor_msgs::msg::PointCloud2 msg =
+			makeXYZCloud({{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {3.0f, 0.0f, 0.0f}});
+	msg.header.stamp = timestampToROS(1000.0);
+	msg.header.frame_id = "lidar";
+
+	rtabmap::LaserScan scan;
+	ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+
+	EXPECT_EQ(scan.size(), 3);
+	expectTransformNear(scan.localTransform(), baseToLidar, 1e-4f);
+	EXPECT_EQ(scan.rangeMax(), 0.0f) << "no max range requested";
+
+	rtabmap::LaserScan limited;
+	ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
+			limited, *buffer, 0.0, /*maxPoints=*/10, /*maxRange=*/2.5f));
+	EXPECT_EQ(limited.maxPoints(), 10);
+	EXPECT_FLOAT_EQ(limited.rangeMax(), 2.5f);
+}
+
+TEST(MsgConversion, convertScan3dMsgFailsWithoutTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();   // empty
+
+	sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
+	msg.header.stamp = timestampToROS(1000.0);
+	msg.header.frame_id = "lidar";
+
+	rtabmap::LaserScan scan;
+	EXPECT_FALSE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+}
+
+TEST(MsgConversion, landmarksFromROSAppliesTfAndDefaultVariance)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+
+	geometry_msgs::msg::PoseWithCovarianceStamped tag;
+	tag.header.stamp = timestampToROS(1000.0);
+	tag.header.frame_id = "camera_link";
+	tag.pose.pose.position.x = 2.0;      // 2 m in front of the camera
+	tag.pose.pose.orientation.w = 1.0;
+	// covariance left at zero -> the defaults must be substituted
+
+	std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
+	tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f)));
+
+	const rtabmap::Landmarks landmarks = landmarksFromROS(
+			tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0,
+			/*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02);
+
+	ASSERT_EQ(landmarks.size(), 1u);
+	ASSERT_TRUE(landmarks.find(7) != landmarks.end());
+
+	// The tag pose must come back in base_link: 0.5 (base->camera) + 2.0 (camera->tag).
+	EXPECT_NEAR(landmarks.at(7).pose().x(), 2.5, 1e-4);
+
+	const cv::Mat cov = landmarks.at(7).covariance();
+	ASSERT_EQ(cov.rows, 6);
+	EXPECT_NEAR(cov.at<double>(0,0), 0.01, 1e-9) << "linear default";
+	EXPECT_NEAR(cov.at<double>(3,3), 0.02, 1e-9) << "angular default";
+}
+
+TEST(MsgConversion, landmarksFromROSCorrectsForOdometryMotion)
+{
+	// The tag is seen 1 s after the odometry stamp, during which the robot drives 1 m.
+	// landmarksFromROS must fold that motion in, otherwise the landmark is placed where
+	// the robot would have seen it had it not moved.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false);
+
+	geometry_msgs::msg::PoseWithCovarianceStamped tag;
+	tag.header.stamp = timestampToROS(1001.0);     // observed at t=1001
+	tag.header.frame_id = "camera_link";
+	tag.pose.pose.position.x = 2.0;
+	tag.pose.pose.orientation.w = 1.0;
+
+	std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
+	tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f)));
+
+	// odomStamp is 1000, one second BEFORE the observation.
+	const rtabmap::Landmarks corrected = landmarksFromROS(
+			tags, "base_link", "odom", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
+
+	ASSERT_EQ(corrected.size(), 1u);
+	// 0.5 (base->camera) + 2.0 (camera->tag) + 1.0 (odometry motion since odomStamp).
+	EXPECT_NEAR(corrected.at(7).pose().x(), 3.5, 1e-3);
+
+	// Without an odom frame the correction cannot be looked up, and the landmark stays
+	// in the frame at the observation stamp.
+	const rtabmap::Landmarks uncorrected = landmarksFromROS(
+			tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
+	ASSERT_EQ(uncorrected.size(), 1u);
+	EXPECT_NEAR(uncorrected.at(7).pose().x(), 2.5, 1e-3);
+}
+
+TEST(MsgConversion, landmarksFromROSKeepsProvidedCovariance)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	geometry_msgs::msg::PoseWithCovarianceStamped tag;
+	tag.header.stamp = timestampToROS(1000.0);
+	tag.header.frame_id = "camera_link";
+	tag.pose.pose.position.x = 1.0;
+	tag.pose.pose.orientation.w = 1.0;
+	for(size_t i=0; i<6; ++i)
+	{
+		tag.pose.covariance[i*6 + i] = 0.5;   // a real, finite covariance
+	}
+
+	std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
+	tags.insert(std::make_pair(1, std::make_pair(tag, 0.1f)));
+
+	const rtabmap::Landmarks landmarks = landmarksFromROS(
+			tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0,
+			/*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02);
+
+	ASSERT_EQ(landmarks.size(), 1u);
+	EXPECT_NEAR(landmarks.at(1).covariance().at<double>(0,0), 0.5, 1e-9)
+		<< "a provided covariance must not be replaced by the default";
+	EXPECT_NEAR(landmarks.at(1).covariance().at<double>(3,3), 0.5, 1e-9);
+}
+
+TEST(MsgConversion, landmarksFromROSRejectsNonPositiveIds)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	geometry_msgs::msg::PoseWithCovarianceStamped tag;
+	tag.header.stamp = timestampToROS(1000.0);
+	tag.header.frame_id = "camera_link";
+	tag.pose.pose.orientation.w = 1.0;
+
+	std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > tags;
+	tags.insert(std::make_pair(0, std::make_pair(tag, 0.1f)));
+	tags.insert(std::make_pair(-3, std::make_pair(tag, 0.1f)));
+	tags.insert(std::make_pair(5, std::make_pair(tag, 0.1f)));
+
+	const rtabmap::Landmarks landmarks = landmarksFromROS(
+			tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02);
+
+	EXPECT_EQ(landmarks.size(), 1u) << "ids <= 0 must be dropped";
+	EXPECT_TRUE(landmarks.find(5) != landmarks.end());
+}
+
+void expectTfDeskewRecoversWall(bool slerp)
+{
+	SCOPED_TRACE(slerp ? "slerp=true" : "slerp=false");
+
+	// base_link advances 0.1 m along odom over the sweep -- the same motion the constant
+	// velocity tests apply at 1 m/s. With slerp the correction is interpolated between
+	// the two end poses; without it, every sample gets its own TF lookup.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false);
+	addTf(*buffer, "odom", "base_link",
+			rtabmap::Transform(float(kSpeed * kScanSpan), 0, 0, 0, 0, 0),
+			1000.0 + kScanSpan, false);
+
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
+	in.header.frame_id = "base_link";
+
+	sensor_msgs::msg::PointCloud2 out;
+	ASSERT_TRUE(deskew(in, out, "odom", *buffer, 0.0, slerp));
+
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readWallX(out, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
+			<< "sample " << i;
+	}
+}
+
+TEST(MsgConversion, deskewWithTfBufferSlerp)
+{
+	expectTfDeskewRecoversWall(/*slerp=*/true);
+}
+
+TEST(MsgConversion, deskewWithTfBufferPerPointLookup)
+{
+	// slerp=false takes a completely different path: a getMovingTransform() per sample
+	// instead of one interpolation between the sweep's end poses.
+	expectTfDeskewRecoversWall(/*slerp=*/false);
+}
+
+TEST(MsgConversion, deskewSlerpLinearizesNonLinearMotion)
+{
+	// A piecewise-linear trajectory: the robot covers most of the sweep's distance in the
+	// first half, then nearly stops. Both the TF buffer and the skew of the input cloud
+	// are generated from this same motion, so the true answer is unambiguous: a correct
+	// deskew must recover the flat wall.
+	const double kneeTime = kScanSpan / 2.0;
+	const double kneeX = 0.09;                        // vs 0.0495 if it were linear
+	const double endX = kSpeed * kScanSpan;           // 0.099
+	// Matches how tf2 interpolates between consecutive samples.
+	auto travelled = [&](double elapsed) {
+		return elapsed <= kneeTime
+				? kneeX * (elapsed / kneeTime)
+				: kneeX + (endX - kneeX) * ((elapsed - kneeTime) / (kScanSpan - kneeTime));
+	};
+
+	auto buildBuffer = [&]() {
+		std::shared_ptr<tf2_ros::Buffer> b = makeTfBuffer();
+		geometry_msgs::msg::TransformStamped m;
+		m.header.frame_id = "odom";
+		m.child_frame_id = "base_link";
+		m.transform.rotation.w = 1.0;
+		for(double elapsed : {0.0, kneeTime, kScanSpan})
+		{
+			m.header.stamp = timestampToROS(1000.0 + elapsed);
+			m.transform.translation.x = travelled(elapsed);
+			b->setTransform(m, "unit_test", false);
+		}
+		return b;
+	};
+
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
+			1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t", false, travelled);
+	in.header.frame_id = "base_link";
+
+	sensor_msgs::msg::PointCloud2 slerped, perPoint;
+	const std::shared_ptr<tf2_ros::Buffer> b1 = buildBuffer();
+	const std::shared_ptr<tf2_ros::Buffer> b2 = buildBuffer();
+	ASSERT_TRUE(deskew(in, slerped, "odom", *b1, 0.0, /*slerp=*/true));
+	ASSERT_TRUE(deskew(in, perPoint, "odom", *b2, 0.0, /*slerp=*/false));
+
+	// Per-point lookups follow the real motion, so they reconstruct the wall exactly.
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		EXPECT_NEAR(readWallX(perPoint, i, 0, kTimeOnColumns), kWallDistance, 1e-3)
+			<< "slerp=false must be exact, sample " << i;
+	}
+
+	// slerp only reads the sweep's two end poses, so it straight-lines through the knee
+	// and leaves a visible residual in the middle of the scan.
+	double worst = 0.0;
+	for(size_t i=0; i<kScanPoints; ++i)
+	{
+		worst = std::max(worst,
+				std::abs(double(readWallX(slerped, i, 0, kTimeOnColumns)) - kWallDistance));
+	}
+	EXPECT_GT(worst, 1e-2) << "slerp must show the error of linearizing the motion";
+}
+
+TEST(MsgConversion, deskewWithTfBufferFailsWithoutTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();   // empty
+
+	sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0);
+	in.header.frame_id = "base_link";
+
+	sensor_msgs::msg::PointCloud2 out;
+	EXPECT_FALSE(deskew(in, out, "odom", *buffer, 0.0, true));
+}
+
+/////////////////////////
+// convertRGBDMsgs / convertStereoMsg
+/////////////////////////
+
+namespace {
+
+/// A rectified pinhole CameraInfo. tx is P(0,3): 0 for the left/depth camera, and
+/// -fx*baseline for the right camera of a stereo pair.
+sensor_msgs::msg::CameraInfo makeCameraInfo(
+		const std::string & frameId, double stamp, int width, int height,
+		double tx = 0.0, double fx = 100.0)
+{
+	sensor_msgs::msg::CameraInfo info;
+	info.header.stamp = timestampToROS(stamp);
+	info.header.frame_id = frameId;
+	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;
+}
+
+cv_bridge::CvImageConstPtr makeImage(
+		const std::string & frameId, double stamp,
+		const cv::Mat & image, const std::string & encoding)
+{
+	std_msgs::msg::Header header;
+	header.stamp = timestampToROS(stamp);
+	header.frame_id = frameId;
+	return std::make_shared<cv_bridge::CvImage>(header, encoding, image);
+}
+
+}  // namespace
+
+TEST(MsgConversion, convertRGBDMsgsSingleCamera)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
+
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths =
+			{makeImage("camera_link", 1000.0, depthImage, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1000.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, /*alreadyRectifiedImages=*/true));
+
+	EXPECT_TRUE(stereoModels.empty()) << "a depth image must not produce a stereo model";
+	ASSERT_EQ(models.size(), 1u);
+	EXPECT_NEAR(models[0].fx(), 100.0, 1e-9);
+	expectTransformNear(models[0].localTransform(), baseToCamera, 1e-4f);
+
+	ASSERT_EQ(rgb.cols, 8);
+	ASSERT_EQ(rgb.rows, 8);
+	EXPECT_EQ(depth.type(), CV_16UC1);
+	EXPECT_EQ(depth.at<unsigned short>(0, 0), 1500);
+}
+
+TEST(MsgConversion, convertRGBDMsgsMultiCameraSideBySide)
+{
+	// Two cameras are concatenated horizontally into one wide image, one model each.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
+	addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
+
+	const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0));
+	const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0));
+	const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000));
+	const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000));
+
+	const std::vector<cv_bridge::CvImageConstPtr> images = {
+			makeImage("cam0", 1000.0, rgb0, "bgr8"),
+			makeImage("cam1", 1000.0, rgb1, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths = {
+			makeImage("cam0", 1000.0, depth0, "16UC1"),
+			makeImage("cam1", 1000.0, depth1, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos = {
+			makeCameraInfo("cam0", 1000.0, 8, 8),
+			makeCameraInfo("cam1", 1000.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, true));
+
+	ASSERT_EQ(models.size(), 2u);
+	EXPECT_EQ(rgb.cols, 16) << "the two 8-wide images must be side by side";
+	EXPECT_EQ(rgb.rows, 8);
+	EXPECT_EQ(depth.cols, 16);
+
+	// Each half keeps its own camera's data.
+	EXPECT_EQ(depth.at<unsigned short>(0, 0), 1000);
+	EXPECT_EQ(depth.at<unsigned short>(0, 8), 2000);
+	EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-4);
+	EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-4);
+}
+
+namespace {
+
+/// base_link sits at odom origin at t=1000 and 1 m along x at t=1001.
+void addOdomMotion(tf2_ros::Buffer & buffer)
+{
+	geometry_msgs::msg::TransformStamped m;
+	m.header.frame_id = "odom";
+	m.child_frame_id = "base_link";
+	m.transform.rotation.w = 1.0;
+	m.header.stamp = timestampToROS(1000.0);
+	m.transform.translation.x = 0.0;
+	ASSERT_TRUE(buffer.setTransform(m, "unit_test", false));
+	m.header.stamp = timestampToROS(1001.0);
+	m.transform.translation.x = 1.0;
+	ASSERT_TRUE(buffer.setTransform(m, "unit_test", false));
+}
+
+}  // namespace
+
+TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp)
+{
+	// The image is captured at t=1001 but must be expressed relative to the base frame
+	// at odomStamp=1000, one meter back.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0);
+	addOdomMotion(*buffer);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1001.0, rgbImage, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths =
+			{makeImage("camera_link", 1001.0, depthImage, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1001.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> corrected;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
+			timestampToROS(1000.0), rgb, depth, corrected, stereoModels, *buffer, 0.0, true));
+	ASSERT_EQ(corrected.size(), 1u);
+	EXPECT_NEAR(corrected[0].localTransform().x(), 1.1, 1e-3)
+		<< "0.1 base->camera plus 1.0 of odometry motion";
+
+	// Without an odom frame the motion is not folded in.
+	std::vector<rtabmap::CameraModel> uncorrected;
+	std::vector<rtabmap::StereoCameraModel> stereoModels2;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, uncorrected, stereoModels2, *buffer, 0.0, true));
+	ASSERT_EQ(uncorrected.size(), 1u);
+	EXPECT_NEAR(uncorrected[0].localTransform().x(), 0.1, 1e-3);
+}
+
+TEST(MsgConversion, convertRGBDMsgsSyncsEachCameraAtItsOwnStamp)
+{
+	// Two cameras captured 1 s apart, on a robot moving 1 m/s along x. Each must be
+	// corrected by its OWN elapsed motion, not by a single shared one.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
+	addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
+	addOdomMotion(*buffer);                                        // x=0 @1000, x=1 @1001
+	geometry_msgs::msg::TransformStamped m;                        // extend to x=2 @1002
+	m.header.frame_id = "odom";
+	m.child_frame_id = "base_link";
+	m.transform.rotation.w = 1.0;
+	m.header.stamp = timestampToROS(1002.0);
+	m.transform.translation.x = 2.0;
+	ASSERT_TRUE(buffer->setTransform(m, "unit_test", false));
+
+	const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0));
+	const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0));
+	const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000));
+	const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000));
+
+	const std::vector<cv_bridge::CvImageConstPtr> images = {
+			makeImage("cam0", 1001.0, rgb0, "bgr8"),
+			makeImage("cam1", 1002.0, rgb1, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths = {
+			makeImage("cam0", 1001.0, depth0, "16UC1"),
+			makeImage("cam1", 1002.0, depth1, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos = {
+			makeCameraInfo("cam0", 1001.0, 8, 8),
+			makeCameraInfo("cam1", 1002.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
+
+	ASSERT_EQ(models.size(), 2u);
+	// cam0 is 1 s after odomStamp, cam1 is 2 s after.
+	EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) << "0.1 + 1.0 of motion";
+	EXPECT_NEAR(models[1].localTransform().x(), 2.1, 1e-3) << "0.1 + 2.0 of motion";
+	// The corrections must differ, which is the whole point of per-camera stamps.
+	EXPECT_GT(models[1].localTransform().x() - models[0].localTransform().x(), 0.5);
+	// The lateral offsets are untouched by a purely forward motion.
+	EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-3);
+	EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-3);
+}
+
+TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer)
+{
+	// The RGB and depth stamps of a camera are assumed to be equal. This pins the
+	// tie-break for when they are not: the depth stamp prevails, since it is the one the
+	// geometry is synchronized to. Not a behavior to rely on -- a camera whose two
+	// stamps disagree is already outside the contract.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0);
+	addOdomMotion(*buffer);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
+
+	// RGB stamped at odomStamp (no motion), depth stamped 1 s later (1 m of motion).
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths =
+			{makeImage("camera_link", 1001.0, depthImage, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1000.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
+
+	ASSERT_EQ(models.size(), 1u);
+	EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3)
+		<< "the depth stamp (1001) prevails over the rgb stamp (1000)";
+}
+
+TEST(MsgConversion, convertRGBDMsgsMultiStereoBuildsOneModelPerPair)
+{
+	// mono8 "right" images make convertRGBDMsgs take the stereo branch and produce
+	// StereoCameraModels instead of CameraModels. The odometry sync is not re-tested
+	// here: it happens in the shared loop before the depth/stereo split, so
+	// convertRGBDMsgsSyncsEachCameraAtItsOwnStamp already covers it for both.
+	const double fx = 100.0;
+	const double baseline0 = 0.15;
+	const double baseline1 = 0.20;
+
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "left0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0);
+	addTf(*buffer, "base_link", "left1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0);
+
+	const cv::Mat left0(8, 8, CV_8UC1, cv::Scalar(40));
+	const cv::Mat left1(8, 8, CV_8UC1, cv::Scalar(60));
+	const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(50));
+
+	const std::vector<cv_bridge::CvImageConstPtr> images = {
+			makeImage("left0", 1000.0, left0, "mono8"),
+			makeImage("left1", 1000.0, left1, "mono8")};
+	const std::vector<cv_bridge::CvImageConstPtr> rights = {
+			makeImage("right0", 1000.0, right, "mono8"),
+			makeImage("right1", 1000.0, right, "mono8")};
+	const std::vector<sensor_msgs::msg::CameraInfo> leftInfos = {
+			makeCameraInfo("left0", 1000.0, 8, 8, 0.0, fx),
+			makeCameraInfo("left1", 1000.0, 8, 8, 0.0, fx)};
+	const std::vector<sensor_msgs::msg::CameraInfo> rightInfos = {
+			makeCameraInfo("right0", 1000.0, 8, 8, -fx*baseline0, fx),
+			makeCameraInfo("right1", 1000.0, 8, 8, -fx*baseline1, fx)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, rights, leftInfos, rightInfos, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true));
+
+	EXPECT_TRUE(models.empty()) << "mono8 right images must give stereo models";
+	ASSERT_EQ(stereoModels.size(), 2u);
+
+	// Each pair keeps its own baseline and its own local transform.
+	EXPECT_NEAR(stereoModels[0].baseline(), baseline0, 1e-6);
+	EXPECT_NEAR(stereoModels[1].baseline(), baseline1, 1e-6);
+	EXPECT_NEAR(stereoModels[0].localTransform().y(), 0.1, 1e-3);
+	EXPECT_NEAR(stereoModels[1].localTransform().y(), -0.1, 1e-3);
+
+	// The two left images are laid out side by side, as in the RGB-D case.
+	EXPECT_EQ(rgb.cols, 16);
+	EXPECT_EQ(depth.cols, 16);
+}
+
+TEST(MsgConversion, convertRGBDMsgsSurvivesAFailedOdomLookup)
+{
+	// A missing odom frame must only warn: the data is still converted, uncorrected.
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1001.0);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1001.0, rgbImage, "bgr8")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1001.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "odom",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true))
+		<< "a failed odometry correction must not be fatal";
+	ASSERT_EQ(models.size(), 1u);
+	EXPECT_NEAR(models[0].localTransform().x(), 0.1, 1e-3) << "left uncorrected";
+}
+
+TEST(MsgConversion, convertStereoMsgSyncsToOdomStamp)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "left_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1000.0);
+	addOdomMotion(*buffer);
+
+	const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(40));
+
+	cv::Mat left, right;
+	rtabmap::StereoCameraModel model;
+	ASSERT_TRUE(convertStereoMsg(
+			makeImage("left_link", 1001.0, mono, "mono8"),
+			makeImage("right_link", 1001.0, mono, "mono8"),
+			makeCameraInfo("left_link", 1001.0, 8, 8, 0.0),
+			makeCameraInfo("right_link", 1001.0, 8, 8, -15.0),
+			"base_link", "odom", timestampToROS(1000.0),
+			left, right, model, *buffer, 0.0, true));
+
+	EXPECT_NEAR(model.localTransform().x(), 1.1, 1e-3);
+}
+
+TEST(MsgConversion, convertScan3dMsgSyncsToOdomStamp)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "lidar", rtabmap::Transform(0.0f, 0, 0.5f, 0, 0, 0), 1000.0);
+	addOdomMotion(*buffer);
+
+	sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}});
+	msg.header.stamp = timestampToROS(1001.0);
+	msg.header.frame_id = "lidar";
+
+	rtabmap::LaserScan scan;
+	ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "odom", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+
+	EXPECT_NEAR(scan.localTransform().x(), 1.0, 1e-3) << "0.0 base->lidar plus 1.0 motion";
+	EXPECT_NEAR(scan.localTransform().z(), 0.5, 1e-3);
+}
+
+TEST(MsgConversion, convertScanMsgSyncsToOdomStamp)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "laser", rtabmap::Transform(0.2f, 0, 0.1f, 0, 0, 0), 1000.0);
+	addOdomMotion(*buffer);
+
+	sensor_msgs::msg::LaserScan msg;
+	msg.header.stamp = timestampToROS(1001.0);
+	msg.header.frame_id = "laser";
+	msg.angle_min = -1.0f;
+	msg.angle_max = 1.0f;
+	msg.angle_increment = 0.1f;
+	msg.time_increment = 0.0f;
+	msg.range_min = 0.1f;
+	msg.range_max = 30.0f;
+	msg.ranges.assign(21, 5.0f);
+
+	rtabmap::LaserScan scan;
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0),
+			scan, *buffer, 0.0));
+
+	EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion";
+}
+
+namespace {
+
+/// 21 rays of 5 m over +-1 rad, swept in 0.5 s, from "laser" at @p stamp.
+sensor_msgs::msg::LaserScan makeSweep(double stamp)
+{
+	sensor_msgs::msg::LaserScan msg;
+	msg.header.stamp = timestampToROS(stamp);
+	msg.header.frame_id = "laser";
+	msg.angle_min = -1.0f;
+	msg.angle_max = 1.0f;
+	msg.angle_increment = 0.1f;
+	msg.time_increment = 0.5f / 20.0f;
+	msg.range_min = 0.1f;
+	msg.range_max = 30.0f;
+	msg.ranges.assign(21, 5.0f);
+	return msg;
+}
+
+}  // namespace
+
+/**
+ * With the odometry frame on TF, each ray is placed where the robot was when it was
+ * measured: at 1 m/s over a 0.5 s sweep, the last ray lands 0.5 m further than it would
+ * from the pose at the scan's stamp, the first one not at all.
+ */
+TEST(MsgConversion, convertScanMsgDeskewsWithOdometryTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0);
+	addOdomMotion(*buffer);
+	const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0);
+
+	rtabmap::LaserScan skewed, deskewed;
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), skewed, *buffer, 0.0));
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), deskewed, *buffer, 0.0));
+	ASSERT_EQ(skewed.size(), deskewed.size());
+	ASSERT_EQ(21, deskewed.size());
+
+	const float * first = deskewed.data().ptr<float>(0, 0);
+	const float * firstSkewed = skewed.data().ptr<float>(0, 0);
+	EXPECT_NEAR(firstSkewed[0], first[0], 1e-4);
+	EXPECT_NEAR(firstSkewed[1], first[1], 1e-4);
+	const float * last = deskewed.data().ptr<float>(0, 20);
+	const float * lastSkewed = skewed.data().ptr<float>(0, 20);
+	EXPECT_NEAR(lastSkewed[0] + 0.5f, last[0], 1e-3) << "moved by the robot's 0.5 m during the sweep";
+	EXPECT_NEAR(lastSkewed[1], last[1], 1e-3);
+}
+
+/**
+ * Without the odometry frame on TF -- odometry published as a topic only -- the scan is
+ * still converted, as it would be without an odometry frame: not deskewed, but not refused
+ * either.
+ */
+TEST(MsgConversion, convertScanMsgUsesTheScanAsItIsWithoutOdometryTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0);
+	const sensor_msgs::msg::LaserScan msg = makeSweep(1000.0);
+
+	rtabmap::LaserScan withoutOdom, withMissingOdom;
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), withoutOdom, *buffer, 0.0));
+	ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), withMissingOdom, *buffer, 0.0));
+
+	expectTransformNear(withMissingOdom.localTransform(), baseToLaser, 1e-4f);
+	ASSERT_EQ(withoutOdom.data().size(), withMissingOdom.data().size());
+	EXPECT_EQ(0.0, cv::norm(withoutOdom.data(), withMissingOdom.data(), cv::NORM_INF));
+}
+
+TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	// 32FC1 is a valid depth encoding but not a valid rgb/left one.
+	const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f));
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1000.0, bad, "32FC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1000.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, true));
+}
+
+TEST(MsgConversion, convertRGBDMsgsFailsWithoutTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();   // empty
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1000.0, 8, 8)};
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, true));
+}
+
+TEST(MsgConversion, convertRGBDMsgsCarriesLocalFeatures)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500));
+	const std::vector<cv_bridge::CvImageConstPtr> images =
+			{makeImage("camera_link", 1000.0, rgbImage, "bgr8")};
+	const std::vector<cv_bridge::CvImageConstPtr> depths =
+			{makeImage("camera_link", 1000.0, depthImage, "16UC1")};
+	const std::vector<sensor_msgs::msg::CameraInfo> infos =
+			{makeCameraInfo("camera_link", 1000.0, 8, 8)};
+
+	std::vector<rtabmap_msgs::msg::KeyPoint> kptMsgs(2);
+	kptMsgs[0].pt.x = 1.0f; kptMsgs[0].pt.y = 2.0f; kptMsgs[0].size = 7.0f;
+	kptMsgs[1].pt.x = 3.0f; kptMsgs[1].pt.y = 4.0f; kptMsgs[1].size = 7.0f;
+	std::vector<rtabmap_msgs::msg::Point3f> ptMsgs(2);
+	ptMsgs[0].x = 1.0f; ptMsgs[1].x = 2.0f;
+	cv::Mat descriptors = cv::Mat::ones(2, 4, CV_32FC1);
+
+	std::vector<cv::KeyPoint> outKpts;
+	std::vector<cv::Point3f> outPts;
+	cv::Mat outDescriptors;
+
+	cv::Mat rgb, depth;
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::StereoCameraModel> stereoModels;
+	ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "",
+			timestampToROS(1000.0), rgb, depth, models, stereoModels,
+			*buffer, 0.0, true,
+			{kptMsgs}, {ptMsgs}, {descriptors},
+			&outKpts, &outPts, &outDescriptors));
+
+	ASSERT_EQ(outKpts.size(), 2u);
+	EXPECT_FLOAT_EQ(outKpts[0].pt.x, 1.0f);
+	EXPECT_FLOAT_EQ(outKpts[1].pt.x, 3.0f);
+	ASSERT_EQ(outPts.size(), 2u);
+	EXPECT_FLOAT_EQ(outPts[1].x, 2.0f);
+	EXPECT_EQ(outDescriptors.rows, 2);
+}
+
+TEST(MsgConversion, convertStereoMsgProducesAStereoModel)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	const rtabmap::Transform baseToLeft(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f);
+	addTf(*buffer, "base_link", "left_link", baseToLeft, 1000.0);
+
+	const double fx = 100.0;
+	const double baseline = 0.15;
+	const cv::Mat leftImage(8, 8, CV_8UC1, cv::Scalar(40));
+	const cv::Mat rightImage(8, 8, CV_8UC1, cv::Scalar(50));
+
+	cv::Mat left, right;
+	rtabmap::StereoCameraModel model;
+	ASSERT_TRUE(convertStereoMsg(
+			makeImage("left_link", 1000.0, leftImage, "mono8"),
+			makeImage("right_link", 1000.0, rightImage, "mono8"),
+			makeCameraInfo("left_link", 1000.0, 8, 8, /*tx=*/0.0, fx),
+			makeCameraInfo("right_link", 1000.0, 8, 8, /*tx=*/-fx*baseline, fx),
+			"base_link", "", timestampToROS(1000.0),
+			left, right, model, *buffer, 0.0, /*alreadyRectified=*/true));
+
+	EXPECT_NEAR(model.baseline(), baseline, 1e-6);
+	EXPECT_NEAR(model.left().fx(), fx, 1e-9);
+	expectTransformNear(model.localTransform(), baseToLeft, 1e-4f);
+
+	ASSERT_EQ(left.type(), CV_8UC1);
+	ASSERT_EQ(right.type(), CV_8UC1);
+	EXPECT_EQ(left.at<unsigned char>(0, 0), 40);
+	EXPECT_EQ(right.at<unsigned char>(0, 0), 50);
+}
+
+TEST(MsgConversion, convertStereoMsgConvertsColorToMono)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	const cv::Mat color(8, 8, CV_8UC3, cv::Scalar(10, 20, 30));
+	const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
+
+	cv::Mat left, right;
+	rtabmap::StereoCameraModel model;
+	ASSERT_TRUE(convertStereoMsg(
+			makeImage("left_link", 1000.0, color, "bgr8"),
+			makeImage("right_link", 1000.0, mono, "mono8"),
+			makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
+			makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
+			"base_link", "", timestampToROS(1000.0),
+			left, right, model, *buffer, 0.0, true));
+
+	// The left image is kept in color; the right is always reduced to mono.
+	EXPECT_EQ(left.type(), CV_8UC3);
+	EXPECT_EQ(right.type(), CV_8UC1);
+}
+
+TEST(MsgConversion, convertStereoMsgRejectsBadEncoding)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();
+	addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0);
+
+	const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f));
+	const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
+
+	cv::Mat left, right;
+	rtabmap::StereoCameraModel model;
+	EXPECT_FALSE(convertStereoMsg(
+			makeImage("left_link", 1000.0, bad, "32FC1"),
+			makeImage("right_link", 1000.0, mono, "mono8"),
+			makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
+			makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
+			"base_link", "", timestampToROS(1000.0),
+			left, right, model, *buffer, 0.0, true));
+}
+
+TEST(MsgConversion, convertStereoMsgFailsWithoutTf)
+{
+	const std::shared_ptr<tf2_ros::Buffer> buffer = makeTfBuffer();   // empty
+	const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50));
+
+	cv::Mat left, right;
+	rtabmap::StereoCameraModel model;
+	EXPECT_FALSE(convertStereoMsg(
+			makeImage("left_link", 1000.0, mono, "mono8"),
+			makeImage("right_link", 1000.0, mono, "mono8"),
+			makeCameraInfo("left_link", 1000.0, 8, 8, 0.0),
+			makeCameraInfo("right_link", 1000.0, 8, 8, -15.0),
+			"base_link", "", timestampToROS(1000.0),
+			left, right, model, *buffer, 0.0, true));
+}
+
+/////////////////////////
+// IMU
+/////////////////////////
+
+TEST(MsgConversion, imuRoundTrip)
+{
+	sensor_msgs::msg::Imu in;
+	in.orientation.x = 0.0;
+	in.orientation.y = 0.0;
+	in.orientation.z = 0.0;
+	in.orientation.w = 1.0;
+	in.angular_velocity.x = 0.1;
+	in.angular_velocity.y = 0.2;
+	in.angular_velocity.z = 0.3;
+	in.linear_acceleration.x = 1.0;
+	in.linear_acceleration.y = 2.0;
+	in.linear_acceleration.z = 9.81;
+	for(size_t i=0; i<9; ++i)
+	{
+		in.orientation_covariance[i] = 0.01 * (i + 1);
+		in.angular_velocity_covariance[i] = 0.02 * (i + 1);
+		in.linear_acceleration_covariance[i] = 0.03 * (i + 1);
+	}
+
+	const rtabmap::IMU imu = imuFromROS(in, rtabmap::Transform::getIdentity());
+
+	sensor_msgs::msg::Imu out;
+	imuToROS(imu, out);
+
+	EXPECT_DOUBLE_EQ(out.orientation.w, in.orientation.w);
+	EXPECT_DOUBLE_EQ(out.angular_velocity.x, in.angular_velocity.x);
+	EXPECT_DOUBLE_EQ(out.angular_velocity.y, in.angular_velocity.y);
+	EXPECT_DOUBLE_EQ(out.angular_velocity.z, in.angular_velocity.z);
+	EXPECT_DOUBLE_EQ(out.linear_acceleration.x, in.linear_acceleration.x);
+	EXPECT_DOUBLE_EQ(out.linear_acceleration.y, in.linear_acceleration.y);
+	EXPECT_DOUBLE_EQ(out.linear_acceleration.z, in.linear_acceleration.z);
+	for(size_t i=0; i<9; ++i)
+	{
+		EXPECT_DOUBLE_EQ(out.orientation_covariance[i], in.orientation_covariance[i])
+			<< "orientation covariance at " << i;
+		EXPECT_DOUBLE_EQ(out.angular_velocity_covariance[i], in.angular_velocity_covariance[i])
+			<< "angular velocity covariance at " << i;
+		EXPECT_DOUBLE_EQ(out.linear_acceleration_covariance[i], in.linear_acceleration_covariance[i])
+			<< "linear acceleration covariance at " << i;
+	}
+}
diff --git a/rtabmap_costmap_plugins/CMakeLists.txt b/rtabmap_costmap_plugins/CMakeLists.txt
index d592a057..eab748d7 100644
--- a/rtabmap_costmap_plugins/CMakeLists.txt
+++ b/rtabmap_costmap_plugins/CMakeLists.txt
@@ -45,6 +45,7 @@ IF("$ENV{ROS_DISTRO}" STRLESS "lyrical")
   IF("$ENV{ROS_DISTRO}" STRLESS "jazzy")
     target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_JAZZY)
   ENDIF()
+  target_compile_definitions(rtabmap_costmap_plugins PRIVATE -DPRE_ROS_LYRICAL)
   ament_target_dependencies(rtabmap_costmap_plugins ${AmentLibraries})
 ELSE()
   target_link_libraries(rtabmap_costmap_plugins PRIVATE ${Libraries})
diff --git a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp
index affc632d..5e1115d1 100644
--- a/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp
+++ b/rtabmap_costmap_plugins/include/rtabmap_costmap_plugins/voxel_layer.hpp
@@ -284,6 +284,15 @@ protected:
   rcl_interfaces::msg::SetParametersResult
   dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
 
+  /**
+   * @brief Declares one of this layer's parameters and returns its value
+   * @param node the node the layer runs in
+   * @param name the parameter's name, without the layer prefix
+   * @param defaultValue the value to use when nothing else sets it
+   */
+  template<typename T, typename NodeT>
+  T declareOrGetParameter(NodeT & node, const std::string & name, const T & defaultValue);
+
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml
index 58ed5be1..c3feac74 100644
--- a/rtabmap_costmap_plugins/package.xml
+++ b/rtabmap_costmap_plugins/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_costmap_plugins</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's costmap plugins.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/rtabmap_costmap_plugins/src/voxel_layer.cpp b/rtabmap_costmap_plugins/src/voxel_layer.cpp
index dcb6ca3f..27a99792 100644
--- a/rtabmap_costmap_plugins/src/voxel_layer.cpp
+++ b/rtabmap_costmap_plugins/src/voxel_layer.cpp
@@ -58,42 +58,76 @@ using rcl_interfaces::msg::ParameterType;
 namespace rtabmap_costmap_plugins
 {
 
+namespace
+{
+
+/// nav2's Observation::cloud_ used to be a raw pointer and is now the cloud itself, so
+/// it is reached through this rather than dereferenced directly.
+inline const sensor_msgs::msg::PointCloud2 & cloudOf(
+  const sensor_msgs::msg::PointCloud2 & cloud)
+{
+  return cloud;
+}
+
+inline const sensor_msgs::msg::PointCloud2 & cloudOf(
+  const sensor_msgs::msg::PointCloud2 * cloud)
+{
+  return *cloud;
+}
+
+/// nav2 hands out observations by value up to kilted and by shared pointer after it.
+inline const nav2_costmap_2d::Observation & obsOf(
+  const nav2_costmap_2d::Observation & observation)
+{
+  return observation;
+}
+
+inline const nav2_costmap_2d::Observation & obsOf(
+  const std::shared_ptr<const nav2_costmap_2d::Observation> & observation)
+{
+  return *observation;
+}
+
+}  // namespace
+
+/// nav2 declared a layer's parameters through Layer::declareParameter up to kilted and
+/// through the node itself after it.
+template<typename T, typename NodeT>
+T VoxelLayer::declareOrGetParameter(
+  NodeT & node, const std::string & name, const T & defaultValue)
+{
+#ifdef PRE_ROS_LYRICAL
+  declareParameter(name, rclcpp::ParameterValue(defaultValue));
+  T value = defaultValue;
+  node->get_parameter(name_ + "." + name, value);
+  return value;
+#else
+  return node->declare_or_get_parameter(name_ + "." + name, defaultValue);
+#endif
+}
+
 void VoxelLayer::onInitialize()
 {
   nav2_costmap_2d::ObstacleLayer::onInitialize();
 
-  declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true));
-  declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0));
-  declareParameter("max_obstacle_height", rclcpp::ParameterValue(2.0));
-  declareParameter("z_voxels", rclcpp::ParameterValue(10));
-  declareParameter("origin_z", rclcpp::ParameterValue(0.0));
-  declareParameter("z_resolution", rclcpp::ParameterValue(0.2));
-  declareParameter("unknown_threshold", rclcpp::ParameterValue(15));
-  declareParameter("mark_threshold", rclcpp::ParameterValue(0));
-  declareParameter("combination_method", rclcpp::ParameterValue(1));
-  declareParameter("publish_voxel_map", rclcpp::ParameterValue(false));
-  declareParameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
-
   auto node = node_.lock();
   if (!node) {
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  node->get_parameter(name_ + "." + "enabled", enabled_);
-  node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
-  node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
-  node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_);
-  node->get_parameter(name_ + "." + "z_voxels", size_z_);
-  node->get_parameter(name_ + "." + "origin_z", origin_z_);
-  node->get_parameter(name_ + "." + "z_resolution", z_resolution_);
-  node->get_parameter(name_ + "." + "unknown_threshold", unknown_threshold_);
-  node->get_parameter(name_ + "." + "mark_threshold", mark_threshold_);
-  node->get_parameter(name_ + "." + "publish_voxel_map", publish_voxel_);
-  node->get_parameter(name_ + "." + "robot_base_frame", robot_base_frame_);
+  enabled_ = declareOrGetParameter(node, "enabled", true);
+  footprint_clearing_enabled_ = declareOrGetParameter(node, "footprint_clearing_enabled", true);
+  min_obstacle_height_ = declareOrGetParameter(node, "min_obstacle_height", 0.0);
+  max_obstacle_height_ = declareOrGetParameter(node, "max_obstacle_height", 2.0);
+  size_z_ = declareOrGetParameter(node, "z_voxels", 10);
+  origin_z_ = declareOrGetParameter(node, "origin_z", 0.0);
+  z_resolution_ = declareOrGetParameter(node, "z_resolution", 0.2);
+  unknown_threshold_ = declareOrGetParameter(node, "unknown_threshold", 15);
+  mark_threshold_ = declareOrGetParameter(node, "mark_threshold", 0);
+  publish_voxel_ = declareOrGetParameter(node, "publish_voxel_map", false);
+  robot_base_frame_ = declareOrGetParameter(node, "robot_base_frame", std::string("base_link"));
 
-  int combination_method_param{};
-  node->get_parameter(name_ + "." + "combination_method", combination_method_param);
+  const int combination_method_param = declareOrGetParameter(node, "combination_method", 1);
 #ifdef PRE_ROS_JAZZY
   combination_method_ = combination_method_param;
 #else
@@ -169,7 +203,11 @@ void VoxelLayer::updateBounds(
   useExtraBounds(min_x, min_y, max_x, max_y);
 
   bool current = true;
+#ifdef PRE_ROS_LYRICAL
   std::vector<nav2_costmap_2d::Observation> observations, clearing_observations;
+#else
+  std::vector<nav2_costmap_2d::Observation::ConstSharedPtr> observations, clearing_observations;
+#endif
 
   // get the marking observations
   current = getMarkingObservations(observations) && current;
@@ -182,16 +220,15 @@ void VoxelLayer::updateBounds(
 
   // raytrace freespace
   for (unsigned int i = 0; i < clearing_observations.size(); ++i) {
-    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
+    raytraceFreespace(obsOf(clearing_observations[i]), min_x, min_y, max_x, max_y);
   }
 
   // place the new obstacles into a priority queue... each with a priority of zero to begin with
-  for (std::vector<nav2_costmap_2d::Observation>::const_iterator it = observations.begin(); it != observations.end();
-    ++it)
+  for (auto it = observations.begin(); it != observations.end(); ++it)
   {
-    const nav2_costmap_2d::Observation & obs = *it;
+    const nav2_costmap_2d::Observation & obs = obsOf(*it);
 
-    const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
+    const sensor_msgs::msg::PointCloud2 & cloud = cloudOf(obs.cloud_);
 
     double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
     double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
@@ -277,7 +314,9 @@ void VoxelLayer::raytraceFreespace(
 {
   auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>();
 
-  if (clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0) {
+  const sensor_msgs::msg::PointCloud2 & clearing_cloud = cloudOf(clearing_observation.cloud_);
+
+  if (clearing_cloud.height == 0 || clearing_cloud.width == 0) {
     return;
   }
 
@@ -311,8 +350,8 @@ void VoxelLayer::raytraceFreespace(
   }
 
   clearing_endpoints_->data.clear();
-  clearing_endpoints_->width = clearing_observation.cloud_->width;
-  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->width = clearing_cloud.width;
+  clearing_endpoints_->height = clearing_cloud.height;
   clearing_endpoints_->is_dense = true;
   clearing_endpoints_->is_bigendian = false;
 
@@ -331,9 +370,9 @@ void VoxelLayer::raytraceFreespace(
   double map_end_y = origin_y_ + getSizeInMetersY();
   double map_end_z = origin_z_ + getSizeInMetersZ();
 
-  sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
-  sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_x(clearing_cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_y(clearing_cloud, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> iter_z(clearing_cloud, "z");
 
   for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
     double wpx = *iter_x;
@@ -431,7 +470,7 @@ void VoxelLayer::raytraceFreespace(
 
   if (publish_clearing_points) {
     clearing_endpoints_->header.frame_id = global_frame_;
-    clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
+    clearing_endpoints_->header.stamp = clearing_cloud.header.stamp;
 
     clearing_endpoints_pub_->publish(std::move(clearing_endpoints_));
   }
diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py
index a66b55ec..22b9633a 100644
--- a/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py
+++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_scan.launch.py
@@ -27,13 +27,17 @@ def launch_setup(context, *args, **kwargs):
     localization = localization == 'True' or localization == 'true'
     icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
     icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
+    deskewing = LaunchConfiguration('deskewing').perform(context)
+    deskewing = deskewing == 'True' or deskewing == 'true'
     
     parameters={
           'frame_id':'base_footprint',
           'use_sim_time':use_sim_time,
           'subscribe_depth':False,
           'subscribe_rgb':False,
-          'subscribe_scan':True,
+          'subscribe_scan':not deskewing,
+          'subscribe_scan_cloud':deskewing,
+          'scan_cloud_is_2d': True,
           'approx_sync':True,
           'use_action_for_goal':True,
           'Reg/Strategy':'1',
@@ -50,13 +54,24 @@ def launch_setup(context, *args, **kwargs):
         arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
                
     remappings=[
-          ('scan', '/scan')]
+          ('scan', '/scan' if not deskewing else 'scan_not_used'),
+          ('scan_cloud', '/scan/deskewed' if deskewing else 'scan_cloud_not_used' )]
     if icp_odometry:
         remappings.append(('odom', 'icp_odom'))
     
     return [
         # Nodes to launch
-        
+
+        # Lidar deskewing (optional, useful with real lidar)
+        Node(
+            condition=IfCondition(LaunchConfiguration('deskewing')),
+            package='rtabmap_util', executable='lidar_deskewing', output='screen',
+            parameters=[{'wait_for_transform': 0.1,
+                         'fixed_frame_id': 'odom',
+                         'slerp': False,
+                         'use_sim_time':use_sim_time}],
+            remappings=[('input_scan', '/scan')]),
+
         # ICP odometry (optional)
         Node(
             condition=IfCondition(LaunchConfiguration('icp_odometry')),
@@ -97,5 +112,9 @@ def generate_launch_description():
             'icp_odometry', default_value='false',
             description='Launch ICP odometry on top of wheel odometry.'),
 
+        DeclareLaunchArgument(
+            'deskewing', default_value='false',
+            description='Do lidar scan deskewing based on wheel odometry.'),
+
         OpaqueFunction(function=launch_setup)
     ])
diff --git a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py
index 3fecc353..3c48bab0 100644
--- a/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py
+++ b/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py
@@ -182,6 +182,12 @@ def generate_launch_description():
             'icp_odometry', default_value='false',
             description='Launch ICP odometry on top of wheel odometry.'),
         
+        DeclareLaunchArgument(
+            'deskewing', default_value='false',
+            description='Do lidar scan deskewing based on wheel odometry. In simulation it '
+                        'doesn\'t matter because laser scans are not skewed, but the option '
+                        'is there for the demo.'),
+
         DeclareLaunchArgument(
             'x_pose', default_value='-2.0',
             description='Initial position of the robot in the simulator.'),
diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml
index d3e398d9..fa753754 100644
--- a/rtabmap_demos/package.xml
+++ b/rtabmap_demos/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_demos</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's demo launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -17,7 +17,7 @@
   <exec_depend>rtabmap_util</exec_depend>
   <exec_depend>rtabmap_rviz_plugins</exec_depend>
   <exec_depend>rtabmap_viz</exec_depend>
-  <!-- <exec_depend>nav2_bringup</exec_depend> Not available on Lyrical -->
+  <exec_depend>nav2_bringup</exec_depend>
   
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml
index 734284e1..9eb290d6 100644
--- a/rtabmap_examples/package.xml
+++ b/rtabmap_examples/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_examples</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's example launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -21,7 +21,7 @@
   <exec_depend>rtabmap_viz</exec_depend>
   <exec_depend>imu_filter_madgwick</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
-  <!-- <exec_depend>realsense2_camera</exec_depend> Not available on Lyrical -->
+  <exec_depend>realsense2_camera</exec_depend>
   <!-- <exec_depend>velodyne</exec_depend> Not available on Lyrical -->
   
   <export>
diff --git a/rtabmap_launch/launch/rtabmap.launch.py b/rtabmap_launch/launch/rtabmap.launch.py
index 416688ab..e54c5b21 100644
--- a/rtabmap_launch/launch/rtabmap.launch.py
+++ b/rtabmap_launch/launch/rtabmap.launch.py
@@ -508,7 +508,7 @@ def generate_launch_description():
         DeclareLaunchArgument('odom_tf_angular_variance',   default_value='0.01',    description='If TF is used to get odometry, this is the default angular variance'),
         DeclareLaunchArgument('odom_tf_linear_variance',    default_value='0.001',   description='If TF is used to get odometry, this is the default linear variance'),
         DeclareLaunchArgument('odom_args',                  default_value='',      description='More arguments for odometry (overwrite same parameters in rtabmap_args).'),
-        DeclareLaunchArgument('odom_sensor_sync',           default_value='false', description=''),
+        DeclareLaunchArgument('odom_sensor_sync',           default_value='true', description='Correct each sensor\'s position for the motion between its stamp and the odometry\'s, using TF.'),
         DeclareLaunchArgument('odom_guess_frame_id',        default_value='',      description=''),
         DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0',   description=''),
         DeclareLaunchArgument('odom_guess_min_rotation',    default_value='0.0',   description=''),
diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml
index dba2b1f6..f308cbb3 100644
--- a/rtabmap_launch/package.xml
+++ b/rtabmap_launch/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_launch</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's main launch files.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/rtabmap_msgs/CMakeLists.txt b/rtabmap_msgs/CMakeLists.txt
index 01e7cebb..d40b0c34 100644
--- a/rtabmap_msgs/CMakeLists.txt
+++ b/rtabmap_msgs/CMakeLists.txt
@@ -11,11 +11,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
-  # issues #1285 #1288
+  # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under
+  # emulated arm64 should not fail the build)
   find_library(
     rcutils_LIB NAMES rcutils
     PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
 endif()
 
diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml
index 1a26dab8..e81c2d50 100644
--- a/rtabmap_msgs/package.xml
+++ b/rtabmap_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_msgs</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's msgs package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/rtabmap_odom/CMakeLists.txt b/rtabmap_odom/CMakeLists.txt
index 7b10ad25..4a1a72e6 100644
--- a/rtabmap_odom/CMakeLists.txt
+++ b/rtabmap_odom/CMakeLists.txt
@@ -157,4 +157,52 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(OpenCV REQUIRED COMPONENTS core imgcodecs)
+  # Recorded sensor input is replayed by the tests themselves, not by "ros2 bag play".
+  find_package(rosbag2_cpp REQUIRED)
+
+  # Real frames the visual odometry tests register against, read from the source tree:
+  # these binaries are never installed, and the fixtures are not either. See
+  # test/data/README.md for where they come from.
+  set(rtabmap_odom_test_data_root "${CMAKE_CURRENT_SOURCE_DIR}/test/data")
+
+  # 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 -- odom,
+  # rgbd_image, scan_cloud -- with rtabmap_sync's and 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 from 30 and rtabmap_sync from 50; keep the ranges apart.
+  set(rtabmap_odom_test_domain_id 70)
+  macro(rtabmap_odom_add_node_test test_name)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_odom_test_domain_id}
+      TIMEOUT 300)
+    math(EXPR rtabmap_odom_test_domain_id "${rtabmap_odom_test_domain_id} + 1")
+    if(TARGET ${test_name})
+      target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test)
+      target_compile_definitions(${test_name} PRIVATE
+        RTABMAP_ODOM_TEST_DATA_ROOT="${rtabmap_odom_test_data_root}")
+      target_link_libraries(${test_name} rtabmap_odom_plugins rtabmap_odom
+        opencv_core opencv_imgcodecs rosbag2_cpp::rosbag2_cpp rtabmap::core)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries})
+      else()
+        target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_odom_add_node_test(test_odometry_ros)
+  rtabmap_odom_add_node_test(test_rgbd_odometry)
+  rtabmap_odom_add_node_test(test_stereo_odometry)
+  rtabmap_odom_add_node_test(test_icp_odometry)
+endif()
+
 ament_package()
diff --git a/rtabmap_odom/README.md b/rtabmap_odom/README.md
new file mode 100644
index 00000000..9c77e6cf
--- /dev/null
+++ b/rtabmap_odom/README.md
@@ -0,0 +1,273 @@
+# rtabmap_odom
+
+Odometry for [RTAB-Map](https://github.com/introlab/rtabmap): where the robot is relative to a local fixed frame, estimated from its own sensors — a pose that moves continuously and never jumps, but drifts over time.
+
+SLAM needs a pose for every measurement it maps. These nodes produce one by registering each new frame against the last — visually from an RGB-D or stereo camera, or geometrically from a lidar — and integrating the result into a [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) and a TF.
+
+## Contents
+
+- [Nodes](#nodes)
+  - [Choosing a sensor modality for the environment](#choosing-a-sensor-modality-for-the-environment)
+- [Library](#library)
+- [Conventions](#conventions)
+  - [Frames and TF](#frames-and-tf)
+  - [RTAB-Map's own parameters](#rtab-maps-own-parameters)
+  - [Feeding in an external guess](#feeding-in-an-external-guess)
+  - [IMU](#imu)
+  - [Update rates and dropped frames](#update-rates-and-dropped-frames)
+  - [Lost frames, resets and new maps](#lost-frames-resets-and-new-maps)
+- [Services](#services)
+- [Published topics](#published-topics)
+  - [Outputting filtered scans and features](#outputting-filtered-scans-and-features)
+- [Diagnostics](#diagnostics)
+- [License](#license)
+
+## Nodes
+
+| Node | Description |
+|---|---|
+| [rgbd_odometry](doc/rgbd_odometry.md) | Visual odometry from a color image and a depth image registered to it. |
+| [stereo_odometry](doc/stereo_odometry.md) | Visual odometry from a stereo pair. |
+| [icp_odometry](doc/icp_odometry.md) | Geometric odometry from a 2D or 3D lidar. |
+
+Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable.
+
+### Choosing a sensor modality for the environment
+
+Which to use is a question about the **environment**, not about which sensor is better. A camera tracks visual texture; a lidar tracks geometry. Each fails where its own cue is missing, and the two failures do not overlap much.
+
+| Environment | Use | Why |
+|---|---|---|
+| Visually textured and well lit — offices, cluttered rooms, daylight outdoors | **Camera** | Plenty of features to match, and appearance gives loop closure for free. |
+| Textureless but geometrically rich — bare corridors with doorways and furniture, warehouse aisles | **Lidar** | Blank walls give a camera nothing; the shape of the space still constrains ICP. |
+| Dark, or lighting that changes abruptly | **Lidar** | A camera is simply blind. Lidar does not care. |
+| Geometrically plain but visually rich — a large open hall with a patterned floor, textured flat walls | **Camera** | [Degenerate geometry](doc/icp_odometry.md#degenerate-geometry) defeats ICP here, while the texture is exactly what a camera needs. |
+| Both plain and textureless — an empty warehouse, a long featureless tunnel | **Wheel odometry**, with either as a corrector | Neither cue is present. This is the case where wheel odometry carries the pose. |
+| Repetitive and self-similar — tiled floors, rows of racking, a long colonnade | **Wheel odometry as the guess**, with either on top | Both cues are present but *ambiguous*: a camera matches the wrong copy of a feature, a lidar the wrong bay of shelving. See [Repetitive patterns](doc/rgbd_odometry.md#repetitive-patterns). |
+| Outdoors at range | **Stereo camera or 3D lidar** | RGB-D depth stops working outdoors; both of these keep going. |
+
+**Do not underestimate wheel odometry.** On a wheeled robot it is locally excellent and only drifts over distance — the opposite failure from both of the above, which are locally noisy but not systematically biased. It is also the only one of the three that keeps working when the environment offers no cue at all — and, because it is indifferent to what the scene *looks* like, the only one that is not fooled when the scene repeats itself.
+
+**Fuse the wheels with an IMU before feeding them in.** [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) is the standard way: its EKF combines wheel odometry with IMU orientation and angular rates into one filtered `odom` topic, which is a markedly better guess than the wheels alone. [FusionCore](https://github.com/manankharwar/fusioncore) is another EKF that does the same job. The IMU fixes exactly what encoders are worst at — yaw through a turn, and wheel slip, which encoders report as motion that never happened. Where the camera or lidar fails outright, that filtered estimate is what carries the robot through, and a pipeline built this way degrades instead of breaking.
+
+Which is why the robust arrangement is rarely one of them alone: feed wheel odometry in as `guess_frame_id` and the registration starts near the answer every frame. That covers the camera's fast-motion and blank-wall failures and the lidar's degenerate-corridor failure, while the camera or lidar in turn corrects the wheels' drift. See [Feeding in an external guess](#feeding-in-an-external-guess).
+
+**For 2D indoor odometry a lidar usually costs less computation.** Registering a few hundred scan points is far less CPU than detecting, describing and matching visual features on every frame, and it needs no GPU — which is what decides whether odometry keeps up on the small onboard computers these robots carry.
+
+With both a camera and a lidar, the usual arrangement is `icp_odometry` for the pose and the camera for appearance — see [Combining a camera and a lidar](doc/icp_odometry.md#combining-a-camera-and-a-lidar).
+
+## Library
+
+The package installs a C++ library, documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_odom/generated/index.html) generated from the headers.
+
+**`OdometryROS`** is the base class all three nodes derive from, and it is where most of this package's behaviour actually lives. It owns the RTAB-Map `Odometry` object, the pose integration, the TF broadcast, the IMU intake, the services and the diagnostics. Each node subclasses it to do one thing: turn its own topics into a `rtabmap::SensorData` and hand it over. That is why the three nodes share nearly all of their parameters and publish exactly the same topics.
+
+It also runs the registration on **its own thread**. A frame arriving while the previous one is still being processed does not block the subscription callback; see [Update rates and dropped frames](#update-rates-and-dropped-frames).
+
+## Conventions
+
+These apply to all three nodes.
+
+### Frames and TF
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `frame_id` | `string` | `"base_link"` | The robot frame being tracked. The pose published is this frame's, not the sensor's — the sensor-to-robot transform is read from TF. |
+| `odom_frame_id` | `string` | `"odom"` | The fixed frame the pose is expressed in. |
+| `publish_tf` | `bool` | `true` | Broadcast the pose on TF. **Turn this off if something else already publishes that transform**, or the two fight and TF alternates between them. What exactly is broadcast depends on `guess_frame_id`: without it, `odom_frame_id` → `frame_id`; with it, a correction `odom_frame_id` → `guess_frame_id` ([why](#it-also-keeps-tf-alive-through-a-failure)). |
+| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a needed transform before giving up on the frame. |
+| `initial_pose` | `string` | `""` | Starting pose, `"x y z roll pitch yaw"`. Also settable at runtime through `reset_odom_to_pose`. |
+| `ground_truth_frame_id` | `string` | `""` | When set, the pose is taken from this TF instead of being computed — for replaying a dataset with a known trajectory. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame within the ground truth TF tree. |
+| `guess_frame_id` | `string` | `""` | A frame carrying another odometry source, used as the initial guess for each registration. Documented with its companions under [Feeding in an external guess](#feeding-in-an-external-guess) -- **the highest-value parameter here for a wheeled robot**. |
+
+The sensor must be connected to `frame_id` in TF **before the first frame arrives**, or that frame is dropped with a warning. A static publisher is the usual answer.
+
+### RTAB-Map's own parameters
+
+Everything in RTAB-Map's odometry parameter set is exposed as a ROS parameter **under its RTAB-Map name**, so tuning is done directly:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --ros-args \
+  -p "Odom/Strategy:='1'" \
+  -p "Vis/MinInliers:='15'" \
+  -p "Odom/ResetCountdown:='1'"
+```
+
+**Note the quoting.** Every RTAB-Map parameter is declared as a **string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. Writing `-p Odom/Strategy:=1` makes ROS infer an integer, and the node throws on startup rather than starting with the wrong value:
+
+```
+parameter 'Odom/Strategy' has invalid type: Wrong parameter type,
+parameter {Odom/Strategy} is of type {string}, setting it to {integer} is not allowed.
+```
+
+The inner quotes are what keeps it a string. Shell quotes alone do not help, since the value is parsed as YAML after the shell is done with it. In a launch file the same rule reads naturally: `{'Odom/Strategy': '1'}`.
+
+This applies **only** to RTAB-Map's own parameters. The nodes' ROS parameters -- `frame_id`, `publish_tf`, `scan_voxel_size`, `approx_sync` -- are declared with their real types and take plain values.
+
+Which parameters exist depends on the node: each declares the set matching its sensor, so `Vis/*` appears on the visual nodes and `Icp/*` only on `icp_odometry`. `ros2 param list` on a running node is the authoritative list; the meaning of each is in [RTAB-Map's parameter reference](https://github.com/introlab/rtabmap/blob/master/corelib/include/rtabmap/core/Parameters.h).
+
+The two worth knowing before anything else:
+
+- **`Odom/Strategy`** selects the registration algorithm — `0` frame-to-map (default, more accurate), `1` frame-to-frame (cheaper), and others for the external VO libraries RTAB-Map can be built against.
+- **`Odom/ResetCountdown`** automatically resets odometry after this many consecutive lost frames instead of staying lost forever. `0` disables it, which is the default.
+
+`config_path` loads the same parameters from an INI file; only the odometry ones are taken from it.
+
+### Feeding in an external guess
+
+Registration works far better when it starts near the answer. Two ways to supply one:
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `guess_frame_id` | `string` | `""` | A TF frame carrying another odometry source — wheels, IMU-integrated, a base driver. Its motion between frames becomes the initial guess. |
+| `guess_min_translation` | `double` | `0.0` | Skip frames whose guessed motion is below this, in meters. `0` disables. |
+| `guess_min_rotation` | `double` | `0.0` | Same, in radians. |
+| `guess_min_time` | `double` | `0.0` | Same, in seconds. |
+| `guess_linear_variance` | `double` | `0.001` | Covariance of the published pose when the guess is used directly. |
+| `guess_angular_variance` | `double` | `0.001` | Same, rotational. |
+
+**`guess_frame_id` is the single biggest improvement available to a wheeled robot.** Wheel odometry is locally excellent and globally hopeless; visual and ICP registration is the reverse. Giving the registration a wheel-odometry guess makes it converge more often, faster, and survive the frames where the camera sees nothing.
+
+The `guess_min_*` parameters additionally suppress processing while the robot is stationary, which stops a static scene from accumulating drift and saves the CPU.
+
+#### It also keeps TF alive through a failure
+
+Setting `guess_frame_id` also changes *how* the pose is broadcast. This is worth understanding before odometry fails on a real robot, because it decides what the rest of the system sees while it is lost.
+
+With a guess frame configured, the node no longer publishes `odom_frame_id` → `frame_id` directly. It publishes a **correction** instead, `odom_frame_id` → `guess_frame_id`.
+
+So the guess source keeps the conventional `odom` frame -- whatever produces it, the robot's driver or [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) -- and **this node takes a different name for its own `odom_frame_id`**. The examples in this repository use `vo` for the visual nodes and `icp_odom` for the lidar one:
+
+```mermaid
+flowchart TD
+    ODOM(["/vo<br><i>odom_frame_id</i>"])
+    GUESS(["/odom<br><i>guess_frame_id</i>"])
+    BASE(["/base_link<br><i>frame_id</i>"])
+    SENSOR(["/camera or /lidar<br><i>the sensor's header.frame_id</i>"])
+    ODOM -->|correction, this node<br>e.g. ~10 Hz, ~50 ms delay| GUESS
+    GUESS -->|robot driver or robot_localization<br>e.g. ~50 Hz, ~1 ms delay| BASE
+    BASE -->|static| SENSOR
+```
+
+*The rates and delays above are examples only — yours depend on the sensor, the base driver and the computer.*
+
+**That chain keeps being published while registration is lost.** The correction freezes at the last successfully computed pose composed with the motion the guess has accumulated since, so `base_link` keeps moving in TF at the guess source's rate, driven entirely by the guess. Nothing downstream stalls or jumps; the pose just accumulates that source's drift until registration recovers. Without `guess_frame_id` there is no correction to publish and **no TF at all is broadcast while lost**, which is what breaks the tree.
+
+Pair it with `Odom/ResetCountdown` and the recovery is complete. Take a robot turning to face a white wall: visual odometry loses tracking, TF keeps flowing from the wheels, and after the configured number of failed frames the odometry resets — not to where it was when it got lost, but to `last computed pose × guess motion`, which is where the wheels say the robot has got to in the meantime. Registration restarts from there and the trajectory carries on with only the drift the wheels accumulated.
+
+What it resets to depends on what is available, in this order:
+
+1. **A guess** — resets to the last pose composed with the guess motion, as above.
+2. **No guess, but `odom_frame_id` → `frame_id` exists in TF at the sensor frame's stamp** — resets to that pose. This is the `publish_tf:=false` arrangement: this node publishes only its odometry topic, [`robot_localization`](https://docs.ros.org/en/jazzy/p/robot_localization/) fuses that topic with the wheels and the IMU, and the filter owns the transform. The reset therefore lands on the filter's current estimate — the odometry gets restarted from where the fused solution says the robot is, having contributed to that solution itself while it was working. `publish_tf` has to be off for this to mean anything, and **`publish_null_when_lost` should be off too**: the null pose is a signal for consumers that read it as one, and a filter fusing this topic is not — it would be handed an invalid pose to fuse. With it off the node simply stops publishing while lost, and the filter carries on from its other inputs until registration recovers.
+3. **Neither** — resets to the last computed pose, so the robot resumes believing it never moved while lost.
+
+After an automatic reset the countdown is left armed, so if odometry still cannot initialize on the next frames it keeps re-resetting to the latest guess rather than getting stuck.
+
+### IMU
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `wait_imu_to_init` | `bool` | `false` | Hold off until an IMU message has arrived, so gravity is known from the first frame. |
+| `imu_queue_size` | `int` | `200` | Depth of the IMU buffer. IMUs run far faster than cameras; this is why it is large. |
+| `qos_imu` | `int` | value of `qos` | Reliability of the `imu` subscription. |
+| `always_check_imu_tf` | `bool` | `false` | Re-read the IMU-to-robot transform every message rather than caching it. |
+
+The `imu` topic is optional on all three nodes. Supplying it lets odometry know which way is down, which constrains roll and pitch — worth doing on any robot that has an IMU, and close to mandatory for a handheld or aerial one.
+
+**With no `guess_frame_id`, the IMU also supplies the rotation half of each frame's guess.** The rotation measured between the previous frame and this one becomes the guess's orientation, leaving the translation to the motion model. That is often the difference between tracking a fast turn and losing it, since rotation is what breaks feature matching first. An external guess takes precedence when there is one: `guess_frame_id` is used whole, and the IMU is not consulted for the guess at all.
+
+### Update rates and dropped frames
+
+Registration runs on its own thread, so a slow frame does not block the subscription. What happens to the frames arriving meanwhile is a choice:
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `always_process_most_recent_frame` | `bool` | `true` | Drop frames that arrive while registration is still running and take the newest. `false` registers every frame in order, on the subscription thread. |
+| `expected_update_rate` | `double` | `0.0` | The rate frames are expected at, in Hz. Used only when `max_update_rate` is unset, and it is a **ceiling**: a frame arriving sooner than `1/expected_update_rate` after the last one is skipped, with a warning that the input is faster than expected. `0` disables. |
+| `max_update_rate` | `double` | `0.0` | Throttle registration to at most this rate, skipping frames silently. Takes precedence over `expected_update_rate`. `0` disables. |
+| `min_update_rate` | `double` | `0.0` | Treat odometry as **lost and reset it** when more than `1/min_update_rate` passes between updates — the motion assumption no longer holds across a gap that long. `0` disables. |
+
+**`always_process_most_recent_frame` already bounds the delay.** A frame arriving while registration is still running is dropped on the spot rather than queued, so the worker always picks up the newest frame and the published pose is at most one registration behind the sensor. No backlog ever forms. `/diagnostics` reports how many frames went this way.
+
+That is why **`max_update_rate` is about CPU, not latency**: given the skipping above, the worst-case delay is roughly the same whether it is set or not. What it changes is how many frames get registered at all. Set it to give the rest of the robot its cores back — not to make the pose fresher, which it will not do.
+
+Setting `always_process_most_recent_frame:=false` is the opposite trade: every frame is registered, in order, on the subscription thread. That is what you want when replaying a bag, where dropping frames loses data you meant to process.
+
+### Lost frames, resets and new maps
+
+A frame that cannot be registered is *lost*: the node publishes an all-zero pose with `9999` down the diagonal of both covariance matrices, which says there is no pose here to use.
+
+The first frame after a reset — from `reset_odom`, `reset_odom_to_pose` or `Odom/ResetCountdown` — carries the same `9999` for a different reason. It is an *initialization* rather than a registration: nothing to measure against, no velocity to carry over. Its pose is real and meant to be used; what the covariance says is that it does not continue the last valid one.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `publish_null_when_lost` | `bool` | `true` | Publish a null pose, with `9999` on the covariance diagonals, when a frame cannot be registered. `false` publishes nothing. |
+
+**Leave it on, unless a filter is consuming this topic.** A consumer that sees the null message knows odometry is lost; one that sees nothing cannot tell that apart from a node that died or a topic that was never connected. `rtabmap` relies on it to know the frame should not be mapped.
+
+`rtabmap` reads an identity pose, or both covariances at `9999`, as a discontinuity, and starts a **new map** rather than deforming the graph across a jump the robot never made:
+
+```
+Odometry is reset (identity pose or high variance detected). Increment map id!
+```
+
+While it is lost, `publish_null_when_lost:=true` publishes a null pose and no velocity for every frame, both marked `9999`. With `:=false` it publishes nothing — except that a guess frame keeps it going: every frame that re-initialises the map, which `Odom/ResetCountdown` makes frequent, is published with the guess's pose and confidence, so the topic has no gap for as long as the guess is there. What differs between configurations is the first frame after the reset, and where it restarts from:
+
+| | First frame after the reset | Second frame | TF while lost | `rtabmap` |
+|---|---|---|---|---|
+| `publish_null_when_lost:=true` (default), with or without a guess | recovered pose, `9999` on both pose and velocity | registered, from the recovered pose | unbroken with a guess, absent without one | new map |
+| `publish_null_when_lost:=false` with `guess_frame_id` | recovered pose and the guess's velocity, both with the guess's covariance | registered, from the recovered pose | unbroken | one session |
+| `publish_null_when_lost:=false`, `publish_tf:=false`, another node publishing `odom` → `base_link` | not published | registered, from the recovered pose | unbroken, published by the other node | one session |
+| `publish_null_when_lost:=false` with neither | not published | registered, from the pose held before the loss | absent until it recovers | one session, across the gap |
+
+*Registered* is the ordinary case: a pose and a velocity measured against the previous valid frame, with the covariance the registration computed.
+
+The two middle rows are the ones to build on. Either an external source is named through `guess_frame_id`, and the restarting frame is published as a continuation of the trajectory — the poses *and* the covariances staying continuous for as long as that guess is published — or a filter such as `robot_localization` owns `odom` → `base_link`, and the reset adopts whatever pose it holds.
+
+**The last row is a trap.** With nothing to say where the robot went while the odometry was lost, the reset resumes at the pose from before it, that motion is dropped from the trajectory, and since no `9999` ever reaches `rtabmap` the map is deformed across the gap rather than split at it. The node reports that combination as an error at startup.
+
+For finer control, put an intermediate node between this one and `rtabmap` and let it set the covariances itself. It decides what counts as a discontinuity, instead of that being inferred from the reset alone — starting a new map when the guess frame has gone quiet, say, and the newly computed pose may be wrong even though registration reported success.
+
+Recovering from lost is what `Odom/ResetCountdown` is for, or the `reset_odom` service. Combined with `guess_frame_id` it also keeps the TF tree intact throughout — see [It also keeps TF alive through a failure](#it-also-keeps-tf-alive-through-a-failure).
+
+## Services
+
+| Service | Type | Description |
+|---|---|---|
+| `reset_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the internal local map and restart the pose — at the identity, or, with `guess_frame_id` configured, at whatever pose the guess frame currently holds, so that odometry restarts where the other source says the robot is. |
+| `reset_odom_to_pose` | [`rtabmap_msgs/srv/ResetPose`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ResetPose.html) | Reset to a given `x y z roll pitch yaw`. |
+| `pause_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop processing incoming frames. |
+| `resume_odom` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume. |
+| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Change RTAB-Map's own log level at runtime. |
+
+## Published topics
+
+Common to all three nodes. **Every one of them, `odom` included, is published only when something is subscribed** -- the work of building each message is skipped otherwise. The TF broadcast is not gated this way and happens whenever `publish_tf` is on -- except while registration is lost with no guess frame configured, when there is nothing to broadcast.
+
+| Topic | Type | Description |
+|---|---|---|
+| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | The pose and velocity. Covariance is meaningful: it grows with registration uncertainty, and is `9999` on the diagonal when lost. |
+| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Everything about how the frame was registered — inlier count, matches, features, timings. The first thing to look at when odometry misbehaves. |
+| `odom_info_lite` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | The same without the per-feature arrays, for logging or a slow link. |
+| `odom_local_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The feature map the current frame was registered against. Visual paths only — built from the frame's visual words, so `icp_odometry` never fills it. |
+| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. |
+| `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. |
+| `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). |
+| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. |
+
+### Outputting filtered scans and features
+
+`odom_rgbd_image` and `odom_sensor_data/*` republish the frame **after** odometry has worked on it, which is the point of them — they are what odometry actually registered, not a copy of the input:
+
+- **Features are included.** Registration writes the keypoints, their 3D positions and their descriptors back into the frame, so these topics carry them. `odom_sensor_data/features` is that alone, with the images and scan removed.
+- **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original.
+- **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too.
+
+## Diagnostics
+
+All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
diff --git a/rtabmap_odom/doc/icp_odometry.md b/rtabmap_odom/doc/icp_odometry.md
new file mode 100644
index 00000000..1877614d
--- /dev/null
+++ b/rtabmap_odom/doc/icp_odometry.md
@@ -0,0 +1,238 @@
+# icp_odometry
+
+Odometry from a 2D or 3D lidar, by registering each scan against the previous one with ICP.
+
+No features and no appearance: the motion is whatever transform best aligns this scan's points with the last. That makes it indifferent to lighting and texture — it works in the dark, and on the blank white corridor where [rgbd_odometry](rgbd_odometry.md) has nothing to track.
+
+What it is sensitive to instead is **geometry**. ICP can only recover motion that the scene's shape constrains, and a scene can fail to constrain it: see [Degenerate geometry](#degenerate-geometry), which is the failure mode worth understanding before deploying this.
+
+The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+  - [Reusing the filtered scan downstream](#reusing-the-filtered-scan-downstream)
+- [Parameters](#parameters)
+  - [Where these defaults come from](#where-these-defaults-come-from)
+  - [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something)
+- [Preparing the scan](#preparing-the-scan)
+- [Deskewing](#deskewing)
+- [Degenerate geometry](#degenerate-geometry)
+- [Combining a camera and a lidar](#combining-a-camera-and-a-lidar)
+- [When it loses track](#when-it-loses-track)
+
+## Usage
+
+2D lidar:
+
+```bash
+ros2 run rtabmap_odom icp_odometry --ros-args \
+  -r scan:=/scan \
+  -p frame_id:=base_link
+```
+
+3D lidar:
+
+```bash
+ros2 run rtabmap_odom icp_odometry --ros-args \
+  -r scan_cloud:=/velodyne_points \
+  -p frame_id:=base_link \
+  -p "Icp/PointToPlane:='true'" \
+  -p scan_normal_k:=10 \
+  -p scan_voxel_size:=0.1
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::ICPOdometry',
+    name='icp_odometry',
+    parameters=[{'frame_id': 'base_link',
+                 'scan_voxel_size': 0.1,
+                 'scan_normal_k': 10,
+                 'Icp/PointToPlane': 'true'}],
+    remappings=[('scan_cloud', '/velodyne_points')])
+```
+
+## Subscribed Topics
+
+One of the two scan topics, not both.
+
+| Topic | Type | Description |
+|---|---|---|
+| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D lidar. |
+| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D lidar, or a 2D one already converted to a cloud. |
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional, and more useful here than elsewhere — it pins roll and pitch, which a lidar alone constrains poorly. |
+
+## Published Topics
+
+Most are common to all three nodes; see [the README](../README.md#published-topics). Two belong to this path:
+
+| Topic | Type | Description |
+|---|---|---|
+| `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The accumulated scan map the current scan was registered against. |
+| `odom_filtered_input_scan` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input scan **after** deskewing, voxelization, range filtering and normal estimation — exactly what ICP registered, carrying the original header. |
+
+### Reusing the filtered scan downstream
+
+Remap `odom_filtered_input_scan` onto `rtabmap`'s `scan_cloud` and the map is built from the scan this node already prepared, rather than from the raw sweep.
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    ICP["icp_odometry"]
+    MAP["rtabmap<br>subscribe_scan_cloud:=true"]
+    LIDAR -->|scan_cloud| ICP
+    ICP -->|odom_filtered_input_scan| MAP
+    ICP -->|odom + TF| MAP
+```
+
+That skips the expensive half twice over. Voxelization and normal estimation are not repeated, since the cloud arrives already decimated and carrying `normal_*` fields, and the deskewing this node did is carried along with it.
+
+The alternative for deskewing is to do it **before** odometry, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md), and fan the corrected cloud out to both nodes:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    DESKEW["lidar_deskewing"]
+    ICP["icp_odometry"]
+    MAP["rtabmap<br>subscribe_scan_cloud:=true"]
+    LIDAR -->|scan_cloud| DESKEW
+    DESKEW -->|deskewed cloud| ICP & MAP
+    ICP -->|odom + TF| MAP
+```
+
+That is the only way to give `rtabmap` **every point of the sweep**. `odom_filtered_input_scan` carries the decimated cloud ICP registered, so a map built from it inherits whatever voxelization odometry applied — and outdoors that is 30 to 50 cm. Deskewing upstream separates the two: odometry can filter as hard as it likes while the map is built from the full-resolution cloud, at the cost of an extra node and an extra copy of every sweep.
+
+## Parameters
+
+Specific to this node: the scan is filtered **before** ICP sees it, and these control that. The registration itself is tuned through RTAB-Map's `Icp/*` parameters.
+
+These two sets overlap, and the node resolves the overlap for you — see [Where these defaults come from](#where-these-defaults-come-from), because the defaults below are not what the source's initializers suggest.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `scan_voxel_size` | `double` | `0.05` | Downsample to one point per voxel of this size, in meters. From `Icp/VoxelSize`. `0` disables it here. |
+| `scan_downsampling_step` | `int` | `1` | Keep every Nth point. From `Icp/DownsamplingStep`. Cheaper than voxelization but density-dependent; prefer `scan_voxel_size`. |
+| `scan_range_min` | `double` | `0.0` | Drop points closer than this, in meters. From `Icp/RangeMin`. `0` disables. Useful against the robot's own body. |
+| `scan_range_max` | `double` | `0.0` | Drop points farther than this. From `Icp/RangeMax`. `0` disables. |
+| `scan_normal_k` | `int` | `5` | Estimate each point's normal from this many neighbours. From `Icp/PointToPlaneK`. **Point-to-plane ICP needs normals**; without them it has nothing to work with. |
+| `scan_normal_radius` | `double` | `0.0` | Estimate normals from neighbours within this radius instead. From `Icp/PointToPlaneRadius`. `0` disables. |
+| `scan_normal_ground_up` | `double` | `0.0` | Force normals to point upward when within this dot-product threshold of vertical. From `Icp/PointToPlaneGroundNormalsUp`. Helps on ground planes. |
+| `scan_cloud_max_points` | `int` | `-1` | How many points a full sweep of the lidar holds. It is the **denominator of the correspondence ratio** — see [Making the correspondence ratio mean something](#making-the-correspondence-ratio-mean-something). `-1` leaves it unset; an organized cloud fills it in automatically. |
+| `scan_cloud_is_2d` | `bool` | `false` | Treat `scan_cloud` as a planar scan even though it carries a `z` field, so it is registered as a 2D scan rather than a 3D one. For a 2D lidar already converted to a cloud. |
+| `deskewing` | `bool` | `false` | Correct for motion during the sweep. See [Deskewing](#deskewing). |
+| `deskewing_slerp` | `bool` | `false` | Interpolate the deskewing transform rather than looking up TF per point. Faster, slightly less accurate. |
+| `topic_queue_size` | `int` | `1` | Queue depth of the scan subscription. Deliberately small: a stale scan is worse than a dropped one. |
+
+### Where these defaults come from
+
+Filtering a scan and filtering it again inside ICP would be wasted work, so at startup the node moves each filter from RTAB-Map's parameter to its own:
+
+> `IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.`
+
+That log line is normal, not a warning about your configuration. Each `scan_*` parameter above **takes its default from the matching `Icp/*` parameter**, and the `Icp/*` one is then set to `0` so the filter runs once, here, rather than twice. This is why `scan_voxel_size` is `0.05` and `scan_normal_k` is `5` out of the box rather than disabled.
+
+Setting the ROS parameter explicitly wins: the transfer is skipped and the `Icp/*` value is zeroed instead. Setting **both** is the case to avoid — the node warns that both are set, and the scan is then filtered twice:
+
+```
+IcpOdometry: Both parameter "Icp/VoxelSize" and ros parameter "scan_voxel_size" are set.
+```
+
+So tune through `scan_*` **or** through `Icp/*`, not both.
+
+### Making the correspondence ratio mean something
+
+`Icp/CorrespondenceRatio` decides whether a registration is trustworthy: the points ICP managed to pair, over the points it could have paired. `scan_cloud_max_points` is what sets that second number. Set it to the theoretical maximum points per sweep. No need to set it explicitly for organized clouds though, `width × height` will be used as maximum points.
+
+Left at `-1`, the denominator becomes the size of the larger of the two scans being matched (for dense clouds). Take two scans that came back with 30 and 50 points — a lidar staring at open space, where most rays returned nothing. Dividing by 50 says "we matched most of what we saw", and the ratio looks healthy. But the sensor emits 10000 rays a sweep, so 50 returns means almost nothing was in range, and the registration is resting on nearly no evidence. Told that a full sweep is 10000 points, ICP divides by that instead and the ratio collapses to what the overlap actually was, so the threshold rejects the frame.
+
+## Preparing the scan
+
+ICP cost grows with the number of points, and a 3D lidar produces far more than registration needs — a 64-beam sensor is a hundred thousand points per sweep, and aligning them all is both slow and *no more accurate* than aligning a well-spread subset. Voxelization is therefore on by default at 5 cm.
+
+What it buys beyond speed is even density, which matters more than the point count: a raw lidar sweep is dense near the sensor and sparse far away, so an unvoxelized ICP is dominated by whatever is closest — often the robot itself or the ground right under it. Size `scan_voxel_size` to the environment: **0.05 to 0.2 m indoors**, and **0.3 to 0.5 m outdoors**, where the scene is far larger and the extra resolution buys nothing but CPU.
+
+**Move `Icp/MaxCorrespondenceDistance` with it — a good rule of thumb is ten times the voxel size.** The two are coupled: voxelizing at 0.3 m leaves neighbouring points that far apart, so a correspondence distance of 0.1 m cannot pair anything and ICP returns nothing at all.
+
+**Point-to-plane ICP converges better than point-to-point** on the flat surfaces that dominate most environments, and it is what the default `scan_normal_k` of 5 is there to support:
+
+```bash
+-p "Icp/PointToPlane:='true'" -p scan_normal_k:=10
+```
+
+The quoting is not optional: RTAB-Map parameters are strings, and an unquoted `true` makes the node throw on startup ([why](../README.md#rtab-maps-own-parameters)).
+
+Whether it is on by default depends on how RTAB-Map was built — `Icp/PointToPlane` defaults to `true` only with libpointmatcher available, and `false` otherwise — so set it explicitly if you care.
+
+`scan_range_min` is worth setting on any robot whose lidar can see parts of itself. Those points are perfectly self-consistent between scans, so they pull the alignment toward "no motion" — a bias that looks like the robot under-travelling rather than like an error.
+
+## Deskewing
+
+A spinning lidar measures its points over a whole revolution, not at an instant. If the robot moves during that revolution, the scan is a smear — the points are in a frame that no longer exists by the time the sweep ends. At walking pace with a 10 Hz lidar this is centimeters; on a fast vehicle it dominates the error budget.
+
+`deskewing:=true` corrects it. It needs the cloud to carry **per-point timestamps**, in a field named `t`, `time`, `stamps` or `timestamp`; without one the node logs an error and drops the frame rather than guessing.
+
+There are two ways it gets the motion to correct with, and which one is used depends on whether you gave it an external guess:
+
+- **With `guess_frame_id` set** — the motion comes from that TF. This is the accurate route, and the reason to pair deskewing with wheel odometry or an IMU-integrated frame.
+- **Without it** — a constant-velocity model from the previous frame's estimate. It cannot deskew the very first frame, and it degrades exactly when velocity changes fastest, which is when deskewing matters most.
+
+`deskewing_slerp` interpolates between the sweep's endpoints rather than looking up a transform per point. Much cheaper, and accurate enough unless the motion within one sweep is strongly non-linear.
+
+## Degenerate geometry
+
+This is the failure that matters, and it is not a bug. ICP recovers only the motion the scene constrains, and some scenes do not constrain all of it:
+
+- **A long featureless corridor** does not constrain motion *along* the corridor. The walls look identical a meter forward, so ICP happily reports no motion while the robot drives. The map then folds the corridor up into a fraction of its length.
+- **A large open space** with everything out of range constrains nothing at all.
+- **A flat plane** — a warehouse floor to a horizontal 2D lidar — constrains height and tilt but not translation.
+
+RTAB-Map detects this rather than walking into it, but only on the point-to-plane path. The defences, in order of effectiveness:
+
+1. **`guess_frame_id` with wheel odometry.** The guess supplies the motion ICP cannot see, and ICP corrects the part it can. This turns the corridor case from a failure into a non-issue, and it is why lidar odometry on a wheeled robot should essentially always have it.
+2. **The structural complexity check**, which is the built-in one. With `Icp/PointToPlane` on, a scan whose normals fail to span the space — the definition of a corridor — scores below `Icp/PointToPlaneMinComplexity` (`0.02`) and is handled by `Icp/PointToPlaneLowComplexityStrategy` instead of being trusted:
+
+   | Value | Behaviour |
+   |---|---|
+   | `0` | Reject the transform outright: the frame is reported lost. |
+   | `1` *(default)* | Recompute with point-to-point and constrain the correction to the axes that *are* observable — in a corridor, y and yaw are kept and **x is taken from the guess**. |
+   | `2` | Recompute with point-to-point and accept the result as is. |
+   | `3` | Keep the point-to-plane transform, with the same axis-constrained projection as `1`. |
+
+   The default pairs with defence 1: it detects the unobservable axis and hands that axis to the guess. Without a `guess_frame_id` there is nothing to hand it to, which is why the two belong together.
+3. **A 3D lidar instead of a 2D one**, which sees ceiling, floor and doorways that a horizontal slice misses.
+4. **`Icp/CorrespondenceRatio`** to reject registrations supported by too few correspondences, so a bad frame is reported lost rather than silently accepted.
+
+With `Icp/PointToPlane` off, none of the complexity machinery runs: there are no normals to measure, so a degenerate scan is registered and trusted like any other.
+
+## Combining a camera and a lidar
+
+With both sensors, the usual arrangement is `icp_odometry` for the pose and the camera for appearance:
+
+```mermaid
+flowchart LR
+    LIDAR["lidar"]
+    CAM["RGB-D camera"]
+    ICP["icp_odometry"]
+    SYNC["rgbd_sync"]
+    ODOM(["odom + TF"])
+    MAP["rtabmap<br>subscribe_rgbd + subscribe_scan_cloud"]
+    LIDAR --> ICP --> ODOM --> MAP
+    LIDAR --> MAP
+    CAM --> SYNC --> MAP
+```
+
+Lidar geometry is the more reliable pose source, while loop closure detection is appearance-based and wants the images. `rtabmap` then subscribes to the camera, the scan and this node's odometry together.
+
+## When it loses track
+
+`odom_info` carries the ICP result. The numbers to look at are the correspondence count and ratio: too few correspondences means the scans do not overlap enough, whether because the robot moved too far between them, the range filters are too aggressive, or the scene genuinely changed.
+
+- **Scans too far apart** — the lidar rate is too low for the speed, or `max_update_rate` is throttling too hard.
+- **`Icp/MaxCorrespondenceDistance` too small** — ICP never associates the points at all. It has to be larger than the motion between scans; too large and it associates the wrong things.
+- **Everything filtered away** — check `scan_range_min`/`scan_range_max` and `scan_voxel_size` against the actual scale of the environment.
+
+As everywhere else in this package, `Odom/ResetCountdown` recovers automatically from a lost state instead of staying lost.
diff --git a/rtabmap_odom/doc/rgbd_odometry.md b/rtabmap_odom/doc/rgbd_odometry.md
new file mode 100644
index 00000000..5ba9220f
--- /dev/null
+++ b/rtabmap_odom/doc/rgbd_odometry.md
@@ -0,0 +1,211 @@
+# rgbd_odometry
+
+Visual odometry from a color image, a registered depth image and a calibration.
+
+Each frame's visual features are matched against the previous frame — or against a small local map of recent features — and the camera motion that best explains the matches becomes the pose. Depth turns the 2D feature matches into 3D correspondences, which is what makes the scale real rather than arbitrary.
+
+Use it when an RGB-D camera is the main sensor. For a stereo pair use [stereo_odometry](stereo_odometry.md); for a lidar, [icp_odometry](icp_odometry.md). All three publish the same topics and share the parameters in the [package README](../README.md#conventions), which covers frames, TF, the RTAB-Map parameters, guesses, the IMU and the services. This page covers what is specific to this node.
+
+## Contents
+
+- [Pipeline arrangements](#pipeline-arrangements)
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Synchronization](#synchronization)
+- [Several cameras](#several-cameras)
+- [Repetitive patterns](#repetitive-patterns)
+- [When it loses track](#when-it-loses-track)
+
+## Pipeline arrangements
+
+A camera-only pipeline, with the camera synchronized once and fanned out -- the arrangement [Synchronization](#synchronization) recommends:
+
+```mermaid
+flowchart LR
+    CAM["RGB-D camera"]
+    SYNC["rgbd_sync"]
+    ODOM["rgbd_odometry<br>subscribe_rgbd:=true"]
+    MAP["rtabmap<br>subscribe_rgbd:=true"]
+    CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Without `rgbd_sync`, both nodes subscribe to the three raw topics and each synchronizes them independently -- which works, but lets the two settle on different pairings.
+
+Another arrangement drops `rgbd_sync` altogether and feeds `rtabmap` from **this node's own output** instead:
+
+```mermaid
+flowchart LR
+    CAM["RGB-D camera"]
+    ODOM["rgbd_odometry"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|rgb/image<br>depth/image<br>rgb/camera_info| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. Two things come for free:
+
+- **No separate synchronization.** This node already matched the three topics to register the frame, and republishes the result, so there is no `rgbd_sync` to run and no second synchronizer to agree with.
+- **The features are reused.** `odom_sensor_data` carries the keypoints, their 3D positions and their descriptors that odometry extracted; they survive the conversion back into RTAB-Map on the other side, so `rtabmap` does not redo feature detection and descriptor extraction.
+
+## Usage
+
+Against a camera's raw topics:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --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 frame_id:=base_link
+```
+
+Against an [`rgbd_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) output, which is the better arrangement when anything else consumes the same camera:
+
+```bash
+ros2 run rtabmap_odom rgbd_odometry --ros-args \
+  -p subscribe_rgbd:=true \
+  -r rgbd_image:=/camera/rgbd_image \
+  -p frame_id:=base_link
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::RGBDOdometry',
+    name='rgbd_odometry',
+    parameters=[{'frame_id': 'base_link', 'subscribe_rgbd': True}],
+    remappings=[('rgbd_image', '/camera/rgbd_image')])
+```
+
+## Subscribed Topics
+
+Which topics are used depends on `subscribe_rgbd` and `rgbd_cameras`.
+
+**Default** — `subscribe_rgbd:=false`:
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Color or mono image. Goes through `image_transport`. |
+| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Depth registered to the color camera. `16UC1` in millimeters or `32FC1` in meters. |
+| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Calibration of the color camera. |
+
+**With `subscribe_rgbd:=true`**, one pre-synchronized message instead of three topics:
+
+| `rgbd_cameras` | Topic | Type |
+|---|---|---|
+| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
+
+`rgbd_cameras:=0` takes any number of cameras in a single message, which is what [`rgbdx_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync) produces — the route that needs no rebuild and the only one that goes past six.
+
+| Topic | Type | Description |
+|---|---|---|
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. Constrains roll and pitch; see [the README](../README.md#imu). |
+
+## Published Topics
+
+`odom`, `odom_info`, `odom_local_map`, `odom_last_frame`, `odom_rgbd_image` and the rest are common to all three nodes and documented in [the README](../README.md#published-topics).
+
+## Parameters
+
+Specific to this node. The shared ones — `frame_id`, `publish_tf`, `guess_frame_id`, `max_update_rate`, all of RTAB-Map's own — are in [the README](../README.md#conventions).
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` instead of three raw topics. |
+| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic carrying any number. Only with `subscribe_rgbd:=true`. |
+| `approx_sync` | `bool` | `true` | Match the raw topics by nearest stamp. See [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. Copied to it with a warning. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. |
+| `keep_color` | `bool` | `false` | Keep the color image in the data handed to the odometry instead of converting to grayscale. Registration is grayscale either way; this only matters for what downstream consumers of `odom_rgbd_image` receive. |
+| `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. |
+| `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. |
+| `rgb_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. |
+
+## Synchronization
+
+With `subscribe_rgbd:=false` this node runs its own synchronizer over the three raw topics, and the same trade-off applies as everywhere else in this stack: **exact matching is cheaper and cannot mismatch, but publishes nothing at all if the stamps differ by a nanosecond**. The default here is approximate because many RGB-D cameras do not stamp color and depth identically.
+
+When several nodes consume the same camera — odometry and `rtabmap`, usually — **synchronize once with `rgbd_sync` and set `subscribe_rgbd:=true` on both**. Two independent approximate synchronizers over the same three topics can settle on different pairings, and then `rtabmap` maps a frame at a pose computed from a different one. Feeding both from one `RGBDImage` removes the possibility.
+
+`approx_sync_max_interval` is worth setting whenever approximate matching stays on: without it, a camera that stalls and resumes silently pairs a fresh color frame with a stale depth frame. A tenth of the frame period is a reasonable start.
+
+## Several cameras
+
+More cameras means more of the scene is textured enough to track, which is the usual reason visual odometry fails indoors. Point them in different directions rather than overlapping.
+
+Two routes, both requiring the frames to be synchronized and each camera to be in TF:
+
+- **`rgbd_cameras:=2..6`** subscribes to `rgbd_image0`…`rgbd_imageN` and synchronizes them here.
+- **`rgbd_cameras:=0`** takes one `rgbd_images` topic from `rgbdx_sync`, which has no upper limit and needs no rebuild.
+
+**RTAB-Map has to be built with OpenGV for this.** The default motion estimation is PnP (`Vis/EstimationType=1`, 3D→2D), and the multi-camera version of it lives in OpenGV. Without that dependency the registration refuses to run and says so:
+
+```
+Multi-camera 2D-3D PnP registration is only available if rtabmap is built with
+OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.
+```
+
+Check with `rtabmap --version`, which prints a `With OpenGV:` line. If it says `false`, either rebuild RTAB-Map against OpenGV or switch to `Vis/EstimationType:='0'` (3D→3D), which needs no extra dependency but registers point cloud to point cloud rather than reprojecting, and is the weaker estimator when depth is noisy.
+
+**Hardware-synchronize the cameras if you can.** The node treats the set as one rigid observation at one timestamp: features from every camera are registered together, with the extrinsics from TF held fixed. There is no equivalent of lidar deskewing here — it cannot estimate the motion that happened *within* the rig between one camera's exposure and the next. If the cameras fire at different instants while the robot moves, that motion is absorbed as though the rig had flexed, and the registration is pulled off by however far the robot travelled in between.
+
+Synchronizing the topics is not the same thing: `approx_sync` only decides which frames are grouped, it cannot undo an exposure that happened 20 ms later than its neighbour's.
+
+Calibration matters more with several cameras than with one for the same reason: the extrinsics between them come from TF, and an error there shows up as a constant bias in the estimated motion rather than as an obvious failure.
+
+## Features computed elsewhere
+
+`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed.
+
+This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
+
+What a publisher has to get right:
+
+- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
+- **Keypoints in their own camera's image coordinates.** The node stitches the images side by side and shifts each camera's keypoints by the images that precede it.
+- **3D points in that camera's optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
+- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
+- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
+
+Both images may be left out entirely: the depth image's job was to give the keypoints their depth and they arrive with it, and the color image's was to have features found in it. A frame is then its calibration and its features, which is the whole point — the images are nearly all of the bandwidth. The `camera_info` of each camera has to be there either way, as it is what says how big the image would have been and, through its `frame_id`, where the camera is.
+
+What stops applying, since nothing is extracted: `Vis/MaxFeatures`, `Vis/DepthAsMask`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
+
+## Repetitive patterns
+
+Not every failure announces itself. A scene full of identical detail — a tiled floor, rows of identical shelving, a patterned carpet, a brick wall — hands the matcher plenty of features and plenty of confident matches, just not always the *right* ones. One tile matched to its neighbour looks like a perfectly good inlier, and the pose comes out shifted by exactly one tile. Inlier counts stay healthy, nothing is reported lost, and the trajectory drifts in steps.
+
+The defence is to constrain **where** a match is allowed to come from:
+
+- **`guess_frame_id`** gives each feature a predicted image position, from wheel odometry or another external source.
+- **`Vis/CorGuessWinSize`** bounds the search around that prediction — 40 pixels by default. Reducing it, to 10 or 20, means a feature can only match something close to where the guess says it should be, so the identical neighbour one tile away is never a candidate.
+
+## When it loses track
+
+Visual odometry fails when there is nothing to match: a blank wall, a dark room, motion blur, or a scene where everything moved. The node then publishes a null pose (see [the README](../README.md#lost-frames-resets-and-new-maps)) and `odom_info` says why.
+
+Look at `odom_info` first — `inliers` is the number that matters:
+
+```bash
+ros2 topic echo /odom_info --field inliers
+```
+
+Inliers falling below `Vis/MinInliers` (default 20) is the definition of a lost frame. Whether the fix is more features, a better guess or a different strategy depends on which part is short:
+
+- **Few features detected at all** — the scene is untextured or too dark. Lowering `Vis/MinInliers` lets frames register on fewer matches, but a pose resting on a handful of inliers is poorly constrained and drifts badly — it buys continuity at the price of accuracy. The real fixes are physical. If it is dark, add a light — a spotlight on the robot restores texture the camera can track, and costs far less than changing sensor. Otherwise it is **more field of view**: a wider lens, or [several cameras](#several-cameras) pointed in different directions. It only takes one textured patch somewhere in view to track, so a blank wall filling a narrow FOV stops being a problem the moment the rig can also see the ceiling or a doorway. Failing that, the camera is the wrong sensor here — a lidar if the scene has geometry, wheel odometry if it has neither. See [Choosing a sensor modality for the environment](../README.md#choosing-a-sensor-modality-for-the-environment).
+- **Features detected but few matched** — motion is too fast for the search window, or the frame rate is too low. A `guess_frame_id` from wheel odometry is what helps most here.
+- **Matched but rejected as outliers** — usually a moving scene, or depth that does not agree with the color image. Check that depth really is registered to color.
+
+`Odom/ResetCountdown` gets the node out of a lost state automatically instead of leaving it lost until something calls `reset_odom`.
+
+**A camera alone is not a great odometry source on a wheeled robot.** If the base publishes wheel odometry, feeding it in through `guess_frame_id` is worth more than any amount of tuning here.
diff --git a/rtabmap_odom/doc/stereo_odometry.md b/rtabmap_odom/doc/stereo_odometry.md
new file mode 100644
index 00000000..d39a7062
--- /dev/null
+++ b/rtabmap_odom/doc/stereo_odometry.md
@@ -0,0 +1,219 @@
+# stereo_odometry
+
+Visual odometry from a stereo pair.
+
+Features are found in the left image and matched into the right one to get their depth by disparity, then matched against the previous frame to get the motion. It is the same registration as [rgbd_odometry](rgbd_odometry.md); only the source of depth differs — computed here from the pair rather than measured by the sensor.
+
+That difference is the reason to choose it. A stereo pair works outdoors and at range, where the projected-pattern depth of an RGB-D camera returns nothing, and its accuracy degrades gracefully with distance instead of cutting off. The cost is that depth is only available where there is texture to match, and that it depends on a good stereo calibration.
+
+The shared parameters — frames, TF, guesses, the IMU, RTAB-Map's own parameters, the services — are in the [package README](../README.md#conventions). This page covers what is specific to this node.
+
+## Contents
+
+- [Pipeline arrangements](#pipeline-arrangements)
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Synchronization](#synchronization)
+- [Getting the scale right](#getting-the-scale-right)
+- [Features computed elsewhere](#features-computed-elsewhere)
+- [Repetitive patterns](#repetitive-patterns)
+- [When it loses track](#when-it-loses-track)
+
+## Pipeline arrangements
+
+A typical stereo pipeline, rectification included:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    PROC["stereo_image_proc"]
+    SYNC["stereo_sync"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap"]
+    CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| PROC
+    PROC -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+`stereo_image_proc` can be dropped when the driver already publishes rectified images:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver<br>publishing rectified images"]
+    SYNC["stereo_sync"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap"]
+    CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| SYNC
+    SYNC -->|rgbd_image| ODOM & MAP
+    ODOM -->|odom + TF| MAP
+```
+
+The same shortcut as on the RGB-D side is available here: drop `stereo_sync` and feed `rtabmap` from **this node's own output**.
+
+```mermaid
+flowchart LR
+    CAM["stereo driver<br>publishing rectified images"]
+    ODOM["stereo_odometry"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|left/image_rect<br>right/image_rect<br>camera_info x2| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Remap `rtabmap`'s `rgbd_image` to `odom_rgbd_image`, or set `subscribe_sensor_data` and remap to `odom_sensor_data/raw`. The stereo pair survives the trip intact -- the left image, the right image and both calibrations travel in the one message, exactly as `stereo_sync` would have packed them -- and the features this node extracted come with it, so `rtabmap` does not redo feature detection and descriptor extraction.
+
+Everything above hands the node rectified images. It can also take the raw pair, straight from the driver:
+
+```mermaid
+flowchart LR
+    CAM["stereo driver"]
+    ODOM["stereo_odometry<br>Rtabmap/ImagesAlreadyRectified:=false"]
+    MAP["rtabmap<br>subscribe_rgbd or subscribe_sensor_data"]
+    CAM -->|left/image_raw<br>right/image_raw<br>camera_info x2| ODOM
+    ODOM -->|odom_rgbd_image<br>or odom_sensor_data| MAP
+    ODOM -->|odom + TF| MAP
+```
+
+Two different things can make that work:
+
+- **The odometry rectifies the pair itself.** With `Rtabmap/ImagesAlreadyRectified:=false` it builds a rectification map from the calibration and applies it to every frame, saying so once:
+
+  ```
+  Rtabmap/ImagesAlreadyRectified parameter is set to false but the selected odometry
+  approach cannot process raw stereo images. We will rectify them for convenience.
+  ```
+
+  It needs the geometry between the two cameras to do that — the right `camera_info` carrying `P(0,3)`, or TF between the two camera frames. If a rectification map cannot be built from what the calibration says, the frame is refused rather than registered wrong.
+
+- **The odometry takes them raw.** A few approaches do their own undistortion and want the unrectified images: `Odom/Strategy` `6` (OKVIS), `8` (MSCKF), `9` (VINS-Fusion) and `10` (OpenVINS), each available only if RTAB-Map was built against that library. Nothing rectifies anything then, and `Rtabmap/ImagesAlreadyRectified:=false` simply tells the pipeline to leave the images alone.
+
+Which of the two applies decides what `rtabmap` needs when it is fed from this node's output. If the odometry rectified the pair, the rectified images are what travels on -- they replace the raw ones in the frame -- and `rtabmap` keeps `Rtabmap/ImagesAlreadyRectified` at its default `true`. If the odometry took them raw, they arrive raw, and `rtabmap` needs `Rtabmap/ImagesAlreadyRectified:=false` of its own to rectify them again on its side. Set `Mem/UseOdomFeatures:=false` along with it, since it defaults to `true`: `rtabmap` rectifies a stereo pair but does not currently map features that travelled with the frame into the rectified image, so any it reused would be read against the wrong one.
+
+Rectifying here costs what `stereo_image_proc` would have cost, but only on the frames the odometry actually registers. When it runs slower than the camera — throttled by `max_update_rate`, or dropping frames that arrive while a registration is still running — the rectification happens at the odometry's rate instead of the camera's, and every frame `stereo_image_proc` would have rectified for nothing is saved. Where something else needs the whole stream rectified, the first arrangement is still the one to use.
+
+## Usage
+
+```bash
+ros2 run rtabmap_odom stereo_odometry --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 \
+  -p frame_id:=base_link
+```
+
+```python
+ComposableNode(
+    package='rtabmap_odom',
+    plugin='rtabmap_odom::StereoOdometry',
+    name='stereo_odometry',
+    parameters=[{'frame_id': 'base_link'}],
+    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')])
+```
+
+**The images are normally rectified**, which is what the `image_rect` topic names assume, and what [`stereo_image_proc`](https://docs.ros.org/en/jazzy/p/stereo_image_proc/) produces when the driver does not.
+
+They do not have to be. RTAB-Map can rectify them itself from the calibration -- set `Rtabmap/ImagesAlreadyRectified:=false` and feed it the raw pair with distortion coefficients in the `camera_info`. What does not work is the silent middle case: **unrectified images with that parameter left at its default of `true`**. Nothing fails loudly; disparity is computed across rows that no longer correspond, giving depths that are wrong in a smoothly varying way and a trajectory that is wrong without looking broken.
+
+## Subscribed Topics
+
+**Default** — `subscribe_rgbd:=false`:
+
+| 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, color or mono. |
+| `right/image_rect` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified right image, color or mono. |
+| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Left calibration. |
+| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Right calibration. Its `P` matrix carries the baseline, which sets the scale of the whole trajectory. |
+
+**With `subscribe_rgbd:=true`**, one pre-synchronized message from [`stereo_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync):
+
+| `rgbd_cameras` | Topic | Type |
+|---|---|---|
+| `1` (default) | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `2`–`6` | `rgbd_image0` … `rgbd_image5` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) |
+| `0` | `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) |
+
+| Topic | Type | Description |
+|---|---|---|
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Optional. See [the README](../README.md#imu). |
+
+## Published Topics
+
+Common to all three nodes; see [the README](../README.md#published-topics).
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `subscribe_rgbd` | `bool` | `false` | Take a pre-synchronized `RGBDImage` from `stereo_sync` instead of four raw topics. |
+| `rgbd_cameras` | `int` | `1` | Number of `RGBDImage` topics. `0` means one `RGBDImages` topic. Only with `subscribe_rgbd:=true`. More than one needs RTAB-Map built with OpenGV, and the cameras hardware-synchronized — see [Several cameras](rgbd_odometry.md#several-cameras). |
+| `approx_sync` | `bool` | `false` | Match the raw topics by nearest stamp. **Defaults to exact**, unlike `rgbd_odometry` — see [Synchronization](#synchronization). |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. Only used when `approx_sync` is on. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `5` | Queue depth of the synchronizer. |
+| `queue_size` | `int` | — | **Deprecated**, renamed to `sync_queue_size`. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions. |
+| `keep_color` | `bool` | `false` | Keep the left image in color in the data passed on, rather than converting to grayscale. Registration is grayscale either way. |
+| `image_transport` | `string` | `"raw"` | Transport for both image topics. |
+
+## Synchronization
+
+**This node defaults to exact matching**, because a stereo pair is normally hardware-triggered and the two images therefore carry identical stamps. That is the right default: exact matching is cheaper and cannot pair the left image with the wrong right one — and a mismatched stereo pair does not produce an error, it produces wrong disparities and a wrong trajectory.
+
+The failure mode to recognize is the other one: if the stamps are *not* identical, **nothing is ever published and nothing says why**. Check before assuming the node is broken:
+
+```bash
+ros2 topic echo --once /stereo/left/image_rect --field header.stamp
+ros2 topic echo --once /stereo/right/image_rect --field header.stamp
+```
+
+If they differ, set `approx_sync:=true` and set `approx_sync_max_interval` to something tight — a stereo pair whose images are more than a fraction of a frame apart is not usable for disparity regardless.
+
+## Getting the scale right
+
+Everything about a stereo trajectory's scale comes from the **baseline**, which this node reads from the right camera's `P` matrix (`P[3] = -fx * baseline`). Two consequences:
+
+- A `right/camera_info` whose `P` matrix is all zeros — which some drivers publish before calibration is loaded — gives a zero baseline and no usable depth at all.
+- A calibration whose baseline is off by a few percent produces a trajectory off by the same few percent, consistently, with nothing else looking wrong.
+
+If the map comes out uniformly too large or too small, check the baseline before anything else.
+
+## Features computed elsewhere
+
+`RGBDImage` has fields for local features — `key_points`, `points` and `descriptors` — and when a frame arrives with them filled, this node hands them to the odometry as they are instead of detecting and describing anything. RTAB-Map extracts features only from a frame that brought none, so nothing is recomputed, and neither is the disparity search that would otherwise give each feature its depth.
+
+This is for a camera, or a driver, that already does the extraction: on a multi-camera rig it is most of the per-frame work, and it can be done once and shared with `rtabmap` rather than repeated in each node.
+
+What a publisher has to get right:
+
+- **One entry per camera**, in the same order as the images, for `rgbd_cameras:=0` as well as the numbered topics.
+- **Keypoints in their own camera's left image.** The node stitches the left images side by side and shifts each camera's keypoints by the images that precede it. A stereo pair's features belong to the left image; nothing is expected in the right one.
+- **3D points in that camera's left optical frame.** They are brought into `frame_id` with the camera's transform from TF, the same one used for the calibration.
+- **Descriptors compressed** with `rtabmap::compressData()`, one row per keypoint, the same type for every camera.
+- **Equal counts.** `points` and `descriptors` may be left empty, but if they are there, they must have as many entries as there are keypoints. A frame whose three disagree has its features dropped with an error rather than used out of step.
+
+Both images may be left out entirely — the left image's job was to have features found in it, the right one's to give them their disparity, and they arrive with their 3D positions already. A frame is then its two calibrations and its features, which is the whole point: the images are nearly all of the bandwidth. Both `camera_info` still have to be there, the left one saying how big the image would have been and where the camera is, the right one carrying the baseline in `P(0,3)` — without it there is no scale, features or not.
+
+What stops applying, since nothing is extracted: `Vis/MaxFeatures`, the detector chosen with `Kp/DetectorStrategy`, and the depth bounds `Vis/MinDepth` and `Vis/MaxDepth`. Whatever is published is what gets registered, so the publisher owns those decisions. `Vis/CorType` must stay at `0` (feature matching); optical flow (`1`) reads the images themselves and has nothing to work with here.
+
+## Repetitive patterns
+
+Identical detail repeated across the scene — a tiled floor, rows of shelving, a brick wall — lets the matcher pair a feature with the wrong copy of itself, which drifts the trajectory by exactly one repeat while the inlier count stays healthy and nothing is reported lost. It bites stereo twice over, since the same ambiguity also misplaces the left/right match that sets the depth.
+
+The fix is the same as for [rgbd_odometry](rgbd_odometry.md#repetitive-patterns): an external guess through `guess_frame_id`, with `Vis/CorGuessWinSize` reduced so a match has to come from close to where the guess predicts. `Stereo/WinWidth` and `Stereo/WinHeight` matter here too — a correlation window smaller than the repeating pattern has nothing unique to lock onto.
+
+## When it loses track
+
+The same diagnosis as [rgbd_odometry](rgbd_odometry.md#when-it-loses-track) — `odom_info`'s `inliers` is the number to watch — with two failure modes specific to stereo:
+
+- **Poor rectification.** Matched features should lie on the same image row. If they do not, either the pair is unrectified while `Rtabmap/ImagesAlreadyRectified` is `true`, or the calibration itself is off.
+- **Untextured scene.** With no texture there is nothing to match *between* left and right either, so there is no depth at all — worse than the RGB-D case, where the sensor still measures wrong depth on a blank wall.
+
+`Stereo/*` parameters tune the disparity matching itself: `Stereo/MaxDisparity` bounds how close a point can be, `Stereo/WinWidth` and `Stereo/WinHeight` the correlation window. They are listed by `ros2 param list` like every other RTAB-Map parameter.
diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
index ca0df5bc..b0e8328c 100644
--- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h
@@ -58,57 +58,144 @@ namespace rtabmap {
 class Odometry;
 }
 
+/**
+ * @file
+ * @brief The node the three odometry nodes of this package are built on.
+ */
+
 namespace rtabmap_odom {
 
+/**
+ * @brief Runs RTAB-Map's odometry as a ROS node: everything but the subscriptions.
+ *
+ * `rgbd_odometry`, `stereo_odometry` and `icp_odometry` differ only in what they listen
+ * to. Each turns its own topics into a rtabmap::SensorData and hands it to processData();
+ * from there on this class does the work -- registration, pose integration, the `odom`
+ * topic and its TF, the IMU intake, the services, the diagnostics, the reset policy when
+ * tracking is lost. That is why the three nodes share nearly all of their parameters and
+ * publish the same topics.
+ *
+ * A subclass is expected to:
+ * - call init() from its constructor, saying which families of RTAB-Map parameters it
+ *   accepts, which decides both the defaults and what the node will accept being set;
+ * - create its subscriptions in onOdomInit() and describe them with initDiagnosticMsg();
+ * - call tick() when a message arrives and processData() once a frame is complete;
+ * - implement flushCallbacks(), so that a reset can drop whatever its synchronizer holds.
+ *
+ * The class is also a UThread. By default the frame handed to processData() is passed to
+ * that thread and the callback returns at once, so a slow registration cannot block the
+ * executor; a frame arriving while the thread is busy is dropped rather than queued. With
+ * `always_process_most_recent_frame:=false` it is registered on the calling thread
+ * instead, which keeps every frame at the cost of holding up the executor.
+ *
+ * @see the package README for the parameters and topics these nodes have in common.
+ */
 class OdometryROS : public rclcpp::Node, public UThread
 {
 
 public:
+	/// Constructs the node under its default name.
 	explicit OdometryROS(const rclcpp::NodeOptions & options);
+	/// Constructs the node under @p name, which is what the three nodes use.
 	explicit OdometryROS(const std::string & name, const rclcpp::NodeOptions & options);
 	virtual ~OdometryROS();
 
+	/**
+	 * @brief Hands a complete frame to the odometry; called by a subclass's callback.
+	 * @param[in,out] data   the frame to register, which comes back carrying the
+	 *                       features the odometry ended up using
+	 * @param[in]     header stamp and frame of the data, used to publish the result
+	 *
+	 * The frame is either queued for the worker thread or registered right here,
+	 * depending on `always_process_most_recent_frame`. Either way, a frame that arrives
+	 * while the previous one is still being registered is dropped: the odometry stays on
+	 * the newest data rather than falling behind.
+	 */
 	void processData(rtabmap::SensorData & data, const std_msgs::msg::Header & header);
 
+	/// `reset_odom` service: starts a new map at the origin, or at the guess frame's pose.
 	void resetOdom(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `reset_odom_to_pose` service: starts a new map at the pose given in the request.
 	void resetToPose(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request>, std::shared_ptr<rtabmap_msgs::srv::ResetPose::Response>);
+	/// `pause_odom` service: keeps the subscriptions but stops registering what arrives.
 	void pause(const std::shared_ptr<rmw_request_id_t>,	const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `resume_odom` service: registers again, starting from the next frame.
 	void resume(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_debug` service: raises RTAB-Map's own log level to debug at runtime.
 	void setLogDebug(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_info` service; see setLogDebug().
 	void setLogInfo(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_warning` service; see setLogDebug().
 	void setLogWarn(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
+	/// `log_error` service; see setLogDebug().
 	void setLogError(const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<std_srvs::srv::Empty::Request>, std::shared_ptr<std_srvs::srv::Empty::Response>);
 
+	/// The robot frame the odometry is computed for, `frame_id`.
 	const std::string & frameId() const {return frameId_;}
+	/// The frame the estimated poses are expressed in, `odom_frame_id`.
 	const std::string & odomFrameId() const {return odomFrameId_;}
+	/// The frame an external motion guess is read from, `guess_frame_id`; empty if unused.
 	const std::string & guessFrameId() const {return guessFrameId_;}
+	/// The RTAB-Map parameters this node was configured with, defaults included.
 	const rtabmap::ParametersMap & parameters() const {return parameters_;}
+	/// Whether the `pause_odom` service has been called and not resumed since.
 	bool isPaused() const {return paused_;}
 
 protected:
+	/**
+	 * @brief Declares the node's parameters and creates the odometry; call it last in the
+	 *        subclass constructor.
+	 * @param[in] stereoParams true if the node takes RTAB-Map's stereo parameters
+	 * @param[in] visParams    true if it takes the visual registration ones
+	 * @param[in] icpParams    true if it takes the scan matching ones
+	 *
+	 * The three flags decide which RTAB-Map parameters the node declares, and so which
+	 * ones it accepts being set: `icp_odometry` refuses a `Vis/` parameter and the other
+	 * two refuse an `Icp/` one. onOdomInit() is called at the end, for the subclass to
+	 * create its subscriptions.
+	 */
 	void init(bool stereoParams, bool visParams, bool icpParams);
+	/// The reliability the subclass should give its own subscriptions, from `qos`.
 	rmw_qos_reliability_policy_t qos() const {return qos_;}
+	/**
+	 * @brief Starts the diagnostics, once the subclass knows what it subscribed to.
+	 * @param[in] subscribedTopicsMsg the human readable list logged at startup and
+	 *                                repeated in the "no data received" warning
+	 * @param[in] approxSync          whether the subclass matches stamps approximately,
+	 *                                which that warning mentions as a likely cause
+	 * @param[in] subscribedTopic     the one topic whose rate is watched, if any
+	 */
 	void initDiagnosticMsg(const std::string & subscribedTopicsMsg, bool approxSync, const std::string & subscribedTopic = "");
 
+	/// Drops whatever the subclass's synchronizer holds; called when the odometry resets.
 	virtual void flushCallbacks() {};
+	/// The node's TF buffer, for the subclass to look up its sensors' frames.
 	tf2_ros::Buffer & tfBuffer() {return *tfBuffer_;}
+	/// How long a TF lookup may block, from `wait_for_transform`.
 	const double & waitForTransform() const {return waitForTransform_;}
+	/// The velocity of the last registered frame, null when there is no estimate yet.
 	rtabmap::Transform velocityGuess() const;
+	/// Stamp of the last registered frame, 0 before the first one.
 	double previousStamp() const {return previousStamp_;}
+	/// Called after a frame has been registered and published, for a subclass to add to it.
 	virtual void postProcessData(const rtabmap::SensorData & /*data*/, const std_msgs::msg::Header & /*header*/) const {}
 
 private:
 	void processData();
 	virtual void mainLoop();
 	virtual void mainLoopKill();
+	/// Lets a subclass adjust the RTAB-Map parameters before the odometry is created.
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
+	/// Called at the end of init(), where a subclass creates its subscriptions.
 	virtual void onOdomInit() {}
 
 	void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
 	void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
 
 protected:
+	/// The callback group the subclass's sensor subscriptions belong to.
 	rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
+	/// Reports the arrival of an input message to the diagnostics, before anything else.
 	void tick(const rclcpp::Time & stamp);
 
 private:
diff --git a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp
index dc50b450..589866b1 100644
--- a/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp
+++ b/rtabmap_odom/include/rtabmap_odom/icp_odometry.hpp
@@ -44,6 +44,17 @@ using namespace rtabmap;
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from a laser scanner, 2D or 3D, by scan matching.
+ *
+ * Takes a sensor_msgs::msg::LaserScan or a sensor_msgs::msg::PointCloud2 and registers
+ * each scan against the previous ones with ICP. A cloud whose points carry their own
+ * timestamps is deskewed first, using TF or the last known velocity, since a scan taken
+ * while the robot moves is not one rigid observation.
+ *
+ * @see doc/icp_odometry.md for the topics, the parameters and the shapes it can and
+ *      cannot constrain.
+ */
 class ICPOdometry : public rtabmap_odom::OdometryROS
 {
 public:
diff --git a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
index 52649daf..b6046da0 100644
--- a/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+++ b/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/subscriber_filter.hpp>
 
 #include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/key_point.hpp>
+#include <rtabmap_msgs/msg/point3f.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
@@ -50,6 +52,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from an RGB-D camera, or from several on one rig.
+ *
+ * Takes either the three raw topics of a camera (`rgb/image`, `depth/image`,
+ * `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per
+ * camera, and registers each frame's visual features against a local feature map. A
+ * frame that arrives with its own keypoints, 3D points and descriptors is registered
+ * with those rather than having them extracted again.
+ *
+ * @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses
+ *      tracking.
+ */
 class RGBDOdometry : public rtabmap_odom::OdometryROS
 {
 public:
@@ -61,10 +75,19 @@ private:
 	virtual void updateParameters(rtabmap::ParametersMap & parameters);
 	virtual void onOdomInit();
 
+	/**
+	 * Local features, when the input topic carries them, are indexed per camera like
+	 * the images are: one entry per camera, in the same order. They are optional, and
+	 * a frame that comes without them is processed exactly as before, the features
+	 * being extracted from the images downstream.
+	 */
 	void commonCallback(
 				const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
 				const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
-				const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos);
+				const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
+				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
+				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
+				const std::vector<cv::Mat> & localDescriptorsMsgs = {});
 
 	void callback(
 			const sensor_msgs::msg::Image::ConstSharedPtr image,
diff --git a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
index 5b47c8d0..96cdfbf5 100644
--- a/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+++ b/rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
@@ -43,12 +43,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.hpp>
 #endif
 #include <sensor_msgs/msg/image.hpp>
+#include <rtabmap_msgs/msg/key_point.hpp>
+#include <rtabmap_msgs/msg/point3f.hpp>
 #include <rtabmap_msgs/msg/rgbd_image.hpp>
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
 namespace rtabmap_odom
 {
 
+/**
+ * @brief Odometry from a stereo pair, or from several on one rig.
+ *
+ * Takes either the four raw topics of a stereo camera (left and right image plus their
+ * calibrations) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages carrying the
+ * pair, one per camera. The right camera's `P(0,3)` is what gives the trajectory its
+ * scale. A frame that arrives with its own keypoints, 3D points and descriptors is
+ * registered with those rather than having them extracted again.
+ *
+ * @see doc/stereo_odometry.md for the topics, the parameters and the scale it depends on.
+ */
 class StereoOdometry : public rtabmap_odom::OdometryROS
 {
 public:
@@ -60,11 +73,20 @@ private:
 	virtual void updateParameters(rtabmap::ParametersMap & parameters);
 	virtual void onOdomInit();
 
+	/**
+	 * Local features, when the input topic carries them, are indexed per camera like
+	 * the images are: one entry per camera, in the same order, and placed in that
+	 * camera's left image. They are optional, and a frame that comes without them is
+	 * processed exactly as before, the features being extracted downstream.
+	 */
 	void commonCallback(
 			const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
 			const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
 			const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
-			const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos);
+			const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
+			const std::vector<cv::Mat> & localDescriptorsMsgs = {});
 
 	void callback(
 			const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml
index 14870d32..2562d4a7 100644
--- a/rtabmap_odom/package.xml
+++ b/rtabmap_odom/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_odom</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's odometry package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -30,8 +30,14 @@
   <depend>rtabmap_util</depend>
   <depend>rtabmap_sync</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+  <!-- The odometry tests replay recorded sensor data from test/data/lidar. -->
+  <test_depend>rosbag2_cpp</test_depend>
+  <test_depend>rosbag2_storage_mcap</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 
 </package>
diff --git a/rtabmap_odom/rosdoc2.yaml b/rtabmap_odom/rosdoc2.yaml
new file mode 100644
index 00000000..6e3ef7d9
--- /dev/null
+++ b/rtabmap_odom/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_odom
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_odom --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_odom 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_odom',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }
diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index bc34b042..6a44b869 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_odom/OdometryROS.h"
 
 #include <sensor_msgs/msg/image.hpp>
@@ -61,6 +62,44 @@ using namespace rtabmap;
 
 namespace rtabmap_odom {
 
+namespace {
+
+/**
+ * @brief The covariance of a pose that came from the guess frame instead of registration.
+ *
+ * Used wherever the guess is what the published pose rests on: a frame the odometry did
+ * not update because it had not moved enough, and the frame that restarts the map after
+ * a reset. Nothing was measured in either case, so the confidence is the one the guess
+ * was declared to have rather than anything the registration computed.
+ */
+cv::Mat guessCovariance(double linearVariance, double angularVariance)
+{
+	cv::Mat covariance = cv::Mat::zeros(6,6,CV_64FC1);
+	covariance.at<double>(0,0) = linearVariance;  // xx
+	covariance.at<double>(1,1) = linearVariance;  // yy
+	covariance.at<double>(2,2) = linearVariance;  // zz
+	covariance.at<double>(3,3) = angularVariance; // rr
+	covariance.at<double>(4,4) = angularVariance; // pp
+	covariance.at<double>(5,5) = angularVariance; // yawyaw
+	return covariance;
+}
+
+/**
+ * @brief The velocity a motion implies, for a frame with no registration to measure one.
+ *
+ * Named apart from the guess itself so that it can be called where a `guessVelocity`
+ * variable is in scope.
+ */
+rtabmap::Transform velocityFrom(const rtabmap::Transform & motion, double dt)
+{
+	UASSERT(dt > 0.0);
+	float x,y,z,roll,pitch,yaw;
+	motion.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+	return rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
+}
+
+}  // namespace
+
 OdometryROS::OdometryROS(const rclcpp::NodeOptions & options) :
 		OdometryROS("odometry", options)
 	{}
@@ -83,6 +122,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	publishNullWhenLost_(true),
 	publishCompressedSensorData_(false),
 	qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT),
+	bufferedDataToProcess_(false),
 	paused_(false),
 	resetCountdown_(0),
 	resetCurrentCount_(0),
@@ -127,7 +167,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
-	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
 
 	std::string initialPoseStr;
 	frameId_ = this->declare_parameter("frame_id", frameId_);
@@ -194,6 +234,17 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 				"are the same frame (value=\"%s\"). \"guess_frame_id\" is disabled.", odomFrameId_.c_str());
 		guessFrameId_.clear();
 	}
+	if(!publishNullWhenLost_ && guessFrameId_.empty() && publishTf_)
+	{
+		RCLCPP_ERROR(this->get_logger(), "\"publish_null_when_lost\" is false, but nothing can "
+				"say where odometry restarts after being lost: \"guess_frame_id\" is not set and "
+				"\"publish_tf\" is true, so the %s->%s fallback returns this node's own pose. "
+				"Whatever the robot did while lost will be silently dropped from the trajectory "
+				"and mapped across. Set \"guess_frame_id\", or set \"publish_tf\" to false if "
+				"another node (e.g. robot_localization) publishes %s->%s, or leave "
+				"\"publish_null_when_lost\" true.",
+				odomFrameId_.c_str(), frameId_.c_str(), odomFrameId_.c_str(), frameId_.c_str());
+	}
 	RCLCPP_INFO(this->get_logger(), "Odometry: frame_id               = %s", frameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: odom_frame_id          = %s", odomFrameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: publish_tf             = %s", publishTf_?"true":"false");
@@ -457,14 +508,14 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
 				imus_.erase(imus_.begin());
 			}
 		}
-		if(dataMutex_.lockTry() == 0)
+		UScopeMutex dataLock(dataMutex_, false);
+		if(dataLock.lockTry() == 0)
 		{
 			if(bufferedDataToProcess_ && rtabmap_conversions::timestampFromROS(dataHeaderToProcess_.stamp) <= stamp)
 			{
 				bufferedDataToProcess_ = false;
 				dataReady_.release();
 			}
-			dataMutex_.unlock();
 		}
 	}
 }
@@ -473,7 +524,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 {
 	//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
 	double clockNow = rtabmap_conversions::timestampFromROS(now());
-	if(dataMutex_.lockTry() == 0)
+	UScopeMutex dataLock(dataMutex_, false);
+	if(dataLock.lockTry() == 0)
 	{
 		if(bufferedDataToProcess_) {
 			RCLCPP_ERROR(this->get_logger(), "We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.",
@@ -486,7 +538,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
 		if(alwaysProcessMostRecentFrame_) {
 			dataReady_.release();
 		}
-		dataMutex_.unlock();
+		dataLock.unlock(); // processData() below must run unlocked
 		++processedMsgs_;
 		if(!alwaysProcessMostRecentFrame_) {
 			processData();
@@ -628,8 +680,18 @@ void OdometryROS::processData()
 		imuProcessed_ = true;
 	}
 
+	// Whether this is a frame at all, as opposed to an IMU-only update. Neither the image
+	// nor the features answer that on their own: a frame that brings its own features has
+	// no image, and a frame of an empty scene has no feature. The calibration does, being
+	// there whenever a camera produced the data -- the same rule RTAB-Map's own
+	// Odometry::process() applies before registering anything.
+	const bool isFrame = !data.imageRaw().empty() ||
+			!data.cameraModels().empty() ||
+			!data.stereoCameraModels().empty() ||
+			!data.laserScanRaw().isEmpty();
+
 	Transform groundTruth;
-	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+	if(isFrame)
 	{
 		// Detect time jump in the past
 		double clockNow = now().seconds();
@@ -688,7 +750,7 @@ void OdometryROS::processData()
 		{
 			groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, *tfBuffer_, waitForTransform_);
 
-			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+			if(isFrame)
 			{
 				// Use only XYZ to handle the case odometry was previously initialized with IMU,
 				// we assume that the ground truth contains also a real initial orientation
@@ -716,39 +778,6 @@ void OdometryROS::processData()
 		}
 	}
 
-	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
-	if(tooOldPreviousData)
-	{
-		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
-				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
-				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
-
-		if(!guess_.isNull())
-		{
-			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
-					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
-			odometry_->reset(odometry_->getPose() * guess_);
-			guess_.setNull();
-			guessPreviousPose_.setNull();
-		}
-		else
-		{
-			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
-			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
-			if(tfPose.isNull())
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
-				odometry_->reset(odometry_->getPose());
-			}
-			else
-			{
-				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
-						odomFrameId_.c_str(), frameId_.c_str());
-				odometry_->reset(tfPose);
-			}
-		}
-	}
-	
 	bool skipOdometryUpdate = false;
 
 	rtabmap::Transform pose;
@@ -756,23 +785,36 @@ void OdometryROS::processData()
 	rtabmap::Transform guessVelocity;
 
 	Transform guessCurrentPose;
+	// Whether the guess has a previous pose to be relative to, which decides how the pose
+	// is seeded from it further down. It is cleared by reset(), so a reset asked for
+	// through a service restarts at the guess frame while an automatic one continues from
+	// the pose it has just carried forward.
+	bool guessIsTheFirstOne = false;
 	if(!guessFrameId_.empty())
 	{
 		guessCurrentPose = rtabmap_conversions::getTransform(guessFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
 
 		Transform previousPose = guessPreviousPose_;
-		if(guessPreviousPose_.isNull())
+		guessIsTheFirstOne = guessPreviousPose_.isNull();
+		if(guessIsTheFirstOne)
 		{
 			previousPose = guessCurrentPose;
-			if(!guessCurrentPose.isNull() && odometry_->getPose().isIdentity())
-			{
-				RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s", guessCurrentPose.prettyPrint().c_str());
-				odometry_->reset(guessCurrentPose);
-			}
 		}
 
 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
+			// What the guess frame says the robot is doing. This is what gets published
+			// for a frame with no registration behind it -- one skipped for not having
+			// moved enough, or one starting a new map, whose twist would otherwise be
+			// unknown although its pose comes from the guess. It is dropped further down
+			// as soon as the registration has a velocity of its own to report.
+			if(previousStamp_ > 0.0 &&
+			   rtabmap_conversions::timestampFromROS(header.stamp) > previousStamp_)
+			{
+				guessVelocity = velocityFrom(previousPose.inverse() * guessCurrentPose,
+						rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_);
+			}
+
 			if(guess_.isNull())
 			{
 				guess_ = previousPose.inverse() * guessCurrentPose;
@@ -791,20 +833,7 @@ void OdometryROS::processData()
 				{
 					// Ignore odometry update, we didn't move enough
 					pose = odometry_->getPose() * guess_;
-					info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1);
-					info.reg.covariance.at<double>(0,0) = guessLinearVariance_;  // xx
-					info.reg.covariance.at<double>(1,1) = guessLinearVariance_;  // yy
-					info.reg.covariance.at<double>(2,2) = guessLinearVariance_; // zz
-					info.reg.covariance.at<double>(3,3) = guessAngularVariance_; // rr
-					info.reg.covariance.at<double>(4,4) = guessAngularVariance_; // pp
-					info.reg.covariance.at<double>(5,5) = guessAngularVariance_; // yawyaw
 
-					//set velocity
-					double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_;
-					UASSERT(dt>0.0);
-					// use part of guess matching dt
-					(previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-					guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt);
 					skipOdometryUpdate = true;
 				}
 			}
@@ -817,16 +846,117 @@ void OdometryROS::processData()
 		}
 	}
 
+	// Handled here rather than before the guess is computed: guess_ only holds the motion
+	// since the previous frame once the block above has run, and resetting without it
+	// throws away everything the guess source measured across the gap -- which is exactly
+	// what this reset is supposed to carry over.
+	bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_;
+	if(tooOldPreviousData)
+	{
+		RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset because last update "
+				"is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.",
+				rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp));
+
+		if(!guess_.isNull())
+		{
+			RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
+					guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
+			odometry_->reset(odometry_->getPose() * guess_);
+			// Cleared because it has just been applied: the odometry now starts from a
+			// pose that already includes it, and leaving it would have the registration
+			// apply it a second time on the frame that initialises the new map.
+			// guessPreviousPose_ is kept, so the next frame measures its motion from this
+			// one rather than starting over and losing a frame of it.
+			guess_.setNull();
+		}
+		else
+		{
+			// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
+			Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
+			if(tfPose.isNull())
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
+				odometry_->reset(odometry_->getPose());
+			}
+			else
+			{
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
+						odomFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(tfPose);
+			}
+		}
+	}
+
 	// process data
 	rclcpp::Time timeStart = rclcpp::Clock().now();
 	if(!groundTruth.isNull())
 	{
 		data.setGroundTruth(groundTruth);
 	}
+	// Set when the guess has already been folded into the pose below, so that a reset
+	// later in this frame does not go looking for a fallback that is no longer needed.
+	bool poseCarriedByGuess = false;
+	// Set when this frame starts a new map and the guess frame says where, which is what
+	// makes the trajectory it starts continuous with the one before it.
+	bool initialisedOnGuess = false;
 	if(!skipOdometryUpdate)
 	{
+		// This frame will initialise the odometry's map whenever no frame has been
+		// registered since the last reset -- at startup, after a service reset, or on
+		// recovery from an automatic one. Registration then returns no motion, so the pose
+		// has to be put where the guess says the robot is *before* the frame is processed:
+		// afterwards the map is already anchored in the wrong place, and the next
+		// registration measures the difference against that anchor and takes the correction
+		// straight back out. Resetting here costs nothing, the map being empty either way.
+		//
+		// There are two ways to be right, depending on what the guess can say:
+		initialisedOnGuess = odometry_->framesProcessed() == 0 && !guessCurrentPose.isNull();
+		if(initialisedOnGuess)
+		{
+			if(guessIsTheFirstOne)
+			{
+				// Nothing to be relative to. Adopt the guess source's own coordinates, so
+				// that odometry restarts where the guess says it is rather than at the
+				// origin. A pose asked for explicitly through reset_odom_to_pose is left
+				// alone: only an odometry still sitting at the identity is seeded this way.
+				if(odometry_->getPose().isIdentity())
+				{
+					RCLCPP_INFO(get_logger(), "Odometry: init pose with guess %s",
+							guessCurrentPose.prettyPrint().c_str());
+					odometry_->reset(guessCurrentPose);
+				}
+			}
+			else if(!guess_.isNull() && !guess_.isIdentity())
+			{
+				// There is a previous guess pose, so the guess describes real motion since
+				// the frame before this one -- which an automatic reset has just carried the
+				// pose through. Advance by it and the trajectory stays continuous; drop it
+				// and the new map is anchored a frame behind, once per reset, accumulating.
+				RCLCPP_DEBUG(this->get_logger(), "Odometry: advancing the pose by the guess "
+						"(%s) before the map is initialised, so the motion measured since the "
+						"previous frame is not lost.", guess_.prettyPrint().c_str());
+				odometry_->reset(odometry_->getPose() * guess_);
+				guess_.setNull();
+				poseCarriedByGuess = true;
+			}
+		}
 		pose = odometry_->process(data, guess_, &info);
 	}
+
+	// 9999 on both covariances is how rtabmap is told a frame starts a new map. When the
+	// guess frame says where it starts, and publish_null_when_lost says this consumer
+	// wants poses rather than the news of a reset, it goes out as a continuation instead.
+	const bool publishAsContinuation = initialisedOnGuess && !publishNullWhenLost_ && !pose.isNull();
+	if(skipOdometryUpdate || publishAsContinuation)
+	{
+		// Both rest on the guess rather than on a registration: its confidence, its velocity.
+		info.reg.covariance = guessCovariance(guessLinearVariance_, guessAngularVariance_);
+	}
+	else
+	{
+		// The registration measured this one, so its velocity is the one to publish.
+		guessVelocity.setNull();
+	}
 	if(!pose.isNull())
 	{
 		if(!skipOdometryUpdate) {
@@ -909,11 +1039,12 @@ void OdometryROS::processData()
 			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
-				if(skipOdometryUpdate) {
-					UASSERT(!guessVelocity.isNull());
-					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-				} else {
+				// Whatever is left of the two: the registration's own velocity, or the
+				// guess's where the frame had no registration to give one.
+				if(guessVelocity.isNull()) {
 					odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				} else {
+					guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 				}
 				odom.twist.twist.linear.x = x;
 				odom.twist.twist.linear.y = y;
@@ -931,7 +1062,7 @@ void OdometryROS::processData()
 			odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
 
 			//publish the message
-			if(setTwist || publishNullWhenLost_)
+			if(setTwist || publishNullWhenLost_ || publishAsContinuation)
 			{
 				odomPub_->publish(odom);
 			}
@@ -953,7 +1084,7 @@ void OdometryROS::processData()
 				cloud.push_back(pt);
 			}
 			sensor_msgs::msg::PointCloud2 cloudMsg;
-			pcl::toROSMsg(cloud, cloudMsg);
+			rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalMap_->publish(cloudMsg);
@@ -976,7 +1107,7 @@ void OdometryROS::processData()
 					}
 
 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -996,7 +1127,7 @@ void OdometryROS::processData()
 						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 					sensor_msgs::msg::PointCloud2 cloudMsg;
-					pcl::toROSMsg(cloud, cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(cloud, cloudMsg);
 					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_->publish(cloudMsg);
@@ -1010,22 +1141,22 @@ void OdometryROS::processData()
 			if(info.localScanMap.hasNormals() && info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasNormals())
 			{
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else if(info.localScanMap.hasIntensity())
 			{
 				pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = util3d::laserScanToPointCloudI(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 			else
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap, info.localScanMap.localTransform());
-				pcl::toROSMsg(*cloud, cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*cloud, cloudMsg);
 			}
 
 			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
@@ -1106,6 +1237,17 @@ void OdometryROS::processData()
 				odometry_->reset(odometry_->getPose() * guess_);
 				guess_.setNull();
 			}
+			else if(poseCarriedByGuess)
+			{
+				// The guess was folded into the pose before this frame was processed, so the
+				// pose already covers the motion since the last one. Going to TF for a
+				// fallback here would block for wait_for_transform on every lost frame and
+				// answer a question that has already been answered.
+				RCLCPP_WARN(this->get_logger(), "Odometry automatically reset, carrying the "
+						"latest guess from TF (%s->%s) that was already applied to the pose!",
+						guessFrameId_.c_str(), frameId_.c_str());
+				odometry_->reset(odometry_->getPose());
+			}
 			else
 			{
 				// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
@@ -1319,6 +1461,11 @@ void OdometryROS::reset(const Transform & pose)
 	UScopeMutex lock(dataMutex_);
 	odometry_->reset(pose);
 	guess_.setNull();
+	// Clearing this is what tells the next frame to restart from the guess frame rather
+	// than continue from here: the seeding step below cannot tell a reset asked for
+	// through a service from one the node decided on its own, and reads this instead. The
+	// automatic resets deliberately leave it alone, so that they carry on from the pose
+	// they just moved.
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	previousClockTime_ = 0.0;
diff --git a/rtabmap_odom/src/RGBDOdometryNode.cpp b/rtabmap_odom/src/RGBDOdometryNode.cpp
index 270d9ede..4a406446 100644
--- a/rtabmap_odom/src/RGBDOdometryNode.cpp
+++ b/rtabmap_odom/src/RGBDOdometryNode.cpp
@@ -71,7 +71,9 @@ int main(int argc, char **argv)
 	}
 
 #ifdef RTABMAP_PYTHON
-	rtabmap::PythonInterface pythonInterface;
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("rgbd_odometry");
 #endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
diff --git a/rtabmap_odom/src/StereoOdometryNode.cpp b/rtabmap_odom/src/StereoOdometryNode.cpp
index 60a729ce..ce4385e7 100644
--- a/rtabmap_odom/src/StereoOdometryNode.cpp
+++ b/rtabmap_odom/src/StereoOdometryNode.cpp
@@ -71,7 +71,9 @@ int main(int argc, char **argv)
 	}
 
 #ifdef RTABMAP_PYTHON
-	rtabmap::PythonInterface pythonInterface;
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("stereo_odometry");
 #endif
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp
index 990976ec..f67453c8 100644
--- a/rtabmap_odom/src/nodelets/icp_odometry.cpp
+++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_odom/icp_odometry.hpp>
 
 #include <laser_geometry/laser_geometry.hpp>
@@ -69,6 +70,7 @@ ICPOdometry::ICPOdometry(const rclcpp::NodeOptions & options) :
 
 ICPOdometry::~ICPOdometry()
 {
+	this->join(true);
 }
 
 void ICPOdometry::onOdomInit()
@@ -313,7 +315,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 				scanMsg->header.frame_id,
 				guessFrameId().empty()?frameId():guessFrameId(),
 				scanMsg->header.stamp,
-				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(scanMsg->ranges.size()*scanMsg->time_increment),
+				rclcpp::Time(scanMsg->header.stamp.sec, scanMsg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((scanMsg->ranges.empty()?0:scanMsg->ranges.size()-1)*scanMsg->time_increment),
 				this->tfBuffer(),
 				this->waitForTransform());
 		if(tmpT.isNull())
@@ -333,7 +335,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model (we are in frameId)
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -362,7 +364,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -399,12 +401,12 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 
 	if(hasIntensity)
 	{
-		pcl::fromROSMsg(scanOut, *pclScanI);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScanI);
 		pclScanI->is_dense = true;
 	}
 	else
 	{
-		pcl::fromROSMsg(scanOut, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 	}
 
@@ -519,7 +521,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr pointCloudMsg)
 {
 	UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
-			uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
+			uFormat("data=%d row_step=%d height=%d", (int)pointCloudMsg->data.size(), (int)pointCloudMsg->row_step, (int)pointCloudMsg->height).c_str());
 
 	if(scanReceived_)
 	{
@@ -583,7 +585,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			}
 
 			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
-			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -668,7 +670,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	if(hasNormals && hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -686,7 +688,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasNormals)
 	{
 		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -704,7 +706,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else if(hasIntensity)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
@@ -750,7 +752,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 	else
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::fromROSMsg(*cloudMsg, *pclScan);
+		rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *pclScan);
 		if(pclScan->size() && scanDownsamplingStep_ > 1)
 		{
 			pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp
index 97d48f6e..5f43acab 100644
--- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_conversions/MsgConversion.h"
 #include <rtabmap_msgs/msg/rgbd_images.hpp>
 
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -73,6 +74,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
 
 RGBDOdometry::~RGBDOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -435,40 +438,58 @@ void RGBDOdometry::updateParameters(ParametersMap & parameters)
 void RGBDOdometry::commonCallback(
 			const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
 			const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
-			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos)
+			const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+			const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
 	rclcpp::Time higherStamp;
 	UASSERT_MSG(rgbImages[0], "RGB image is null!");
-	int imageWidth = rgbImages[0]->image.cols;
-	int imageHeight = rgbImages[0]->image.rows;
 	UASSERT_MSG(depthImages[0], "Depth image is null!");
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the image would have been,
+	// where the camera is, what it sees. (An RGB-D message with no depth image at all also
+	// used to divide by zero below.)
+	const bool hasRgb = !rgbImages[0]->image.empty();
+	const bool hasDepth = !depthImages[0]->image.empty();
+
+	int imageWidth = hasRgb?rgbImages[0]->image.cols:(int)cameraInfos[0].width;
+	int imageHeight = hasRgb?rgbImages[0]->image.rows:(int)cameraInfos[0].height;
 	int depthWidth = depthImages[0]->image.cols;
 	int depthHeight = depthImages[0]->image.rows;
 
-	UASSERT_MSG(
-			imageWidth/depthWidth == imageHeight/depthHeight,
-			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	if(hasDepth)
+	{
+		UASSERT_MSG(
+				imageWidth/depthWidth == imageHeight/depthHeight,
+				uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	}
 
 	int cameraCount = rgbImages.size();
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<rtabmap::CameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<rgbImages.size(); ++i)
 	{
 		UASSERT_MSG(rgbImages[i], uFormat("RGB image is null for camera %d", i).c_str());
 		UASSERT_MSG(depthImages[i], uFormat("Depth image is null for camera %d", i).c_str());
-		if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if((hasRgb && !(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
-			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
-			!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0)) ||
+			(hasDepth && !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+			 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
 			"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
@@ -476,20 +497,33 @@ void RGBDOdometry::commonCallback(
 				depthImages[i]->encoding.c_str());
 			return;
 		}
-		UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
-				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
-						imageWidth,
-						rgbImages[i]->image.cols,
-						imageHeight,
-						rgbImages[i]->image.rows).c_str());
-		UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
-				uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
-						depthWidth,
-						depthImages[i]->image.cols,
-						depthHeight,
-						depthImages[i]->image.rows).c_str());
+		if(hasRgb)
+		{
+			UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
+					uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+							imageWidth,
+							rgbImages[i]->image.cols,
+							imageHeight,
+							rgbImages[i]->image.rows).c_str());
+		}
+		if(hasDepth)
+		{
+			UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
+					uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
+							depthWidth,
+							depthImages[i]->image.cols,
+							depthHeight,
+							depthImages[i]->image.rows).c_str());
+		}
 
-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasRgb?rgbImages[i]->header.frame_id:cameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = cameraInfos[i].header.stamp;
+		if(hasRgb || hasDepth)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(rgbImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(depthImages[i]->header.stamp)?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
+		}
 
 		if(i == 0)
 		{
@@ -500,7 +534,7 @@ void RGBDOdometry::commonCallback(
 			higherStamp = stamp;
 		}
 
-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -528,53 +562,75 @@ void RGBDOdometry::commonCallback(
 			}
 		}
 
-		cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
-		if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-		   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+		if(hasRgb)
 		{
-			if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+			cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
+			if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+			   rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				}
+			}
+
+			// initialize
+			if(rgb.empty())
+			{
+				rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+			}
+
+			if(ptrImage->image.type() == rgb.type())
+			{
+				ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
 			}
 			else
 			{
-				ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
+				RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
+				return;
 			}
 		}
 
-		cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+		if(hasDepth)
+		{
+			cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
+			if(depth.empty())
+			{
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
+			}
 
-		// initialize
-		if(rgb.empty())
-		{
-			rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
-		}
-		if(depth.empty())
-		{
-			depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
-		}
-
-		if(ptrImage->image.type() == rgb.type())
-		{
-			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
-			return;
-		}
-
-		if(ptrDepth->image.type() == depth.type())
-		{
-			ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
-			return;
+			if(ptrDepth->image.type() == depth.type())
+			{
+				ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
+				return;
+			}
 		}
 
 		cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
+
+		// The images of all cameras are stitched side by side above, so the keypoints of
+		// camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, imageWidth*i);
+		}
+		if(localPoints3dMsgs.size() == rgbImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == rgbImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
+		}
 	}
 
 	rtabmap::SensorData data(
@@ -584,9 +640,30 @@ void RGBDOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));
 
+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the depth lookup that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
+	header.frame_id = rgbImages.size()==1?(hasRgb?rgbImages[0]->header.frame_id:cameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }
 
@@ -627,6 +704,27 @@ void RGBDOdometry::callback(
 	}
 }
 
+namespace {
+
+/**
+ * @brief Collects the local features one camera's image carries, cameras in order.
+ *
+ * An image that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void RGBDOdometry::callbackRGBDX(
 		const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
 {
@@ -642,13 +740,17 @@ void RGBDOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
 		std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
 			infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -665,7 +767,12 @@ void RGBDOdometry::callbackRGBD(
 		rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
 		infoMsgs.push_back(image->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -685,7 +792,13 @@ void RGBDOdometry::callbackRGBD2(
 		infoMsgs.push_back(image->rgb_camera_info);
 		infoMsgs.push_back(image2->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -708,7 +821,14 @@ void RGBDOdometry::callbackRGBD3(
 		infoMsgs.push_back(image2->rgb_camera_info);
 		infoMsgs.push_back(image3->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -734,7 +854,15 @@ void RGBDOdometry::callbackRGBD4(
 		infoMsgs.push_back(image3->rgb_camera_info);
 		infoMsgs.push_back(image4->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -763,7 +891,16 @@ void RGBDOdometry::callbackRGBD5(
 		infoMsgs.push_back(image4->rgb_camera_info);
 		infoMsgs.push_back(image5->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -795,7 +932,17 @@ void RGBDOdometry::callbackRGBD6(
 		infoMsgs.push_back(image5->rgb_camera_info);
 		infoMsgs.push_back(image6->rgb_camera_info);
 
-		this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(imageMsgs, depthMsgs, infoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp
index f56b2cc3..72cd9cb0 100644
--- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp
+++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/Odometry.h>
 
 using namespace rtabmap;
@@ -73,6 +74,8 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
 
 StereoOdometry::~StereoOdometry()
 {
+	this->join(true);
+
 	delete approxSync_;
 	delete exactSync_;
 	delete approxSync2_;
@@ -407,29 +410,46 @@ void StereoOdometry::commonCallback(
 		const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
 		const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
 		const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
-		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
+		const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos,
+		const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
+		const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
+		const std::vector<cv::Mat> & localDescriptorsMsgs)
 {
 	UASSERT(leftImages.size() > 0 &&
 			leftImages.size() == rightImages.size() &&
 			leftImages.size() == leftCameraInfos.size() &&
 			rightImages.size() == rightCameraInfos.size());
 	rclcpp::Time higherStamp;
-	int leftWidth = leftImages[0]->image.cols;
-	int leftHeight = leftImages[0]->image.rows;
+
+	// The images are what the local features would otherwise be extracted from, so a frame
+	// that brings its own can leave them out -- which is nearly all of the bandwidth. It
+	// then describes itself with its calibration alone: how big the left image would have
+	// been, where the rig is, how far apart the two cameras are.
+	const bool hasImages = !leftImages[0]->image.empty() && !rightImages[0]->image.empty();
+
+	int leftWidth = hasImages?leftImages[0]->image.cols:(int)leftCameraInfos[0].width;
+	int leftHeight = hasImages?leftImages[0]->image.rows:(int)leftCameraInfos[0].height;
 	int rightWidth = rightImages[0]->image.cols;
 	int rightHeight = rightImages[0]->image.rows;
 
-	UASSERT_MSG(
-			leftWidth == rightWidth && leftHeight == rightHeight,
-		uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	if(hasImages)
+	{
+		UASSERT_MSG(
+				leftWidth == rightWidth && leftHeight == rightHeight,
+			uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
+	}
 
 	int cameraCount = leftImages.size();
 	cv::Mat left;
 	cv::Mat right;
 	std::vector<rtabmap::StereoCameraModel> cameraModels;
+	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> points3d;
+	cv::Mat descriptors;
 	for(unsigned int i=0; i<leftImages.size(); ++i)
 	{
-		if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+		if(hasImages &&
+		   (!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
@@ -442,14 +462,21 @@ void StereoOdometry::commonCallback(
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
+			  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)))
 		{
 			RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
 					leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
 			return;
 		}
 
-		rclcpp::Time stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		// An image that is not there carries no header either, so a frame that has none is
+		// stamped and placed by its calibration, which is all it has.
+		const std::string & cameraFrameId = hasImages?leftImages[i]->header.frame_id:leftCameraInfos[i].header.frame_id;
+		rclcpp::Time stamp = leftCameraInfos[i].header.stamp;
+		if(hasImages)
+		{
+			stamp = rtabmap_conversions::timestampFromROS(leftImages[i]->header.stamp)>rtabmap_conversions::timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
+		}
 
 		if(i == 0)
 		{
@@ -460,7 +487,7 @@ void StereoOdometry::commonCallback(
 			higherStamp = stamp;
 		}
 
-		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
+		Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), cameraFrameId, stamp, tfBuffer(), waitForTransform());
 		if(localTransform.isNull())
 		{
 			return;
@@ -488,7 +515,13 @@ void StereoOdometry::commonCallback(
 			}
 		}
 
-		if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
+		if(hasImages != (!leftImages[i]->image.empty() && !rightImages[i]->image.empty()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Odom: camera %d of this frame has images while "
+					"another one doesn't (or the other way around)?!?", i);
+			return;
+		}
+
 		{
 			bool alreadyRectified = true;
 			Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -602,62 +635,76 @@ void StereoOdometry::commonCallback(
 					shown = true;
 				}
 			}
-			cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
-			if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+			if(hasImages)
 			{
-				if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+				cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
+				if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
+					}
+					else
+					{
+						ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					}
+				}
+				cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
+				if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
+				   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
+				{
+					ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
+				}
+
+				// initialize
+				if(left.empty())
+				{
+					left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
+				}
+				if(right.empty())
+				{
+					right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
+				}
+
+				if(ptrLeft->image.type() == left.type())
+				{
+					ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
 				}
 				else
 				{
-					ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
+					RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
+					return;
 				}
-			}
-			cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
-			if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
-			   rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
-			{
-				ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
-			}
 
-			// initialize
-			if(left.empty())
-			{
-				left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
-			}
-			if(right.empty())
-			{
-				right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
-			}
-
-			if(ptrLeft->image.type() == left.type())
-			{
-				ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
-				return;
-			}
-
-			if(ptrRight->image.type() == right.type())
-			{
-				ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
-			}
-			else
-			{
-				RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
-				return;
+				if(ptrRight->image.type() == right.type())
+				{
+					ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
+				}
+				else
+				{
+					RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
+					return;
+				}
 			}
 
 			cameraModels.push_back(stereoModel);
 		}
-		else
+
+		// The left images of all cameras are stitched side by side above, so the keypoints
+		// of camera i are shifted by as many images as come before it, and their 3D points,
+		// which arrive in that camera's optical frame, are brought back to the base frame.
+		if(localKeyPointsMsgs.size() == leftImages.size())
 		{
-			RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
-			return;
+			rtabmap_conversions::keypointsFromROS(localKeyPointsMsgs[i], keypoints, leftWidth*i);
+		}
+		if(localPoints3dMsgs.size() == leftImages.size())
+		{
+			rtabmap_conversions::points3fFromROS(localPoints3dMsgs[i], points3d, localTransform);
+		}
+		if(localDescriptorsMsgs.size() == leftImages.size())
+		{
+			descriptors.push_back(localDescriptorsMsgs[i]);
 		}
 	}
 
@@ -669,9 +716,30 @@ void StereoOdometry::commonCallback(
 			0,
 			rtabmap_conversions::timestampFromROS(higherStamp));
 
+	// Features that came with the frame are used as they are: the odometry then skips
+	// detection, description and the disparity search that would otherwise rebuild them
+	// (see RegistrationVis, which extracts only when the frame carries no keypoints).
+	// They are dropped rather than trusted if the three of them disagree, as using them
+	// out of step would silently mismatch keypoints with their descriptors or 3D points.
+	if(!keypoints.empty())
+	{
+		if((!points3d.empty() && points3d.size() != keypoints.size()) ||
+		   (!descriptors.empty() && descriptors.rows != (int)keypoints.size()))
+		{
+			RCLCPP_ERROR(this->get_logger(), "Ignoring the local features received with this frame: "
+					"%d keypoints, %d 3D points and %d descriptors, which should be the same count "
+					"(or none at all for the 3D points and the descriptors).",
+					(int)keypoints.size(), (int)points3d.size(), descriptors.rows);
+		}
+		else
+		{
+			data.setFeatures(keypoints, points3d, descriptors);
+		}
+	}
+
 	std_msgs::msg::Header header;
 	header.stamp = higherStamp;
-	header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
+	header.frame_id = leftImages.size()==1?(hasImages?leftImages[0]->header.frame_id:leftCameraInfos[0].header.frame_id):"";
 	this->processData(data, header);
 }
 
@@ -715,6 +783,27 @@ void StereoOdometry::callback(
 	}
 }
 
+namespace {
+
+/**
+ * @brief Collects the local features one camera's frame carries, cameras in order.
+ *
+ * A frame that carries none pushes empty entries rather than nothing, so that the
+ * per-camera indexing still lines up with the images.
+ */
+void appendLocalFeatures(
+		const rtabmap_msgs::msg::RGBDImage & image,
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & keyPoints,
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & points3d,
+		std::vector<cv::Mat> & descriptors)
+{
+	keyPoints.push_back(image.key_points);
+	points3d.push_back(image.points);
+	descriptors.push_back(rtabmap::uncompressData(image.descriptors));
+}
+
+}  // namespace
+
 void StereoOdometry::callbackRGBD(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
 {
@@ -730,7 +819,12 @@ void StereoOdometry::callbackRGBD(
 		leftInfoMsgs.push_back(image->rgb_camera_info);
 		rightInfoMsgs.push_back(image->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -750,14 +844,18 @@ void StereoOdometry::callbackRGBDX(
 		std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
 		std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
 		std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
 		for(size_t i=0; i<images->rgbd_images.size(); ++i)
 		{
 			rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
 			leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
 			rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
+			appendLocalFeatures(images->rgbd_images[i], localKeyPoints, localPoints3d, localDescriptors);
 		}
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -780,7 +878,13 @@ void StereoOdometry::callbackRGBD2(
 		rightInfoMsgs.push_back(image->depth_camera_info);
 		rightInfoMsgs.push_back(image2->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -807,7 +911,14 @@ void StereoOdometry::callbackRGBD3(
 		rightInfoMsgs.push_back(image2->depth_camera_info);
 		rightInfoMsgs.push_back(image3->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -838,7 +949,15 @@ void StereoOdometry::callbackRGBD4(
 		rightInfoMsgs.push_back(image3->depth_camera_info);
 		rightInfoMsgs.push_back(image4->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -873,7 +992,16 @@ void StereoOdometry::callbackRGBD5(
 		rightInfoMsgs.push_back(image4->depth_camera_info);
 		rightInfoMsgs.push_back(image5->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
@@ -912,7 +1040,17 @@ void StereoOdometry::callbackRGBD6(
 		rightInfoMsgs.push_back(image5->depth_camera_info);
 		rightInfoMsgs.push_back(image6->depth_camera_info);
 
-		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
+		std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > localKeyPoints;
+		std::vector<std::vector<rtabmap_msgs::msg::Point3f> > localPoints3d;
+		std::vector<cv::Mat> localDescriptors;
+		appendLocalFeatures(*image, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image2, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image3, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image4, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image5, localKeyPoints, localPoints3d, localDescriptors);
+		appendLocalFeatures(*image6, localKeyPoints, localPoints3d, localDescriptors);
+
+		this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs, localKeyPoints, localPoints3d, localDescriptors);
 	}
 }
 
diff --git a/rtabmap_odom/test/bag_playback.hpp b/rtabmap_odom/test/bag_playback.hpp
new file mode 100644
index 00000000..aa636031
--- /dev/null
+++ b/rtabmap_odom/test/bag_playback.hpp
@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+
+#include <rclcpp/serialization.hpp>
+#include <rosbag2_cpp/reader.hpp>
+
+#include <string>
+#include <vector>
+
+#include "test_data.hpp"
+
+/**
+ * @file
+ * @brief Reads recorded messages out of a bag, for tests that need real sensor input.
+ *
+ * The messages are read and replayed by the test itself rather than by `ros2 bag play`:
+ * no second process, no wall-clock pacing, and the test controls exactly when each
+ * message reaches the node. What the recording provides is the part that cannot be
+ * written by hand -- a real driver's cloud layout and a dense TF history around it.
+ */
+
+namespace rtabmap_odom_test {
+
+/**
+ * @brief The Ouster recording in test/data/lidar; see that directory's README.
+ *
+ * Two sweeps half a mast turn apart, from a platform that never moves: the only thing
+ * between them is the mast's rotation, which TF describes in full.
+ */
+inline std::string ousterHalfTurnBag()
+{
+	return testDataRoot() + "/lidar/ouster_half_turn";
+}
+
+/**
+ * @brief Every message recorded on @p topic, deserialized.
+ *
+ * Returns an empty vector if the bag or the topic is missing, which the caller is
+ * expected to assert on -- a silently empty fixture would make a test pass for the wrong
+ * reason.
+ */
+template <typename MsgT>
+std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
+{
+	std::vector<MsgT> messages;
+	rosbag2_cpp::Reader reader;
+	try
+	{
+		reader.open(bagPath);
+	}
+	catch(const std::exception & e)
+	{
+		return messages;
+	}
+
+	rclcpp::Serialization<MsgT> serialization;
+	while(reader.has_next())
+	{
+		const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
+		if(message->topic_name != topic)
+		{
+			continue;
+		}
+		rclcpp::SerializedMessage serialized(*message->serialized_data);
+		MsgT deserialized;
+		serialization.deserialize_message(&serialized, &deserialized);
+		messages.push_back(deserialized);
+	}
+	return messages;
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */
diff --git a/rtabmap_odom/test/camera_rig.hpp b/rtabmap_odom/test/camera_rig.hpp
new file mode 100644
index 00000000..77979a07
--- /dev/null
+++ b/rtabmap_odom/test/camera_rig.hpp
@@ -0,0 +1,319 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_CAMERA_RIG_HPP_
+#define RTABMAP_ODOM_CAMERA_RIG_HPP_
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/util3d_transforms.h>
+
+#include <opencv2/core/core.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <random>
+#include <string>
+#include <vector>
+
+#include "msg_builders.hpp"
+
+/**
+ * @file
+ * @brief A camera rig in a world of points, for the multi-camera odometry tests.
+ *
+ * Several cameras looking outward from one body is the case that cannot be covered with
+ * recorded frames: it needs a calibrated rig, a scene all of them can see, and ground
+ * truth for where the body went. So the scene is made rather than recorded -- points
+ * scattered around the rig, projected into each camera at each pose along a known
+ * trajectory, which is the approach of RTAB-Map's own multi-camera tests
+ * (`EstimateMotion3DTo2DMultiCam*` in corelib/test/test_util3d_motion_estimation.cpp and
+ * the four-camera rig of test_optimizer.cpp).
+ *
+ * What comes out is the *features*, not imagery: keypoints, their 3D positions and a
+ * descriptor per point, which is what a camera driver doing its own feature extraction
+ * would publish on `rgbd_images`, and all it needs to publish. The frames carry no image
+ * at all, so a run that recovers the trajectory can only have done it from them.
+ */
+
+namespace rtabmap_odom_test {
+
+/// Descriptors are matched between frames, so the same point has to keep the same one.
+const int kRigDescriptorSize = 32;
+
+/**
+ * @brief Cameras looking outward from one body, and the points they see.
+ *
+ * The cameras are spread evenly around the body and mounted on its rim, so opposite
+ * cameras are a real distance apart rather than sharing an optical centre.
+ */
+struct CameraRig
+{
+	int width = 0;
+	int height = 0;
+	double fx = 0.0;
+
+	/// base_link -> camera i, optical frame, in the order the cameras are published.
+	std::vector<rtabmap::Transform> localTransforms;
+	std::vector<std::string> frameIds;
+
+	/// The world: points in the odometry frame, and one descriptor row per point.
+	std::vector<cv::Point3f> points;
+	cv::Mat descriptors;
+
+	size_t cameras() const { return localTransforms.size(); }
+
+	/// Camera @p i as RTAB-Map sees it, intrinsics and mounting together.
+	rtabmap::CameraModel model(size_t i) const
+	{
+		return rtabmap::CameraModel(fx, fx, width/2.0, height/2.0,
+				localTransforms[i], 0, cv::Size(width, height));
+	}
+};
+
+/**
+ * @brief A rig of @p cameras cameras in a world of @p numPoints points.
+ *
+ * The horizontal field of view is 360/cameras degrees, up to 90, so the cameras tile as
+ * much of the circle as they can without overlapping: a point is then seen by at most one
+ * of them, which keeps every descriptor unique within a frame. Two cameras seeing the same
+ * point would put two identical descriptors in the same frame, and the ratio test that
+ * accepts a match only when the best candidate is clearly better than the second would
+ * throw both away.
+ *
+ * The points sit in a box wider than the trajectory, minus a hole around it: something
+ * closer than @p minRange would swing through a camera's field of view, or behind it,
+ * over the course of the run.
+ */
+inline CameraRig makeCameraRig(
+		int cameras = 4,
+		int numPoints = 300,
+		float boxXY = 8.0f,
+		float boxZ = 1.5f,
+		float minRange = 2.5f,
+		int width = 160,
+		int height = 120,
+		float rimRadius = 0.175f,
+		float rimHeight = 0.05f,
+		uint32_t seed = 7)
+{
+	CameraRig rig;
+	rig.width = width;
+	rig.height = height;
+	// Half the horizontal field of view spans half the angle between two cameras, capped
+	// at 45 degrees: one or two cameras would otherwise be asked for a 360 or 180 degree
+	// view, which no pinhole model has. The cap only widens the gaps between cameras, so
+	// a point is still seen by at most one of them.
+	rig.fx = (width/2.0) / std::tan(std::min(M_PI/double(cameras), M_PI/4.0));
+
+	for(int i=0; i<cameras; ++i)
+	{
+		const float yaw = 2.0f*float(M_PI)*float(i)/float(cameras);
+		rig.localTransforms.push_back(
+				rtabmap::Transform(rimRadius*std::cos(yaw), rimRadius*std::sin(yaw), rimHeight,
+						0.0f, 0.0f, yaw)
+				* rtabmap::CameraModel::opticalRotation());
+		rig.frameIds.push_back("camera" + std::to_string(i));
+	}
+
+	std::mt19937 rng(seed);
+	std::uniform_real_distribution<float> distXY(-boxXY, boxXY);
+	std::uniform_real_distribution<float> distZ(-boxZ, boxZ);
+	std::normal_distribution<float> distDescriptor(0.0f, 1.0f);
+
+	rig.descriptors = cv::Mat(numPoints, kRigDescriptorSize, CV_32FC1);
+	for(int i=0; i<numPoints; )
+	{
+		const cv::Point3f point(distXY(rng), distXY(rng), distZ(rng));
+		if(std::sqrt(point.x*point.x + point.y*point.y) < minRange)
+		{
+			continue;
+		}
+		rig.points.push_back(point);
+		for(int c=0; c<kRigDescriptorSize; ++c)
+		{
+			rig.descriptors.at<float>(i, c) = distDescriptor(rng);
+		}
+		++i;
+	}
+	return rig;
+}
+
+/// The mounting of each camera, as the rig's driver would publish it on /tf_static.
+inline std::vector<geometry_msgs::msg::TransformStamped> cameraRigTransforms(
+		const CameraRig & rig, const rclcpp::Time & stamp,
+		const std::string & baseFrame = "base_link")
+{
+	std::vector<geometry_msgs::msg::TransformStamped> transforms;
+	for(size_t i=0; i<rig.cameras(); ++i)
+	{
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = stamp;
+		tf.header.frame_id = baseFrame;
+		tf.child_frame_id = rig.frameIds[i];
+		rtabmap_conversions::transformToGeometryMsg(rig.localTransforms[i], tf.transform);
+		transforms.push_back(tf);
+	}
+	return transforms;
+}
+
+/// What one camera of the rig saw: its keypoints, their 3D points and their descriptors.
+struct RigObservations
+{
+	std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > keyPoints;
+	std::vector<std::vector<rtabmap_msgs::msg::Point3f> > points;
+	std::vector<cv::Mat> descriptors;
+};
+
+/**
+ * @brief What the rig sees from @p pose, one entry per camera.
+ *
+ * Each point is given to the first camera that has it in view, so no point is reported
+ * twice. The keypoints are in their own camera's image, the 3D points in their own
+ * camera's optical frame, and the descriptors in the order of the keypoints -- which is
+ * how a driver publishes them, and what the node has to reassemble.
+ */
+inline RigObservations observeCameraRig(const CameraRig & rig, const rtabmap::Transform & pose)
+{
+	const size_t cameras = rig.cameras();
+	std::vector<rtabmap::CameraModel> models;
+	std::vector<rtabmap::Transform> worldToCamera;
+	for(size_t i=0; i<cameras; ++i)
+	{
+		models.push_back(rig.model(i));
+		worldToCamera.push_back((pose * rig.localTransforms[i]).inverse());
+	}
+
+	RigObservations seen;
+	seen.keyPoints.resize(cameras);
+	seen.points.resize(cameras);
+	seen.descriptors.resize(cameras);
+
+	for(size_t p=0; p<rig.points.size(); ++p)
+	{
+		for(size_t i=0; i<cameras; ++i)
+		{
+			const cv::Point3f inCamera =
+					rtabmap::util3d::transformPoint(rig.points[p], worldToCamera[i]);
+			if(inCamera.z <= 0.1f)
+			{
+				continue;
+			}
+			float u = 0.0f;
+			float v = 0.0f;
+			models[i].reproject(inCamera.x, inCamera.y, inCamera.z, u, v);
+			// Truncation would let a point just off the left or top edge through, at a
+			// negative pixel the odometry drops later, so the bounds are checked directly.
+			if(u < 0.0f || v < 0.0f || u >= float(rig.width) || v >= float(rig.height))
+			{
+				continue;
+			}
+
+			rtabmap_msgs::msg::KeyPoint keyPoint;
+			keyPoint.pt.x = u;
+			keyPoint.pt.y = v;
+			keyPoint.size = 3;
+			keyPoint.response = 1.0f;
+			seen.keyPoints[i].push_back(keyPoint);
+
+			rtabmap_msgs::msg::Point3f point;
+			point.x = inCamera.x;
+			point.y = inCamera.y;
+			point.z = inCamera.z;
+			seen.points[i].push_back(point);
+
+			seen.descriptors[i].push_back(rig.descriptors.row(int(p)));
+			break;
+		}
+	}
+	return seen;
+}
+
+/**
+ * @brief The rig's observations from @p pose as RGB-D frames, one per camera.
+ *
+ * @p withImages attaches a blank image to each camera. There is nothing to find in it,
+ * but the odometry only takes the paths that touch images when one is there.
+ */
+inline rtabmap_msgs::msg::RGBDImages cameraRigFrame(
+		const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
+		bool withImages = false)
+{
+	const RigObservations seen = observeCameraRig(rig, pose);
+
+	rtabmap_msgs::msg::RGBDImages msg;
+	msg.header.stamp = stampOf(stamp);
+	msg.header.frame_id = rig.frameIds[0];
+	for(size_t i=0; i<rig.cameras(); ++i)
+	{
+		rtabmap_msgs::msg::RGBDImage image;
+		image.header.stamp = msg.header.stamp;
+		image.header.frame_id = rig.frameIds[i];
+		// No image at all by default, neither color nor depth: this frame is its
+		// calibration and its features, which is everything a camera doing its own
+		// extraction has to send.
+		if(withImages)
+		{
+			image.rgb = makeImage(rig.frameIds[i], stamp,
+					cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
+		}
+		image.rgb_camera_info = makeCameraInfo(
+				rig.frameIds[i], stamp, rig.width, rig.height, 0.0, rig.fx);
+		image.depth_camera_info = image.rgb_camera_info;
+		image.key_points = seen.keyPoints[i];
+		image.points = seen.points[i];
+		image.descriptors = rtabmap::compressData(seen.descriptors[i]);
+		msg.rgbd_images.push_back(image);
+	}
+	return msg;
+}
+
+/**
+ * @brief The same observations as stereo frames: left camera plus a right one @p baseline
+ * to its side.
+ *
+ * `RGBDImage` carries a stereo pair as its color and depth fields, so this differs from
+ * the RGB-D frames above only in the second calibration, whose `P(0,3)` is what tells the
+ * node how far apart the two cameras are. The features belong to the left image either
+ * way, which is where a stereo pipeline finds them.
+ */
+inline rtabmap_msgs::msg::RGBDImages cameraRigStereoFrame(
+		const CameraRig & rig, const rtabmap::Transform & pose, double stamp,
+		double baseline = 0.12, bool withImages = false)
+{
+	rtabmap_msgs::msg::RGBDImages msg = cameraRigFrame(rig, pose, stamp, withImages);
+	for(size_t i=0; i<msg.rgbd_images.size(); ++i)
+	{
+		msg.rgbd_images[i].depth_camera_info = makeCameraInfo(
+				rig.frameIds[i], stamp, rig.width, rig.height, -baseline*rig.fx, rig.fx);
+		if(withImages)
+		{
+			msg.rgbd_images[i].depth = makeImage(rig.frameIds[i], stamp,
+					cv::Mat::zeros(rig.height, rig.width, CV_8UC1), "mono8");
+		}
+	}
+	return msg;
+}
+
+/// How many features @p frame carries, all cameras together.
+inline size_t cameraRigFeatureCount(const rtabmap_msgs::msg::RGBDImages & frame)
+{
+	size_t count = 0;
+	for(size_t i=0; i<frame.rgbd_images.size(); ++i)
+	{
+		count += frame.rgbd_images[i].key_points.size();
+	}
+	return count;
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_CAMERA_RIG_HPP_ */
diff --git a/rtabmap_odom/test/data/README.md b/rtabmap_odom/test/data/README.md
new file mode 100644
index 00000000..4981134e
--- /dev/null
+++ b/rtabmap_odom/test/data/README.md
@@ -0,0 +1,103 @@
+# Test data
+
+Real frames for the odometry node tests, so that they register actual imagery instead of
+synthetic noise. Synthetic textures give a detector corners that match nothing between
+frames, which makes a "motion" that only proves the node did not crash.
+
+Everything here is BSD-3-Clause, same authors and same terms as the rest of this
+repository.
+
+| Path | Origin | Used by |
+| --- | --- | --- |
+| `stereo/rect/{left,right}/{50,60}.jpg` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/rect/stereo_{left,right}.yaml` | [RTAB-Map `data/stereo_rect`](https://github.com/introlab/rtabmap/tree/master/data/stereo_rect) | `test_stereo_odometry.cpp` |
+| `stereo/raw/{left,right}/{420,425}.jpg` | frames 420 and 425 (21.00 s and 21.25 s at 20 Hz) of the [stereo indoor tutorial](https://github.com/introlab/rtabmap/wiki/Stereo-mapping) test sequence | `test_stereo_odometry.cpp` |
+| `stereo/raw/stereo_{left,right}.yaml`, `stereo/raw/stereo_pose.yaml` | that rig's own calibration (`stereo_tutorial_*`) | `test_stereo_odometry.cpp` |
+| `lidar/ouster_half_turn/` | frames 1613418430.682 and 1613418435.082 of an Ouster recording on a rotating mast | `test_icp_odometry.cpp` |
+| `rgbd/rgb/{17,154}.jpg`, `rgbd/depth/{17,154}.png` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+| `rgbd/calib/{17,154}.yaml` | [RTAB-Map `data/rgbd`](https://github.com/introlab/rtabmap/tree/master/data/rgbd) | `test_rgbd_odometry.cpp` |
+
+They are vendored rather than read from an RTAB-Map checkout because RTAB-Map reaches us
+as an installed library: the `introlab3it/rtabmap` images used by CI delete the source tree
+after `make install`, and the ROS buildfarm has no network during a build. A copy here is
+what makes these tests run everywhere rather than skip.
+
+## What the frames are
+
+Two frames of each kind is the minimum that says anything: the first initialises the
+odometry at the origin, the second has to be registered against it.
+
+- **`stereo/rect`** -- `50` and `60`, two rectified pairs of the same scene a short motion
+  apart. The estimate comes out at 0.171 m, steady to well under a centimetre across runs.
+  These back `corelib/test/test_odometry.cpp` upstream, so a failure here that also fails
+  there is an RTAB-Map issue rather than a ROS one.
+- **`stereo/raw`** -- `420` and `425`, an unrectified pair a quarter second apart, for the
+  `Rtabmap/ImagesAlreadyRectified:=false` path described in `doc/stereo_odometry.md`, and
+  for pinning what happens when that parameter is left at its default on distorted images.
+  A quarter second of walking forward, ~0.233 m, reproduced to a fraction of a percent
+  across runs. Wider gaps in the same window register too, but not reliably: 21.00 s to
+  22.00 s is ~1.04 m at around 45 inliers and lost tracking outright in one run out of
+  eight, where this pair holds 210 or more.
+- **`lidar/ouster_half_turn`** -- two Ouster sweeps 4.40 s apart, recorded as a ROS 2 mcap
+  bag (`/tf`, `/tf_static`, `/os_cloud_node/points`) rather than as loose files, because
+  what makes them worth keeping is the TF history around them.
+
+  The sensor sits on a mast turning at ~42 deg/s while the platform stays put, so it moves
+  through its own 0.1 s sweep and the cloud comes off the driver skewed. The two sweeps are
+  half a turn apart (184.5 deg), which puts the skew in opposite directions and makes a
+  missing correction obvious. And because the platform never moves -- `base_link` ->
+  `box_link` does not translate by a single millimetre over the whole recording -- the
+  answer is known: the transform between the two is the identity. That is what the test
+  measures against, rather than a value taken from a previous run.
+
+  TF runs from 0.1 s before each sweep to 0.1 s past its end, with nothing in between: the
+  gap holds transforms nobody looks up, and keeping them would have tripled the file.
+- **`rgbd`** -- `17` and `154`, two frames of a hand-held Kinect sequence, far enough apart
+  that losing tracking between them is a legitimate outcome.
+
+In every set the left image is color and the right one grayscale, as the cameras recorded
+them.
+
+Both stereo sets come from the same 640x480 rig, but **not** from the same calibration, and
+the two are not interchangeable:
+
+| | `stereo/rect` | `stereo/raw` |
+| --- | --- | --- |
+| `distortion_coefficients` | zeros | the lens's real plumb_bob values (~-0.34) |
+| `rectification_matrix` | identity | the rotation into the rectified frame |
+| `projection_matrix` fx | 487.61 | 500.22 |
+| baseline (`-Tx/fx`) | 0.1197 m | 0.1197 m |
+
+Rectification is what leaves a calibration with no distortion and an identity rotation, so
+the rectified file describes the output of `stereo_image_proc`, not what the camera
+produced. Handing it to a raw pair claims a distortion-free lens the images do not have:
+doing that costs roughly a third of the inliers (110 against 214) and triples the reported
+standard deviation. `stereo_pose.yaml` holds the rig's measured extrinsics -- ~12 cm along
+x plus a few milliradians of rotation -- which the tests publish as the TF between
+`camera_left` and `camera_right`, the transform the node looks up when it has to rectify
+the pair itself.
+
+## File formats
+
+The images are copied as they are. The calibration files differ from their originals by one
+line: the format directive is commented out (`#%YAML:1.0`, with no `---`), which is the ROS
+flavour of the same file. `%YAML:1.0` is not a valid YAML directive, so plain YAML parsers
+-- `camera_info_manager`, `rosparam`, PyYAML -- reject the original form.
+`test/test_data.hpp` puts the directive back in memory before handing the text to
+`cv::FileStorage`, so the files stay readable by both.
+
+Note that they carry no OpenCV `dt` field either, so `>> cv::Mat` cannot read them;
+`rows`/`cols`/`data` are read element by element, as RTAB-Map's own `CameraModel::load`
+does.
+
+The depth images are 16-bit millimetres. `rgbd/calib/*.yaml` also carries a
+`local_transform` (the optical-frame-to-robot transform RTAB-Map stores with the camera
+model); the ROS nodes take that from TF instead, so the tests publish it as a `base_link`
+-> `camera` static transform rather than reading it here.
+
+## Using them
+
+`test/test_data.hpp` loads these into `cv::Mat`, `sensor_msgs/CameraInfo` and
+`geometry_msgs/Transform`. CMake passes the directory as `RTABMAP_ODOM_TEST_DATA_ROOT`,
+pointing into the source tree: the test binaries are not installed, and neither are these
+files.
diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml b/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml
new file mode 100644
index 00000000..c24a12b9
--- /dev/null
+++ b/rtabmap_odom/test/data/lidar/ouster_half_turn/metadata.yaml
@@ -0,0 +1,52 @@
+rosbag2_bagfile_information:
+  compression_format: ''
+  compression_mode: ''
+  custom_data: null
+  duration:
+    nanoseconds: 5066021600
+  files:
+  - duration:
+      nanoseconds: 5066021600
+    message_count: 120
+    path: ouster_half_turn.mcap
+    starting_time:
+      nanoseconds_since_epoch: 1613418430206475184
+  message_count: 120
+  relative_file_paths:
+  - ouster_half_turn.mcap
+  ros_distro: rosbags
+  starting_time:
+    nanoseconds_since_epoch: 1613418430206475184
+  storage_identifier: mcap
+  topics_with_message_count:
+  - message_count: 2
+    topic_metadata:
+      name: /tf_static
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 1\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 116
+    topic_metadata:
+      name: /tf
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: tf2_msgs/msg/TFMessage
+      type_description_hash: RIHS01_e369d0f05a23ae52508854b66f6aa0437f3449d652e8cbf22d5abe85d020f087
+  - message_count: 2
+    topic_metadata:
+      name: /os_cloud_node/points
+      offered_qos_profiles: "- avoid_ros_namespace_conventions: false\n  deadline:
+        {nsec: 0, sec: 0}\n  depth: 10\n  durability: 2\n  history: 1\n  lifespan:
+        {nsec: 0, sec: 0}\n  liveliness: 1\n  liveliness_lease_duration: {nsec: 0,
+        sec: 0}\n  reliability: 1"
+      serialization_format: cdr
+      type: sensor_msgs/msg/PointCloud2
+      type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5
+  version: 8
diff --git a/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap b/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap
new file mode 100644
index 00000000..7e25ec58
Binary files /dev/null and b/rtabmap_odom/test/data/lidar/ouster_half_turn/ouster_half_turn.mcap differ
diff --git a/rtabmap_odom/test/data/rgbd/calib/154.yaml b/rtabmap_odom/test/data/rgbd/calib/154.yaml
new file mode 100644
index 00000000..650e8bb7
--- /dev/null
+++ b/rtabmap_odom/test/data/rgbd/calib/154.yaml
@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "154"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.09767914e-03, 5.99460304e-02, 9.98201132e-01,
+       4.20193411e-02, -9.99999523e-01, -6.60419464e-05, -1.09562278e-03,
+       -8.86659764e-05, 8.94069672e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]
diff --git a/rtabmap_odom/test/data/rgbd/calib/17.yaml b/rtabmap_odom/test/data/rgbd/calib/17.yaml
new file mode 100644
index 00000000..c635c70a
--- /dev/null
+++ b/rtabmap_odom/test/data/rgbd/calib/17.yaml
@@ -0,0 +1,16 @@
+#%YAML:1.0
+camera_name: "17"
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 525., 0., 3.1950000000000000e+02, 0., 525.,
+       2.3950000000000000e+02, 0., 0., 1. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ -1.46162510e-03, 5.99460006e-02, 9.98200655e-01,
+       6.20153509e-02, -9.99999106e-01, -8.77380371e-05, -1.45888329e-03,
+       -1.63501027e-04, -2.98023224e-08, -9.98201728e-01, 5.99459410e-02,
+       4.28920656e-01 ]
diff --git a/rtabmap_odom/test/data/rgbd/depth/154.png b/rtabmap_odom/test/data/rgbd/depth/154.png
new file mode 100644
index 00000000..8b861747
Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/depth/154.png differ
diff --git a/rtabmap_odom/test/data/rgbd/depth/17.png b/rtabmap_odom/test/data/rgbd/depth/17.png
new file mode 100644
index 00000000..f950fb18
Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/depth/17.png differ
diff --git a/rtabmap_odom/test/data/rgbd/rgb/154.jpg b/rtabmap_odom/test/data/rgbd/rgb/154.jpg
new file mode 100644
index 00000000..dec36ba9
Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/rgb/154.jpg differ
diff --git a/rtabmap_odom/test/data/rgbd/rgb/17.jpg b/rtabmap_odom/test/data/rgbd/rgb/17.jpg
new file mode 100644
index 00000000..45a51a63
Binary files /dev/null and b/rtabmap_odom/test/data/rgbd/rgb/17.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/raw/left/420.jpg b/rtabmap_odom/test/data/stereo/raw/left/420.jpg
new file mode 100644
index 00000000..1892922a
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/left/420.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/raw/left/425.jpg b/rtabmap_odom/test/data/stereo/raw/left/425.jpg
new file mode 100644
index 00000000..c9c48c53
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/left/425.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/raw/right/420.jpg b/rtabmap_odom/test/data/stereo/raw/right/420.jpg
new file mode 100644
index 00000000..82350527
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/right/420.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/raw/right/425.jpg b/rtabmap_odom/test/data/stereo/raw/right/425.jpg
new file mode 100644
index 00000000..ad2de7f0
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/raw/right/425.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_left.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_left.yaml
new file mode 100644
index 00000000..0cde8cb7
--- /dev/null
+++ b/rtabmap_odom/test/data/stereo/raw/stereo_left.yaml
@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.2500741669069100e+02, 0., 3.1964931134995413e+02, 0.,
+       5.2447137699104690e+02, 2.4893842459131687e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.4256158963391159e-01, 1.5067561187251743e-01,
+       -1.1171618396794343e-03, -1.1496904882258663e-03,
+       -3.4792309849001175e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9847710457215999e-01, -7.5204006718293083e-03,
+       -5.4652678058179430e-02, 7.6102614351676607e-03,
+       9.9997001005750097e-01, 1.4362821762221084e-03,
+       5.4640237610064049e-02, -1.8500160368176528e-03,
+       9.9850439251641721e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02, 0., 0.,
+       5.0021545003868783e+02, 2.4750019836425781e+02, 0., 0., 0., 1.,
+       0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]
diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml
new file mode 100644
index 00000000..f5ab2be2
--- /dev/null
+++ b/rtabmap_odom/test/data/stereo/raw/stereo_pose.yaml
@@ -0,0 +1,30 @@
+#%YAML:1.0
+camera_name: stereo_tutorial
+rotation_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9998244174079198e-01, -9.1486052999498408e-04,
+       5.8548475927491656e-03, 8.9587501387632107e-04,
+       9.9999433529188531e-01, 3.2445018261516327e-03,
+       -5.8577826934067389e-03, -3.2391996466791719e-03,
+       9.9997759673282971e-01 ]
+translation_matrix:
+   rows: 3
+   cols: 1
+   data: [ -1.1944376542810328e-01, 8.1410498203477041e-04,
+       7.2368896323297812e-03 ]
+essential_matrix:
+   rows: 3
+   cols: 3
+   data: [ -1.1252198674164329e-05, -7.2394856860325228e-03,
+       7.9060664179560138e-04, 6.5370869431856798e-03,
+       -3.9352294745729046e-04, 1.1948346038335741e-01,
+       -9.2109737277881536e-04, -1.1944234402152067e-01,
+       -3.9230197564821978e-04 ]
+fundamental_matrix:
+   rows: 3
+   cols: 3
+   data: [ -2.5309754099766013e-08, -1.6300536361959590e-05,
+       4.9995535977749410e-03, 1.4720205166478256e-05,
+       -8.8704021902692202e-07, 1.3677019067204871e-01,
+       -4.6630670977502930e-03, -1.3776019369349130e-01, 1. ]
diff --git a/rtabmap_odom/test/data/stereo/raw/stereo_right.yaml b/rtabmap_odom/test/data/stereo/raw/stereo_right.yaml
new file mode 100644
index 00000000..f0cbeacc
--- /dev/null
+++ b/rtabmap_odom/test/data/stereo/raw/stereo_right.yaml
@@ -0,0 +1,34 @@
+#%YAML:1.0
+camera_name: stereo_tutorial_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 5.3173015723615038e+02, 0., 3.0841183762414585e+02, 0.,
+       5.3114393189729469e+02, 2.4247036103612575e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ -3.2382605112765667e-01, 4.3067676369775459e-02,
+       -1.0212741406694578e-03, 5.1816193644504472e-04,
+       9.9965184406768520e-02 ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 9.9814647006952373e-01, -6.8031680948065307e-03,
+       -6.0475955483342586e-02, 6.7037039320888168e-03,
+       9.9997582338248303e-01, -1.8474317621778344e-03,
+       6.0487061768119681e-02, 1.4385945915416094e-03,
+       9.9816794468879888e-01 ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 5.0021545003868783e+02, 0., 3.5945997238159180e+02,
+       -5.9858566522579160e+01, 0., 5.0021545003868783e+02,
+       2.4750019836425781e+02, 0., 0., 0., 1., 0. ]
+local_transform:
+   rows: 3
+   cols: 4
+   data: [ 0., 0., 1., 0., -1., 0., 0., 0., 0., -1., 0., 0. ]
diff --git a/rtabmap_odom/test/data/stereo/rect/left/50.jpg b/rtabmap_odom/test/data/stereo/rect/left/50.jpg
new file mode 100644
index 00000000..93438f3c
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/left/50.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/rect/left/60.jpg b/rtabmap_odom/test/data/stereo/rect/left/60.jpg
new file mode 100644
index 00000000..04630c50
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/left/60.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/rect/right/50.jpg b/rtabmap_odom/test/data/stereo/rect/right/50.jpg
new file mode 100644
index 00000000..f81336dc
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/right/50.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/rect/right/60.jpg b/rtabmap_odom/test/data/stereo/rect/right/60.jpg
new file mode 100644
index 00000000..3b10e301
Binary files /dev/null and b/rtabmap_odom/test/data/stereo/rect/right/60.jpg differ
diff --git a/rtabmap_odom/test/data/stereo/rect/stereo_left.yaml b/rtabmap_odom/test/data/stereo/rect/stereo_left.yaml
new file mode 100644
index 00000000..ecd65446
--- /dev/null
+++ b/rtabmap_odom/test/data/stereo/rect/stereo_left.yaml
@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_left
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0., 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 0., 1.,
+       0. ]
diff --git a/rtabmap_odom/test/data/stereo/rect/stereo_right.yaml b/rtabmap_odom/test/data/stereo/rect/stereo_right.yaml
new file mode 100644
index 00000000..7fc7708a
--- /dev/null
+++ b/rtabmap_odom/test/data/stereo/rect/stereo_right.yaml
@@ -0,0 +1,24 @@
+#%YAML:1.0
+camera_name: stereo_right
+image_width: 640
+image_height: 480
+camera_matrix:
+   rows: 3
+   cols: 3
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
+       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
+distortion_coefficients:
+   rows: 1
+   cols: 5
+   data: [ 0., 0., 0., 0., 0. ]
+distortion_model: plumb_bob
+rectification_matrix:
+   rows: 3
+   cols: 3
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection_matrix:
+   rows: 3
+   cols: 4
+   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02,
+       -5.8362700032946350e+01, 0., 4.8760873413085938e+02,
+       2.4944424438476562e+02, 0., 0., 0., 1., 0. ]
diff --git a/rtabmap_odom/test/msg_builders.hpp b/rtabmap_odom/test/msg_builders.hpp
new file mode 100644
index 00000000..b6c0d5ac
--- /dev/null
+++ b/rtabmap_odom/test/msg_builders.hpp
@@ -0,0 +1,362 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_MSG_BUILDERS_HPP_
+#define RTABMAP_ODOM_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_odom_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;
+}
+
+/**
+ * @brief An XYZ cloud carrying the optional fields a 3D lidar may add.
+ *
+ * `intensity` and the `normal_*`/`curvature` group each change which PCL point type the
+ * odometry converts the cloud into, so a driver that sends them takes a different path
+ * through the node than one that sends plain XYZ. `t` is the per-point offset from the
+ * header stamp that deskewing needs, spread evenly over @p sweep seconds.
+ */
+inline sensor_msgs::msg::PointCloud2 makeCloudWithFields(
+		const std::string & frameId, double stamp,
+		const std::vector<cv::Point3f> & points,
+		bool withIntensity, bool withNormals,
+		const cv::Point3f & normal = cv::Point3f(0, 0, 1),
+		bool withTime = false, float sweep = 0.01f)
+{
+	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;
+
+	std::vector<std::string> names = {"x", "y", "z"};
+	if(withIntensity)
+	{
+		names.push_back("intensity");
+	}
+	if(withNormals)
+	{
+		names.push_back("normal_x");
+		names.push_back("normal_y");
+		names.push_back("normal_z");
+		names.push_back("curvature");
+	}
+	if(withTime)
+	{
+		names.push_back("t");
+	}
+	cloud.fields.resize(names.size());
+	for(size_t i=0; i<names.size(); ++i)
+	{
+		cloud.fields[i].name = names[i];
+		cloud.fields[i].offset = uint32_t(4 * i);
+		cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
+		cloud.fields[i].count = 1;
+	}
+	cloud.point_step = uint32_t(4 * names.size());
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(size_t(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]);
+		size_t f = 0;
+		p[f++] = points[i].x;
+		p[f++] = points[i].y;
+		p[f++] = points[i].z;
+		if(withIntensity)
+		{
+			p[f++] = float(i % 256);
+		}
+		if(withNormals)
+		{
+			p[f++] = normal.x;
+			p[f++] = normal.y;
+			p[f++] = normal.z;
+			p[f++] = 0.0f;   // curvature
+		}
+		if(withTime)
+		{
+			p[f++] = points.size() > 1 ?
+					sweep * float(i) / float(points.size() - 1) : 0.0f;
+		}
+	}
+	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_odom_test
+
+#endif /* RTABMAP_ODOM_MSG_BUILDERS_HPP_ */
diff --git a/rtabmap_odom/test/node_test_utils.hpp b/rtabmap_odom/test/node_test_utils.hpp
new file mode 100644
index 00000000..ee9152ef
--- /dev/null
+++ b/rtabmap_odom/test/node_test_utils.hpp
@@ -0,0 +1,256 @@
+/*
+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_ODOM_NODE_TEST_UTILS_HPP_
+#define RTABMAP_ODOM_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace rtabmap_odom_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_odom_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();
+	}
+
+	/**
+	 * @brief Adds a node under test to the shared executor and keeps it alive for the test.
+	 *
+	 * The wait is for tf2_ros, not for anything the test does with the node.
+	 * ~TransformListener cancels its worker's executor and joins it without ordering the
+	 * cancel after the worker reached spin(), so a node dropped microseconds after it was
+	 * built -- which a test that only reads a parameter back does -- hangs the binary for
+	 * good (ros2/geometry2#517). The window is a few instructions wide and nothing here
+	 * can observe that thread, so this buys time instead. Drop it once #752 lands.
+	 */
+	template <typename NodeT>
+	std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
+	{
+		executor_->add_node(node);
+		nodes_.push_back(node);
+		spinFor(std::chrono::milliseconds(50));
+		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(); }
+
+		/**
+		 * @brief The last (first) message received.
+		 *
+		 * A test that reads these without having waited for the topic it is reading --
+		 * having waited for a different one, say -- gets a legible failure rather than a
+		 * segmentation fault: std::vector::back() on an empty vector dereferences
+		 * nullptr-1, which crashes the whole binary and takes the rest of its tests with
+		 * it. gtest turns the exception into a failure of the test that threw it.
+		 */
+		const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
+		const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
+
+	private:
+		const typename MsgT::ConstSharedPtr & checked(
+				const typename MsgT::ConstSharedPtr * msg) const
+		{
+			if(msg == 0)
+			{
+				throw std::out_of_range(
+						std::string("nothing was received on \"") +
+						(subscription?subscription->get_topic_name():"?") +
+						"\", so there is no message to read: wait for it to arrive first");
+			}
+			return *msg;
+		}
+	};
+
+	/**
+	 * @brief Subscribes the helper node to @p topic and records everything it receives.
+	 *
+	 * The callback holds the collector weakly. Capturing it by shared_ptr would close a
+	 * cycle -- collector owns the subscription, the subscription owns the callback, the
+	 * callback owns the collector -- and neither would ever be freed. A subscription that
+	 * outlives its test keeps the helper node's rcl handle alive with it, which leaves the
+	 * node's rosout publisher registered and greets the next test with "Publisher already
+	 * registered for node name: 'rtabmap_odom_test_helper'".
+	 */
+	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>>();
+		std::weak_ptr<Collector<MsgT>> weak = collector;
+		collector->subscription = helper_->create_subscription<MsgT>(
+				topic, qos,
+				[weak](const typename MsgT::ConstSharedPtr msg) {
+					if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
+					{
+						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_odom_test
+
+#endif /* RTABMAP_ODOM_NODE_TEST_UTILS_HPP_ */
diff --git a/rtabmap_odom/test/scan_scenes.hpp b/rtabmap_odom/test/scan_scenes.hpp
new file mode 100644
index 00000000..ec67a00c
--- /dev/null
+++ b/rtabmap_odom/test/scan_scenes.hpp
@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_SCAN_SCENES_HPP_
+#define RTABMAP_ODOM_SCAN_SCENES_HPP_
+
+#include <opencv2/core/core.hpp>
+
+#include <cmath>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <vector>
+
+namespace rtabmap_odom_test {
+
+/**
+ * A 3D corner -- floor plus two walls -- so all six degrees of freedom are constrained.
+ *
+ * Ported from makeCorner3D() in RTAB-Map's corelib/test/test_odometry.cpp. The jitter is
+ * not decoration: a perfectly flat lattice gives degenerate per-point normals and ICP
+ * finds no correspondences at all.
+ */
+inline std::vector<cv::Point3f> corner3D(
+		const cv::Point3f & offset = cv::Point3f(0,0,0),
+		float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
+{
+	cv::RNG rng(seed);
+	const float half = 0.5f * length;
+	std::vector<cv::Point3f> points;
+	points.reserve(3 * pointsPerSurface);
+	for(int i=0; i<pointsPerSurface; ++i)  // floor z=-half
+	{
+		points.push_back(cv::Point3f(rng.uniform(-half, half), rng.uniform(-half, half),
+				-half + float(rng.gaussian(0.005))) - offset);
+	}
+	for(int i=0; i<pointsPerSurface; ++i)  // wall x=-half
+	{
+		points.push_back(cv::Point3f(-half + float(rng.gaussian(0.005)),
+				rng.uniform(-half, half), rng.uniform(-half, half)) - offset);
+	}
+	for(int i=0; i<pointsPerSurface; ++i)  // wall y=-half
+	{
+		points.push_back(cv::Point3f(rng.uniform(-half, half),
+				-half + float(rng.gaussian(0.005)), rng.uniform(-half, half)) - offset);
+	}
+	return points;
+}
+
+/**
+ * @brief The same corner seen after the sensor has turned @p yaw about z.
+ *
+ * The corner does not move; the sensor does, so in the sensor's own frame every point
+ * turns the other way. This is what a scan taken after a rotation looks like.
+ */
+inline std::vector<cv::Point3f> corner3DTurned(double yaw)
+{
+	const double c = std::cos(-yaw);
+	const double s = std::sin(-yaw);
+	std::vector<cv::Point3f> points = corner3D();
+	for(cv::Point3f & p : points)
+	{
+		const float x = p.x;
+		p.x = float(c * x - s * p.y);
+		p.y = float(s * x + c * p.y);
+	}
+	return points;
+}
+
+/**
+ * @brief Range to a 2D corner from a sensor at (@p sensorX, @p sensorY) looking along +x.
+ *
+ * Two perpendicular walls, one ahead and one to the left. A single wall would leave the
+ * motion along it unobservable and ICP would settle wherever it started; the corner pins
+ * both axes and the heading.
+ *
+ * @return the nearer wall along the ray, or 0 if the ray reaches neither
+ */
+inline float corner2DRange(
+		double sensorX, double sensorY, double angle,
+		float frontWall = 5.0f, float leftWall = 3.0f)
+{
+	const double dx = std::cos(angle);
+	const double dy = std::sin(angle);
+	double best = 0.0;
+	if(dx > 1e-6)
+	{
+		best = (frontWall - sensorX) / dx;
+	}
+	if(dy > 1e-6)
+	{
+		const double toLeft = (leftWall - sensorY) / dy;
+		best = (best <= 0.0 || toLeft < best) ? toLeft : best;
+	}
+	return float(best);
+}
+
+/**
+ * The ICP settings RTAB-Map's own odometry tests use for this scene: point-to-point, no
+ * voxelization, and a correspondence ratio low enough for a synthetic scan.
+ */
+inline std::vector<rclcpp::Parameter> icpTestParameters()
+{
+	return {
+		rclcpp::Parameter("Icp/PointToPlane", "false"),
+		rclcpp::Parameter("scan_voxel_size", 0.0),
+		rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
+	};
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_SCAN_SCENES_HPP_ */
diff --git a/rtabmap_odom/test/test_data.hpp b/rtabmap_odom/test/test_data.hpp
new file mode 100644
index 00000000..f2fece3b
--- /dev/null
+++ b/rtabmap_odom/test/test_data.hpp
@@ -0,0 +1,268 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_TEST_DATA_HPP_
+#define RTABMAP_ODOM_TEST_DATA_HPP_
+
+#include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#include <tf2/LinearMath/Matrix3x3.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "msg_builders.hpp"
+
+/**
+ * @file
+ * @brief Real frames for the odometry node tests, from test/data.
+ *
+ * Visual odometry needs imagery it can actually track: synthetic noise gives a detector
+ * corners that match nothing between frames, so a generated "motion" tells us only that
+ * the node did not crash. These loaders hand the tests the same stereo pairs and RGB-D
+ * frames that RTAB-Map registers in corelib/test/test_odometry.cpp, which lets a ROS test
+ * assert that a plausible transform came out the other end.
+ *
+ * See test/data/README.md for where the files come from and why they are vendored.
+ */
+
+namespace rtabmap_odom_test {
+
+/// Root of the vendored fixtures, set by CMake to test/data in the source tree.
+inline std::string testDataRoot()
+{
+	return std::string(RTABMAP_ODOM_TEST_DATA_ROOT);
+}
+
+/**
+ * @brief Opens a ROS camera calibration file with OpenCV's parser.
+ *
+ * The files are the ROS flavour: the format line is commented out (`#%YAML:1.0`) so that
+ * a plain YAML parser -- camera_info_manager, rosparam, PyYAML -- accepts them, since
+ * `%YAML:1.0` is not a valid YAML directive and makes those parsers fail. cv::FileStorage
+ * wants the directive, so it is put back here, in memory, and the file on disk stays
+ * loadable by both.
+ *
+ * @return a closed FileStorage if the file is missing
+ */
+inline cv::FileStorage openCalibration(const std::string & path)
+{
+	std::ifstream file(path.c_str());
+	if(!file.is_open())
+	{
+		return cv::FileStorage();
+	}
+	std::ostringstream buffer;
+	buffer << file.rdbuf();
+	std::string text = buffer.str();
+
+	const std::string rosHeader = "#%YAML:1.0";
+	if(text.compare(0, rosHeader.size(), rosHeader) == 0)
+	{
+		text = "%YAML:1.0\n---" + text.substr(rosHeader.size());
+	}
+	return cv::FileStorage(text, cv::FileStorage::READ | cv::FileStorage::MEMORY);
+}
+
+/**
+ * @brief Reads a rows/cols/data matrix from a calibration file node.
+ *
+ * ROS calibration files have no OpenCV `dt` field, so `>> cv::Mat` cannot read them;
+ * the elements are taken one by one instead, as RTAB-Map's CameraModel::load does.
+ *
+ * @return an empty vector if the node is missing or malformed
+ */
+inline std::vector<double> readCalibrationMatrix(
+		cv::FileStorage & fs, const std::string & name, int rows, int cols)
+{
+	const cv::FileNode node = fs[name];
+	if(node.empty())
+	{
+		return std::vector<double>();
+	}
+	std::vector<double> data;
+	node["data"] >> data;
+	if((int)node["rows"] != rows || (int)node["cols"] != cols ||
+	   data.size() != size_t(rows * cols))
+	{
+		return std::vector<double>();
+	}
+	return data;
+}
+
+/**
+ * @brief A CameraInfo from a ROS calibration file.
+ *
+ * The RGB-D files carry only camera_matrix, so R falls back to identity and P to [K|0] --
+ * which is what a driver publishes for an already-rectified monocular camera anyway.
+ */
+inline sensor_msgs::msg::CameraInfo cameraInfoFromCalibration(
+		const std::string & path, const std::string & frameId, double stamp)
+{
+	sensor_msgs::msg::CameraInfo info;
+	info.header.frame_id = frameId;
+	info.header.stamp = stampOf(stamp);
+
+	cv::FileStorage fs = openCalibration(path);
+	if(!fs.isOpened())
+	{
+		return info;
+	}
+	info.width = uint32_t((int)fs["image_width"]);
+	info.height = uint32_t((int)fs["image_height"]);
+	info.distortion_model = fs["distortion_model"].isString() ?
+			(std::string)fs["distortion_model"] : std::string("plumb_bob");
+
+	const std::vector<double> k = readCalibrationMatrix(fs, "camera_matrix", 3, 3);
+	const std::vector<double> r = readCalibrationMatrix(fs, "rectification_matrix", 3, 3);
+	const std::vector<double> p = readCalibrationMatrix(fs, "projection_matrix", 3, 4);
+	const std::vector<double> d = readCalibrationMatrix(fs, "distortion_coefficients", 1, 5);
+
+	info.d.assign(5, 0.0);
+	for(size_t i=0; i<d.size() && i<info.d.size(); ++i)
+	{
+		info.d[i] = d[i];
+	}
+	for(size_t i=0; i<9; ++i)
+	{
+		info.k[i] = k.empty() ? 0.0 : k[i];
+		info.r[i] = r.empty() ? (i%4 == 0 ? 1.0 : 0.0) : r[i];
+	}
+	for(size_t i=0; i<12; ++i)
+	{
+		info.p[i] = p.empty() ? (i%4 == 3 ? 0.0 : info.k[(i/4)*3 + i%4]) : p[i];
+	}
+	return info;
+}
+
+// ---------------------------------------------------------------------------
+// Stereo pairs: data/stereo, from one 640x480 rig at 20 Hz.
+//
+// "rect" holds rectified pairs (frames 50 and 60), what the nodes expect on
+// left/image_rect by default. "raw" holds pairs straight off the camera (frames 420 and
+// 425), for the Rtabmap/ImagesAlreadyRectified path.
+//
+// Each set carries the calibration that describes it, and the two are not
+// interchangeable: the rectified one has no distortion and an identity rectification
+// matrix, because that is what rectification leaves behind, while the raw one carries the
+// lens's real plumb_bob coefficients and the rotation into the rectified frame. Handing
+// the rectified calibration to a raw pair claims a distortion-free lens the images do not
+// have.
+// ---------------------------------------------------------------------------
+
+/// "rect" or "raw"; a rectified pair unless a test says otherwise.
+enum StereoSet { kRectified, kRaw };
+
+inline std::string stereoSetDir(StereoSet set)
+{
+	return testDataRoot() + (set == kRaw ? "/stereo/raw" : "/stereo/rect");
+}
+
+/// The left image of a pair, in color as a stereo driver publishes it (bgr8).
+inline cv::Mat stereoLeftImage(const std::string & name, StereoSet set = kRectified)
+{
+	return cv::imread(stereoSetDir(set) + "/left/" + name + ".jpg", cv::IMREAD_COLOR);
+}
+
+/// The right image of a pair, grayscale (mono8), which is all the matcher uses.
+inline cv::Mat stereoRightImage(const std::string & name, StereoSet set = kRectified)
+{
+	return cv::imread(stereoSetDir(set) + "/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
+}
+
+inline sensor_msgs::msg::CameraInfo stereoLeftInfo(
+		const std::string & frameId, double stamp, StereoSet set = kRectified)
+{
+	return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_left.yaml", frameId, stamp);
+}
+
+/// The right camera's P(0,3) is -fx * baseline, which is where the stereo scale comes from.
+inline sensor_msgs::msg::CameraInfo stereoRightInfo(
+		const std::string & frameId, double stamp, StereoSet set = kRectified)
+{
+	return cameraInfoFromCalibration(stereoSetDir(set) + "/stereo_right.yaml", frameId, stamp);
+}
+
+/**
+ * @brief The right camera's pose in the left camera's frame, from the rig's stereo
+ *        calibration (data/stereo/raw/stereo_pose.yaml).
+ *
+ * Stereo calibration stores the transform the other way round -- a point in left
+ * coordinates maps to the right camera as X_right = R * X_left + T -- so this inverts it
+ * into what TF wants to publish, camera_left -> camera_right. Feeding it to TF is what
+ * lets Rtabmap/ImagesAlreadyRectified:=false rectify the pair against the rig's real
+ * extrinsics, small inter-camera rotation included, rather than an assumed ideal baseline.
+ *
+ * @return an identity transform if the file is missing
+ */
+inline geometry_msgs::msg::Transform stereoRightInLeftFrame()
+{
+	geometry_msgs::msg::Transform transform;
+	transform.rotation.w = 1.0;
+
+	cv::FileStorage fs = openCalibration(stereoSetDir(kRaw) + "/stereo_pose.yaml");
+	if(!fs.isOpened())
+	{
+		return transform;
+	}
+	const std::vector<double> r = readCalibrationMatrix(fs, "rotation_matrix", 3, 3);
+	const std::vector<double> t = readCalibrationMatrix(fs, "translation_matrix", 3, 1);
+	if(r.empty() || t.empty())
+	{
+		return transform;
+	}
+
+	// (R, T) -> (R', -R'T)
+	const tf2::Matrix3x3 rotation(
+			r[0], r[1], r[2],
+			r[3], r[4], r[5],
+			r[6], r[7], r[8]);
+	const tf2::Matrix3x3 inverse = rotation.transpose();
+	const tf2::Vector3 translation = inverse * -tf2::Vector3(t[0], t[1], t[2]);
+
+	tf2::Quaternion q;
+	inverse.getRotation(q);
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+	transform.rotation.w = q.w();
+	transform.translation.x = translation.x();
+	transform.translation.y = translation.y();
+	transform.translation.z = translation.z();
+	return transform;
+}
+
+// ---------------------------------------------------------------------------
+// RGB-D frames: data/rgbd, frames "17" and "154".
+// ---------------------------------------------------------------------------
+
+inline cv::Mat rgbdColorImage(const std::string & name)
+{
+	return cv::imread(testDataRoot() + "/rgbd/rgb/" + name + ".jpg", cv::IMREAD_COLOR);
+}
+
+/// 16-bit millimetres, the encoding the RGB-D drivers publish (16UC1).
+inline cv::Mat rgbdDepthImage(const std::string & name)
+{
+	return cv::imread(testDataRoot() + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
+}
+
+inline sensor_msgs::msg::CameraInfo rgbdInfo(
+		const std::string & name, const std::string & frameId, double stamp)
+{
+	return cameraInfoFromCalibration(
+			testDataRoot() + "/rgbd/calib/" + name + ".yaml", frameId, stamp);
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_TEST_DATA_HPP_ */
diff --git a/rtabmap_odom/test/test_icp_odometry.cpp b/rtabmap_odom/test/test_icp_odometry.cpp
new file mode 100644
index 00000000..eeac9099
--- /dev/null
+++ b/rtabmap_odom/test/test_icp_odometry.cpp
@@ -0,0 +1,2103 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <tf2_ros/static_transform_broadcaster.hpp>
+
+
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/sensor_data.hpp>
+
+#include <rtabmap_odom/icp_odometry.hpp>
+
+#include <rtabmap/core/util3d_registration.h>
+
+#include <rtabmap_conversions/PointCloudConversion.h>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "bag_playback.hpp"
+#include "msg_builders.hpp"
+#include "scan_scenes.hpp"
+#include "node_test_utils.hpp"
+
+namespace rtabmap_odom_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
+bool isLost(const nav_msgs::msg::Odometry & odom)
+{
+	return odom.pose.covariance[0] >= 9999.0;
+}
+
+double translationNorm(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Point & p = odom.pose.pose.position;
+	return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
+}
+
+/// The rotation carried by a geometry_msgs quaternion, in radians.
+double rotationAngleOf(const geometry_msgs::msg::Quaternion & q)
+{
+	return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
+}
+
+double rotationAngle(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
+	return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
+}
+
+/**
+ * @brief The 2D corner seen from (@p x, @p y), every ray taken at the same instant.
+ *
+ * The skewed scans further down bend the walls with the robot's motion; this one does
+ * not, so it is what a sweep taken from a standstill looks like. @p withIntensities adds
+ * the channel a real lidar reports alongside the range, which is what decides whether the
+ * node carries the scan as PointXYZI or as PointXYZ.
+ */
+sensor_msgs::msg::LaserScan cornerScan(
+		double stamp, double x = 0.0, double y = 0.0, bool withIntensities = false,
+		float sweep = 0.0f)
+{
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = "lidar";
+	scan.header.stamp = stampOf(stamp);
+	scan.angle_min = -1.0f;
+	scan.angle_max = 1.0f;
+	scan.angle_increment = 0.01f;
+	scan.scan_time = 0.1f;
+	scan.range_min = 0.1f;
+	scan.range_max = 30.0f;
+	const size_t rays = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1;
+	// Zero unless the caller wants the per-ray stamps deskewing needs.
+	scan.time_increment = rays > 1 ? sweep / float(rays - 1) : 0.0f;
+	scan.ranges.resize(rays);
+	for(size_t i=0; i<rays; ++i)
+	{
+		scan.ranges[i] = corner2DRange(x, y, scan.angle_min + double(i)*scan.angle_increment);
+	}
+	if(withIntensities)
+	{
+		// Flat, but present: the node only looks at whether the channel is there.
+		scan.intensities.assign(rays, 100.0f);
+	}
+	return scan;
+}
+
+/// @p cloud rearranged into @p width columns, the shape a spinning lidar publishes: one
+/// row per ring, and the node reads a full sweep's size off those dimensions.
+sensor_msgs::msg::PointCloud2 organized(sensor_msgs::msg::PointCloud2 cloud, uint32_t width)
+{
+	const uint32_t points = cloud.width * cloud.height;
+	cloud.width = width;
+	cloud.height = points / width;
+	cloud.row_step = cloud.point_step * cloud.width;
+	cloud.data.resize(size_t(cloud.row_step) * cloud.height);
+	return cloud;
+}
+
+/// Every value @p cloud carries in @p field, in point order; empty if it has no such field.
+std::vector<float> fieldValues(
+		const sensor_msgs::msg::PointCloud2 & cloud, const std::string & field)
+{
+	std::vector<float> values;
+	for(const sensor_msgs::msg::PointField & f : cloud.fields)
+	{
+		if(f.name == field && f.datatype == sensor_msgs::msg::PointField::FLOAT32)
+		{
+			for(size_t i=0; i<size_t(cloud.width)*cloud.height; ++i)
+			{
+				float value = 0.0f;
+				memcpy(&value, &cloud.data[i*cloud.point_step + f.offset], sizeof(float));
+				values.push_back(value);
+			}
+			break;
+		}
+	}
+	return values;
+}
+
+/// How many of @p values are above zero.
+size_t countPositive(const std::vector<float> & values)
+{
+	return size_t(std::count_if(values.begin(), values.end(),
+			[](float value) { return value > 0.0f; }));
+}
+
+/**
+ * @brief The share of @p cloud's points that still sit within @p maxDistance of @p reference.
+ *
+ * Deskewing moves points along the sweep rather than adding or removing them, and
+ * voxelizing the result renumbers whatever is left, so the two clouds cannot be compared
+ * index by index. This is RTAB-Map's own correspondence count, which does not care about
+ * the ordering: 1.0 means the two clouds are on top of each other, and anything less is
+ * how much of one moved away from the other.
+ */
+double correspondenceRatio(
+		const sensor_msgs::msg::PointCloud2 & cloud,
+		const sensor_msgs::msg::PointCloud2 & reference,
+		double maxDistance)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
+	rtabmap_conversions::fromPointCloud2Msg(cloud, *source);
+	rtabmap_conversions::fromPointCloud2Msg(reference, *target);
+	if(source->empty() || target->empty())
+	{
+		return 0.0;
+	}
+
+	double variance = 0.0;
+	int correspondences = 0;
+	rtabmap::util3d::computeVarianceAndCorrespondences(
+			source, target, maxDistance, variance, correspondences, false);
+	return double(correspondences) / double(source->size());
+}
+
+class IcpOdometryTest : public NodeTest
+{
+protected:
+	/// The sensor has to be connected to frame_id in TF before the first frame arrives.
+	void publishSensorTf(const std::string & sensorFrame = "lidar")
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = helper()->now();
+		tf.header.frame_id = "base_link";
+		tf.child_frame_id = sensorFrame;
+		tf.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(tf);
+	}
+
+	/// Calls an Empty service the node advertises under its own name.
+	bool callEmptyService(const std::string & name)
+	{
+		rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+				helper()->create_client<std_srvs::srv::Empty>("/icp_odometry/" + name);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return false;
+		}
+		std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
+				client->async_send_request(
+						std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
+		return spinUntil([&]() {
+			return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
+	}
+
+	std::shared_ptr<rtabmap_odom::ICPOdometry> makeNode(
+			std::vector<rclcpp::Parameter> params = {}, const std::string & name = "")
+	{
+		// Defaults first, so a test that passes the same parameter overrides them.
+		//
+		// always_process_most_recent_frame:=false is what the node itself recommends for
+		// data that arrives faster than its stamps: these tests publish a whole sequence
+		// back to back with stamps a tenth of a second apart, and when the executor is
+		// slow enough that two of them land in the same spin -- a loaded CI runner, a
+		// single core -- the node drops the second as a replay glitch and the test waits
+		// for a message that will never come. It also keeps processing on the calling
+		// thread instead of the node's worker, which is what makes these tests observable
+		// at all: the odometry is finished by the time the publish returns.
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("frame_id", "base_link"),
+			rclcpp::Parameter("publish_tf", false),
+			rclcpp::Parameter("always_process_most_recent_frame", false),
+		};
+		all.insert(all.end(), params.begin(), params.end());
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(all);
+		if(!name.empty())
+		{
+			// A test that runs two nodes at once has to keep their names and their odom
+			// topics apart, or they publish over each other.
+			options.arguments({"--ros-args", "-r", "__node:=" + name,
+			                   "-r", "odom:=odom_" + name,
+			                   "-r", "odom_info:=odom_info_" + name,
+			                   "-r", "odom_sensor_data/raw:=odom_sensor_data_" + name + "/raw",
+			                   "-r", "odom_filtered_input_scan:=odom_filtered_input_scan_" + name});
+		}
+		return addNode(std::make_shared<rtabmap_odom::ICPOdometry>(options));
+	}
+
+	/**
+	 * @brief Publishes the recorded TF history, then hands the clouds over one at a time.
+	 *
+	 * All of TF goes out first, so every lookup the node makes is already in the buffer:
+	 * the recording covers each sweep from end to end (see test/data/README.md), and
+	 * replaying it up front removes any race between TF arriving and a cloud being
+	 * processed. The clouds keep their recorded stamps and frame -- os_sensor, which TF
+	 * ties back to base_link through the rig's rotating joint.
+	 */
+
+	// -----------------------------------------------------------------------
+	// A 2D lidar on a robot driving at a corner, for the LaserScan deskewing path.
+	// -----------------------------------------------------------------------
+
+	static constexpr double kScanSweep = 0.1;     ///< first ray to last, seconds
+	static constexpr double kFirstScan = 1.0;     ///< stamp of the first scan
+	static constexpr double kSecondScan = 1.5;    ///< stamp of the second
+
+	/**
+	 * @brief Where the robot is at time @p t, in odom.
+	 *
+	 * It drives at 1 m/s through the first sweep and the gap after it, then stops before
+	 * the second. That difference is the whole point: at a constant speed both sweeps bend
+	 * by the same amount and even an unskewed registration lands in the right place, so
+	 * the bug would hide. Braking makes the first scan bent and the second straight.
+	 */
+	static double robotX(double t)
+	{
+		const double cruise = kSecondScan - kScanSweep;   // stops one sweep early
+		return t <= kFirstScan ? 0.0
+				: (t < cruise ? (t - kFirstScan) : (cruise - kFirstScan));
+	}
+
+	/// What the odometry should report between the two scan stamps.
+	static double trueDisplacement()
+	{
+		return robotX(kSecondScan) - robotX(kFirstScan);
+	}
+
+	/**
+	 * @brief One scan of the corner, skewed by the robot's motion during the sweep.
+	 *
+	 * Each ray is cast from where the sensor actually was when that ray was taken, which
+	 * is what a real lidar does and what makes the wall come out bent.
+	 */
+	sensor_msgs::msg::LaserScan makeSkewedCornerScan(double stamp)
+	{
+		sensor_msgs::msg::LaserScan scan;
+		scan.header.frame_id = "lidar";
+		scan.header.stamp = stampOf(stamp);
+		scan.angle_min = -1.0f;
+		scan.angle_max = 1.0f;
+		scan.angle_increment = 0.01f;
+		scan.range_min = 0.1f;
+		scan.range_max = 30.0f;
+		const size_t rays = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1;
+		scan.time_increment = float(kScanSweep / double(rays - 1));
+		scan.scan_time = float(kScanSweep);
+		scan.ranges.resize(rays);
+		for(size_t i=0; i<rays; ++i)
+		{
+			const double rayTime = stamp + double(i) * scan.time_increment;
+			const double angle = scan.angle_min + double(i) * scan.angle_increment;
+			scan.ranges[i] = corner2DRange(robotX(rayTime), 0.0, angle);
+		}
+		return scan;
+	}
+
+	/**
+	 * @brief Publishes odom -> base_link along that trajectory, plus base_link -> lidar.
+	 *
+	 * Sampled at 100 Hz across both sweeps: laser_geometry interpolates between whatever
+	 * TF holds, and the correction is only as good as the trajectory it can see.
+	 */
+	bool publishRobotTrajectory()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped sensor;
+		sensor.header.stamp = helper()->now();
+		sensor.header.frame_id = "base_link";
+		sensor.child_frame_id = "lidar";
+		sensor.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(sensor);
+
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> echo =
+				collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		if(!waitForSubscriber(tf, 2))
+		{
+			return false;
+		}
+		size_t published = 0;
+		for(double t = kFirstScan - 0.1; t <= kSecondScan + kScanSweep + 0.1; t += 0.01)
+		{
+			geometry_msgs::msg::TransformStamped pose;
+			pose.header.stamp = stampOf(t);
+			pose.header.frame_id = "odom";
+			pose.child_frame_id = "base_link";
+			pose.transform.translation.x = robotX(t);
+			pose.transform.rotation.w = 1.0;
+			tf2_msgs::msg::TFMessage message;
+			message.transforms.push_back(pose);
+			tf->publish(message);
+			if(++published % 10 == 0)
+			{
+				spinFor(std::chrono::milliseconds(5));
+			}
+		}
+		tfPublisher_ = tf;
+		return spinUntil([&]() { return echo->size() >= published; });
+	}
+
+	struct Recording
+	{
+		std::vector<tf2_msgs::msg::TFMessage> staticTransforms;
+		std::vector<tf2_msgs::msg::TFMessage> transforms;
+		std::vector<sensor_msgs::msg::PointCloud2> clouds;
+		bool valid() const
+		{
+			return !staticTransforms.empty() && !transforms.empty() && clouds.size() >= 2;
+		}
+	};
+
+	/**
+	 * What the sensor turns between the two scans, whoever predicts it.
+	 *
+	 * 60 degrees is chosen: large enough that neither ICP backend finds it from an
+	 * identity start -- both settle within 0.03 rad of no motion at all -- and clear of
+	 * the corner scene's own 90 degree symmetry, where a wall matched onto the next wall
+	 * would be a second, equally good answer.
+	 */
+	static constexpr double kPredictedTurn = 1.05;
+	/// Where the IMU's heading starts; see publishImuTurn() for why it is not zero.
+	static constexpr double kImuHeading = 0.2;
+
+	/// base_link -> imu_link, which the IMU callback requires before it accepts anything.
+	void publishImuTf()
+	{
+		imuTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped sensor;
+		sensor.header.stamp = helper()->now();
+		sensor.header.frame_id = "base_link";
+		sensor.child_frame_id = "imu_link";
+		sensor.transform.rotation.w = 1.0;
+		imuTf_->sendTransform(sensor);
+	}
+
+	/// One IMU sample, heading @p yaw about z.
+	sensor_msgs::msg::Imu imuSample(double stamp, double yaw)
+	{
+		sensor_msgs::msg::Imu sample;
+		sample.header.frame_id = "imu_link";
+		sample.header.stamp = stampOf(stamp);
+		sample.orientation.z = std::sin(yaw / 2.0);
+		sample.orientation.w = std::cos(yaw / 2.0);
+		return sample;
+	}
+
+	/**
+	 * @brief Publishes an IMU turning by kPredictedTurn between @p stamp and @p stamp + 0.1.
+	 *
+	 * The heading starts away from zero on purpose: RTAB-Map reads an orientation whose x,
+	 * y and z are all zero as "not set" and ignores the sample, so an IMU sitting at
+	 * exactly identity would leave the odometry with nothing to difference against and the
+	 * test would pass while exercising nothing.
+	 *
+	 * The whole history goes out before any scan does: with wait_imu_to_init a frame is
+	 * held back until an IMU sample at or after its stamp has arrived, and dropped when
+	 * the next frame arrives without one.
+	 */
+	bool publishImuTurn(double stamp, double keepTurningTo = 0.0)
+	{
+		imuTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped sensor;
+		sensor.header.stamp = helper()->now();
+		sensor.header.frame_id = "base_link";
+		sensor.child_frame_id = "imu_link";
+		sensor.transform.rotation.w = 1.0;
+		imuTf_->sendTransform(sensor);
+
+		rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+				helper()->create_publisher<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(200));
+		if(!waitForSubscriber(imu))
+		{
+			return false;
+		}
+		const double last = keepTurningTo > 0.0 ? stamp + 0.3 : stamp + 0.15;
+		for(double t = stamp - 0.1; t <= last; t += 0.01)
+		{
+			double turn = t <= stamp ? 0.0
+					: (t >= stamp + 0.1 ? kPredictedTurn : kPredictedTurn * (t - stamp) / 0.1);
+			if(keepTurningTo > 0.0 && t > stamp + 0.1)
+			{
+				// Carries on turning after the second frame's stamp, so that a guess built
+				// from the newest sample instead of the one at the stamp would show it.
+				const double past = std::min(1.0, (t - stamp - 0.1) / 0.2);
+				turn = kPredictedTurn + (keepTurningTo - kPredictedTurn) * past;
+			}
+			const double turned = kImuHeading + turn;
+			sensor_msgs::msg::Imu sample;
+			sample.header.frame_id = "imu_link";
+			sample.header.stamp = stampOf(t);
+			sample.orientation.z = std::sin(turned / 2.0);
+			sample.orientation.w = std::cos(turned / 2.0);
+			imu->publish(sample);
+		}
+		imuPublisher_ = imu;
+		spinFor(std::chrono::milliseconds(200));
+		return true;
+	}
+
+	/**
+	 * @brief Publishes wheel_odom -> base_link turning by kPredictedTurn, the same motion the
+	 *        IMU reports in publishImuTurn().
+	 *
+	 * This is the other way to hand the odometry a prediction: a pose source in TF rather
+	 * than an orientation on a topic. The node differences it between consecutive scan
+	 * stamps and passes the result to ICP as the guess.
+	 */
+	bool publishGuessTurn(double stamp)
+	{
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> echo =
+				collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		if(!waitForSubscriber(tf, 2))
+		{
+			return false;
+		}
+		size_t published = 0;
+		for(double t = stamp - 0.1; t <= stamp + 0.15; t += 0.01)
+		{
+			const double turned = t <= stamp ? 0.0
+					: (t >= stamp + 0.1 ? kPredictedTurn : kPredictedTurn * (t - stamp) / 0.1);
+			geometry_msgs::msg::TransformStamped pose;
+			pose.header.stamp = stampOf(t);
+			pose.header.frame_id = "wheel_odom";
+			pose.child_frame_id = "base_link";
+			pose.transform.rotation.z = std::sin(turned / 2.0);
+			pose.transform.rotation.w = std::cos(turned / 2.0);
+			tf2_msgs::msg::TFMessage message;
+			message.transforms.push_back(pose);
+			tf->publish(message);
+			if(++published % 10 == 0)
+			{
+				spinFor(std::chrono::milliseconds(5));
+			}
+		}
+		tfPublisher_ = tf;
+		return spinUntil([&]() { return echo->size() >= published; });
+	}
+
+	Recording readOusterRecording()
+	{
+		Recording recording;
+		recording.staticTransforms =
+				readBagMessages<tf2_msgs::msg::TFMessage>(ousterHalfTurnBag(), "/tf_static");
+		recording.transforms =
+				readBagMessages<tf2_msgs::msg::TFMessage>(ousterHalfTurnBag(), "/tf");
+		recording.clouds = readBagMessages<sensor_msgs::msg::PointCloud2>(
+				ousterHalfTurnBag(), "/os_cloud_node/points");
+		return recording;
+	}
+
+	/**
+	 * @brief Replays the recorded TF history, after the node under test exists.
+	 *
+	 * Order matters: /tf is a volatile topic, so transforms published before the node's
+	 * listener has subscribed are simply dropped and every lookup then fails with "TF of
+	 * received scan cloud is not set". /tf_static survives that (it is transient-local)
+	 * which makes the mistake look like a half-working tree rather than an empty one.
+	 *
+	 * @return false if the node never subscribed
+	 */
+	bool publishRecordedTf(const Recording & recording)
+	{
+		staticBroadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		for(const tf2_msgs::msg::TFMessage & message : recording.staticTransforms)
+		{
+			staticBroadcaster_->sendTransform(message.transforms);
+		}
+		tfPublisher_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>(
+				"/tf", rclcpp::QoS(200));
+		// Subscribe to the same topic, so delivery can be waited on rather than guessed
+		// at: a fixed pause is enough on an idle machine and not enough on a loaded one,
+		// and a cloud that arrives before the transforms is refused outright.
+		std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> echo =
+				collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		if(!waitForSubscriber(tfPublisher_, 2))   // the node's listener, and this echo
+		{
+			return false;
+		}
+		// In chunks, spinning in between, rather than all at once: tf2's listener reads
+		// /tf on its own thread with a bounded queue, and a burst of a hundred messages
+		// overflows it on a machine that cannot drain them -- dropping the oldest, which
+		// are exactly the ones covering the first cloud. The whole history still goes out
+		// before any cloud does.
+		size_t published = 0;
+		for(const tf2_msgs::msg::TFMessage & message : recording.transforms)
+		{
+			tfPublisher_->publish(message);
+			if(++published % 10 == 0)
+			{
+				spinFor(std::chrono::milliseconds(10));
+			}
+		}
+		return spinUntil([&]() { return echo->size() >= recording.transforms.size(); });
+	}
+
+private:
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticBroadcaster_;
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> imuTf_;
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPublisher_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPublisher_;
+};
+
+/**
+ * The first scan initializes odometry rather than registering anything: the pose is the
+ * identity and the covariance is RTAB-Map's "not estimated" value, not a real one.
+ */
+/**
+ * The first scan initializes odometry rather than registering anything: the pose is the
+ * identity, and it is the frame every later pose is relative to.
+ */
+TEST_F(IcpOdometryTest, publishes_an_identity_pose_for_the_first_scan_cloud)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+
+	const nav_msgs::msg::Odometry & msg = odom->back();
+	EXPECT_EQ("odom", msg.header.frame_id);
+	EXPECT_EQ("base_link", msg.child_frame_id);
+	EXPECT_NEAR(0.0, msg.pose.pose.position.x, 1e-6);
+	EXPECT_NEAR(0.0, msg.pose.pose.position.y, 1e-6);
+	EXPECT_NEAR(0.0, msg.pose.pose.position.z, 1e-6);
+}
+
+/// A 2D lidar goes in on `scan` instead of `scan_cloud`, and reaches the same odometry.
+TEST_F(IcpOdometryTest, accepts_a_laser_scan)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeLaserScan("lidar", 1.0));
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+}
+
+/**
+ * The point of the node: a second scan taken from a known offset comes back as that
+ * offset in the published odometry.
+ *
+ * The motion matches the one RTAB-Map's own Icp3DCornerRecoversMotionWithoutGuess uses --
+ * about 12 cm spread over three axes. Size matters here: a step much larger than
+ * Icp/MaxCorrespondenceDistance (0.1 m by default) leaves ICP with nothing to associate
+ * and it recovers nothing at all, which is the behaviour described under "When it loses
+ * track" in doc/icp_odometry.md.
+ *
+ * The scene is a corner, so the motion is fully constrained -- see "Degenerate geometry"
+ * for the environments where it is not.
+ */
+TEST_F(IcpOdometryTest, recovers_a_known_motion_between_two_scans)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; }));
+
+	// The robot moved by this much, so the corner is seen that much nearer.
+	const cv::Point3f motion(0.10f, 0.06f, 0.04f);
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(motion)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & msg = odom->back();
+	EXPECT_NEAR(motion.x, msg.pose.pose.position.x, 0.01);
+	EXPECT_NEAR(motion.y, msg.pose.pose.position.y, 0.01);
+	EXPECT_NEAR(motion.z, msg.pose.pose.position.z, 0.01);
+}
+
+/// Two steps in a row accumulate, rather than each being reported relative to the last.
+TEST_F(IcpOdometryTest, integrates_successive_motions_into_a_pose)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	for(int i=0; i<3; ++i)
+	{
+		pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0))));
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
+	}
+
+	// Three frames at 0, 0.05 and 0.10 m: the pose is the total, not the last step.
+	EXPECT_NEAR(0.10, odom->back().pose.pose.position.x, 0.01);
+}
+
+/**
+ * The scan filters default to RTAB-Map's own Icp parameter values rather than to the zeros the
+ * source's member initializers suggest. See "Where these defaults come from" in the doc.
+ */
+TEST_F(IcpOdometryTest, scan_filters_default_to_the_icp_parameter_values)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node = makeNode();
+
+	EXPECT_NEAR(0.05, node->get_parameter("scan_voxel_size").as_double(), 1e-6);
+	EXPECT_EQ(5, node->get_parameter("scan_normal_k").as_int());
+}
+
+/// Setting the ROS parameter explicitly takes precedence over the Icp/* value.
+TEST_F(IcpOdometryTest, an_explicit_scan_voxel_size_wins_over_the_icp_parameter)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node =
+			makeNode({rclcpp::Parameter("scan_voxel_size", 0.25)});
+
+	EXPECT_NEAR(0.25, node->get_parameter("scan_voxel_size").as_double(), 1e-6);
+}
+
+/// odom_info carries the registration result, and is only built when something subscribes.
+TEST_F(IcpOdometryTest, publishes_odom_info_describing_the_registration)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !info->empty(); }));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0.0f, 0.0f))));
+	ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
+
+	// The second frame registered against the first, so the scan map is populated and
+	// correspondences were found.
+	const rtabmap_msgs::msg::OdomInfo & msg = *info->messages[1];
+	EXPECT_FALSE(msg.lost);
+	EXPECT_GT(msg.local_scan_map_size, 0);
+	EXPECT_GT(msg.icp_correspondences, 0);
+}
+
+
+// ---------------------------------------------------------------------------
+// Deskewing, against a real rotating lidar: test/data/lidar/ouster_pair.
+//
+// The Ouster sits on a mast that turns on a dynamixel joint while the base stays put, so
+// the sensor moves through its own 0.1 s sweep and the cloud comes off the driver skewed
+// -- points recorded early in the sweep are expressed in a pose the sensor has already
+// left. Deskewing undoes that from TF, and doing it wrong is invisible in a synthetic
+// scene where the sensor is motionless within a sweep.
+//
+// The recording carries the per-point `t` field deskewing needs, plus TF from 0.1 s
+// before the first sweep to past the end of the last one.
+// ---------------------------------------------------------------------------
+
+/// The fixture is worthless if the recording is not there, so say so plainly.
+TEST_F(IcpOdometryTest, the_recorded_lidar_pair_is_readable)
+{
+	const std::vector<sensor_msgs::msg::PointCloud2> clouds =
+			readBagMessages<sensor_msgs::msg::PointCloud2>(
+					ousterHalfTurnBag(), "/os_cloud_node/points");
+
+	ASSERT_EQ(2u, clouds.size()) << "expected two clouds in " << ousterHalfTurnBag();
+	EXPECT_EQ("os_sensor", clouds[0].header.frame_id);
+	EXPECT_EQ(1024u, clouds[0].width);
+	EXPECT_EQ(32u, clouds[0].height);
+
+	// Deskewing needs a per-point time offset; without this field it refuses the cloud.
+	bool hasTime = false;
+	for(const sensor_msgs::msg::PointField & field : clouds[0].fields)
+	{
+		hasTime = hasTime || field.name == "t";
+	}
+	EXPECT_TRUE(hasTime) << "the cloud has no per-point t field to deskew with";
+}
+
+/**
+ * deskewing:=true with a fixed frame: the node corrects each sweep against TF before
+ * registering it, and both clouds come back as odometry in base_link.
+ */
+TEST_F(IcpOdometryTest, deskews_a_rotating_lidar_sweep_against_tf)
+{
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom_deskewed");
+	const Recording recording = readOusterRecording();
+	ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag();
+
+	// guess_frame_id is what selects the TF path: with only two clouds there is no
+	// velocity estimate yet, so the constant-velocity fallback would never run. base_link
+	// is the rig's root -- the mast turns relative to it, which is the motion to undo.
+	makeNode({rclcpp::Parameter("deskewing", true),
+	          rclcpp::Parameter("guess_frame_id", "base_link"),
+	          rclcpp::Parameter("scan_cloud_max_points", 65536),
+	          // The room is metres across and the sweeps start half a turn apart.
+	          rclcpp::Parameter("scan_voxel_size", 0.2),
+	          rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"),
+	          // Pinned: a build without libpointmatcher defaults it to false.
+	          rclcpp::Parameter("Icp/PointToPlane", "true"),
+	          // Raised from 0.2 m, which the uncorrected run walks past and is refused for.
+	          rclcpp::Parameter("Icp/MaxTranslation", "0.5"),
+	          // The recorded transforms may still be arriving when the first cloud lands.
+	          rclcpp::Parameter("wait_for_transform", 2.0)},
+	         "deskewed");
+	ASSERT_TRUE(publishRecordedTf(recording));
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(recording.clouds[0]);
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }))
+			<< "the first deskewed sweep produced no odometry";
+	pub->publish(recording.clouds[1]);
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }))
+			<< "the second deskewed sweep produced no odometry";
+
+	EXPECT_EQ("odom", odom->back().header.frame_id);
+	EXPECT_EQ("base_link", odom->back().child_frame_id);
+	ASSERT_FALSE(isLost(odom->back())) << "lost tracking between the two sweeps";
+
+	// The ground truth is known: the platform never moves in this recording, only the
+	// mast turns, so base_link is where it started and the pose should be the identity.
+	// What is left after deskewing half a turn of rotation out of two sweeps is 0.013 m
+	// and 0.012 rad with libpointmatcher, 0.09 m and 0.015 rad with PCL -- point to
+	// plane either way, which is why the parameter above is not left to its default.
+	EXPECT_LT(translationNorm(odom->back()), 0.15)
+			<< "the platform never moved; this is too far from the origin";
+	EXPECT_LT(rotationAngle(odom->back()), 0.05)
+			<< "the platform never turned; this is too far from the origin";
+}
+
+/**
+ * The same two sweeps with and without deskewing, registered side by side.
+ *
+ * The platform never moved, so the answer is known: the identity. Corrected, the pair
+ * lands 0.013 m and 0.012 rad from it with libpointmatcher and 0.09 m with PCL.
+ * Uncorrected, it still registers -- on nearly as many points -- but arrives several
+ * times further out. That is the shape of a deskewing bug in the field: not a failure, a
+ * quietly worse answer.
+ *
+ * The correction itself is measured too, on the scan each node republishes on
+ * odom_filtered_input_scan. That one does not depend on the backend at all, so it is the
+ * assertion that catches a deskewing step that quietly stopped working.
+ *
+ * At RTAB-Map's default Icp/MaxTranslation of 0.2 m the uncorrected run does not even get
+ * that far: libpointmatcher aborts with "limit out of bounds: tr 0.214016/0.2" and the
+ * pose comes back unusable. The limit is raised here so that both runs produce a number
+ * to compare, which says more than one of them failing.
+ */
+TEST_F(IcpOdometryTest, deskewing_is_what_lets_a_half_turn_pair_register)
+{
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> deskewed =
+			collect<nav_msgs::msg::Odometry>("odom_deskewed");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> asRecorded =
+			collect<nav_msgs::msg::Odometry>("odom_as_recorded");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> deskewedInfo =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info_deskewed");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> asRecordedInfo =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info_as_recorded");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> deskewedScan =
+			collect<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan_deskewed");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> asRecordedScan =
+			collect<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan_as_recorded");
+	const Recording recording = readOusterRecording();
+	ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag();
+
+	// Identical in every respect but the one parameter.
+	makeNode({rclcpp::Parameter("deskewing", true),
+	          rclcpp::Parameter("guess_frame_id", "base_link"),
+	          rclcpp::Parameter("scan_cloud_max_points", 65536),
+	          // The room is metres across and the sweeps start half a turn apart.
+	          rclcpp::Parameter("scan_voxel_size", 0.2),
+	          rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"),
+	          // Pinned: a build without libpointmatcher defaults it to false.
+	          rclcpp::Parameter("Icp/PointToPlane", "true"),
+	          // Raised from 0.2 m, which the uncorrected run walks past and is refused for.
+	          rclcpp::Parameter("Icp/MaxTranslation", "0.5"),
+	          // The recorded transforms may still be arriving when the first cloud lands.
+	          rclcpp::Parameter("wait_for_transform", 2.0)},
+	         "deskewed");
+	makeNode({rclcpp::Parameter("deskewing", false),
+	          rclcpp::Parameter("guess_frame_id", "base_link"),
+	          rclcpp::Parameter("scan_cloud_max_points", 65536),
+	          rclcpp::Parameter("scan_voxel_size", 0.2),
+	          rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"),
+	          rclcpp::Parameter("Icp/PointToPlane", "true"),
+	          rclcpp::Parameter("Icp/MaxTranslation", "0.5"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)},
+	         "as_recorded");
+	ASSERT_TRUE(publishRecordedTf(recording));
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub, 2));
+	ASSERT_TRUE(waitForPublisher(deskewedScan->subscription));
+	ASSERT_TRUE(waitForPublisher(asRecordedScan->subscription));
+
+	pub->publish(recording.clouds[0]);
+	ASSERT_TRUE(spinUntil([&]() { return !deskewed->empty() && !asRecorded->empty(); }));
+	pub->publish(recording.clouds[1]);
+	ASSERT_TRUE(spinUntil([&]() {
+		return deskewed->size() >= 2 && asRecorded->size() >= 2 &&
+		       deskewedInfo->size() >= 2 && asRecordedInfo->size() >= 2; }));
+
+	ASSERT_FALSE(isLost(deskewed->back())) << "the deskewed pair failed to register";
+	EXPECT_GT(deskewedInfo->back().icp_inliers_ratio, 0.1f)
+			<< "the deskewed pair barely matched itself, so something else is wrong";
+
+	// The correction itself: the same sweep, as the two nodes handed it to ICP. Half a
+	// turn of mast rotation across one sweep moves the far end of it by a good fraction
+	// of a metre, and a deskewing step that stopped working would leave these two clouds
+	// on top of each other.
+	ASSERT_FALSE(deskewedScan->empty()) << "the deskewed run republished no scan";
+	ASSERT_FALSE(asRecordedScan->empty()) << "the uncorrected run republished no scan";
+	// The control: the measure has to say every point of a cloud corresponds to itself,
+	// so a number well below that is the two clouds genuinely having moved apart rather
+	// than the measurement being broken.
+	EXPECT_NEAR(1.0, correspondenceRatio(deskewedScan->back(), deskewedScan->back(), 0.01),
+			1e-6) << "the correspondence measure does not even match a cloud to itself";
+
+	// How far apart the two are depends on PCL's correspondence estimation, which is not
+	// the same from one version to the next -- 0.12 with 1.12 against 0.07 with 1.15 --
+	// so the bound is loose. A deskewing step that stopped working would leave the two
+	// clouds identical and put this back at 1.0, which is what it is here to catch.
+	const double stillTogether =
+			correspondenceRatio(deskewedScan->back(), asRecordedScan->back(), 0.01);
+	EXPECT_LT(stillTogether, 0.5)
+			<< stillTogether*100.0 << "% of the deskewed scan is still within a centimetre "
+			   "of the uncorrected one, so the correction never reached the cloud";
+
+	// Both runs register, so this part of the claim is about accuracy rather than
+	// survival.
+	EXPECT_LT(translationNorm(deskewed->back()), 0.15)
+			<< "the platform never moved; the deskewed estimate should say so";
+	EXPECT_LT(rotationAngle(deskewed->back()), 0.05)
+			<< "the platform never turned; the deskewed estimate should say so";
+	if(isLost(asRecorded->back()))
+	{
+		// It failed outright instead -- an even stronger version of the same claim.
+		SUCCEED() << "the uncorrected pair could not be registered at all";
+	}
+	else
+	{
+		EXPECT_GT(translationNorm(asRecorded->back()), translationNorm(deskewed->back()) * 1.1)
+				<< "deskewing did not improve the translation error (deskewed="
+				<< translationNorm(deskewed->back()) << " m, as recorded="
+				<< translationNorm(asRecorded->back()) << " m)";
+		EXPECT_GT(rotationAngle(asRecorded->back()), rotationAngle(deskewed->back()) * 1.1)
+				<< "deskewing did not improve the rotation error (deskewed="
+				<< rotationAngle(deskewed->back()) << " rad, as recorded="
+				<< rotationAngle(asRecorded->back()) << " rad)";
+	}
+}
+
+
+
+/**
+ * The same turn again, predicted from TF instead of an IMU: guess_frame_id names a pose
+ * source, the node differences it between the two scan stamps, and ICP gets the same
+ * 0.35 rad guess it got from the IMU. Different input, same prediction.
+ */
+TEST_F(IcpOdometryTest, takes_the_rotation_of_its_guess_from_the_guess_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("guess_frame_id", "wheel_odom"));
+	params.push_back(rclcpp::Parameter("wait_for_transform", 2.0));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishGuessTurn(1.0));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "no odometry for the first scan";
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }))
+			<< "no odometry for the second scan";
+
+	// The same guess the IMU produced: 0.35 rad of rotation, no translation.
+	EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.01)
+			<< "the guess handed to ICP did not come from the guess frame";
+	EXPECT_NEAR(0.0, info->back().guess.translation.x, 1e-6);
+	EXPECT_NEAR(0.0, info->back().guess.translation.y, 1e-6);
+	EXPECT_NEAR(0.0, info->back().guess.translation.z, 1e-6);
+
+	ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge";
+	// The pose is the turn itself, where the IMU version also carries kImuHeading: both
+	// seed the first pose from their prediction, and this one starts at the identity.
+	EXPECT_NEAR(kPredictedTurn, rotationAngle(odom->back()), 0.02);
+}
+
+
+/**
+ * A frame stamped ahead of every IMU sample in the buffer is held, not processed: the
+ * odometry would otherwise register it without the orientation that belongs to it. It
+ * comes out as soon as an IMU sample reaches its stamp.
+ */
+TEST_F(IcpOdometryTest, holds_a_frame_until_an_imu_covers_its_stamp)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("wait_imu_to_init", true));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(200));
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForSubscriber(imu));
+	publishImuTf();
+
+	// IMU up to 1.0 only...
+	for(double t = 0.9; t <= 1.0; t += 0.01)
+	{
+		imu->publish(imuSample(t, kImuHeading));
+	}
+	spinFor(std::chrono::milliseconds(200));
+
+	// ...and a frame stamped after all of it.
+	pub->publish(makeXYZCloud("lidar", 1.05, corner3D()));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(odom->empty())
+			<< "the frame was registered before any IMU covered its stamp";
+
+	// One sample at or past the frame's stamp releases it.
+	imu->publish(imuSample(1.06, kImuHeading));
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }))
+			<< "the held frame was never processed once the IMU caught up";
+}
+
+/**
+ * The orientation handed to the odometry is the one belonging to the frame's stamp, not
+ * whatever the IMU has reached by the time the frame is processed. Here the IMU keeps
+ * turning well past the second frame -- to 2.0 rad, twice the turn between the scans --
+ * and the guess still comes out at the turn the frame saw.
+ */
+TEST_F(IcpOdometryTest, uses_the_orientation_at_the_frame_stamp_not_the_newest_one)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("wait_imu_to_init", true));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishImuTurn(1.0, 2.0));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	// 1.05, the turn between the two stamps -- not the 2.0 the IMU has reached by then.
+	EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.02)
+			<< "the guess did not correspond to the frame's own stamp";
+	ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge";
+	EXPECT_NEAR(kImuHeading + kPredictedTurn, rotationAngle(odom->back()), 0.02);
+}
+
+// ---------------------------------------------------------------------------
+// 2D scan deskewing: a lidar on a robot driving at a corner.
+// ---------------------------------------------------------------------------
+
+/**
+ * The LaserScan path through icp_odometry, with deskewing against a fixed frame.
+ *
+ * The robot drives at the corner at 1 m/s and stops just before the second scan, so the
+ * first sweep is bent and the second is straight. Deskewed, both describe the same corner
+ * and the registration returns the distance actually travelled between the two stamps.
+ */
+TEST_F(IcpOdometryTest, deskews_a_2d_scan_against_a_fixed_frame)
+{
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom_deskewed");
+	makeNode({rclcpp::Parameter("deskewing", true),
+	          rclcpp::Parameter("guess_frame_id", "odom"),
+	          rclcpp::Parameter("Icp/PointToPlane", "false"),
+	          rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
+	          rclcpp::Parameter("Icp/MaxTranslation", "0.0"),
+	          rclcpp::Parameter("Reg/Force3DoF", "true"),
+	          rclcpp::Parameter("scan_voxel_size", 0.0),
+	          rclcpp::Parameter("wait_for_transform", 2.0)},
+	         "deskewed");
+	ASSERT_TRUE(publishRobotTrajectory());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeSkewedCornerScan(kFirstScan));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "the first scan produced nothing";
+	pub->publish(makeSkewedCornerScan(kSecondScan));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; })) << "the second scan produced nothing";
+
+	ASSERT_FALSE(isLost(odom->back())) << "the deskewed pair failed to register";
+	// 0.399909 m against a truth of 0.4, and 5e-5 m sideways, repeatable to the last
+	// digit. A centimetre of tolerance leaves room for a different ICP backend without
+	// leaving room for the 5 cm error an unskewed registration makes on this scene.
+	EXPECT_NEAR(trueDisplacement(), odom->back().pose.pose.position.x, 0.01)
+			<< "the robot drove " << trueDisplacement() << " m between the two scans";
+	EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.01)
+			<< "it drove straight at the corner, so there is no sideways motion to find";
+}
+
+
+/**
+ * The same two scans with deskewing off, side by side with a node that has it on.
+ *
+ * The first sweep is bent by the motion and the second is not, so an uncorrected
+ * registration is matching two different shapes and pays for it in the estimate.
+ */
+TEST_F(IcpOdometryTest, deskewing_a_2d_scan_changes_what_is_registered)
+{
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> deskewed =
+			collect<nav_msgs::msg::Odometry>("odom_deskewed");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> asScanned =
+			collect<nav_msgs::msg::Odometry>("odom_as_scanned");
+	const std::vector<rclcpp::Parameter> icp = {
+		rclcpp::Parameter("guess_frame_id", "odom"),
+		rclcpp::Parameter("Icp/PointToPlane", "false"),
+		rclcpp::Parameter("Icp/CorrespondenceRatio", "0.1"),
+		rclcpp::Parameter("Icp/MaxTranslation", "0.0"),
+		rclcpp::Parameter("Reg/Force3DoF", "true"),
+		rclcpp::Parameter("scan_voxel_size", 0.0),
+		rclcpp::Parameter("wait_for_transform", 2.0)};
+	std::vector<rclcpp::Parameter> on = icp, off = icp;
+	on.push_back(rclcpp::Parameter("deskewing", true));
+	off.push_back(rclcpp::Parameter("deskewing", false));
+	makeNode(on, "deskewed");
+	makeNode(off, "as_scanned");
+	ASSERT_TRUE(publishRobotTrajectory());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub, 2));
+
+	pub->publish(makeSkewedCornerScan(kFirstScan));
+	ASSERT_TRUE(spinUntil([&]() { return !deskewed->empty() && !asScanned->empty(); }));
+	pub->publish(makeSkewedCornerScan(kSecondScan));
+	ASSERT_TRUE(spinUntil([&]() {
+		return deskewed->size() >= 2 && asScanned->size() >= 2; }));
+
+	ASSERT_FALSE(isLost(deskewed->back()));
+	ASSERT_FALSE(isLost(asScanned->back()))
+			<< "the uncorrected pair was expected to register, just badly";
+
+	const double deskewedError =
+			std::fabs(deskewed->back().pose.pose.position.x - trueDisplacement());
+	const double skewedError =
+			std::fabs(asScanned->back().pose.pose.position.x - trueDisplacement());
+
+	// Measured: 0.0001 m corrected against 0.0499 m uncorrected. That 5 cm is half the
+	// 0.1 m the robot covered during the bent sweep, which is what it costs to match a
+	// bent corner against a straight one.
+	EXPECT_LT(deskewedError, 0.01) << "deskewed estimate is " << deskewed->back().pose.pose.position.x;
+	EXPECT_GT(skewedError, 0.02)
+			<< "the uncorrected registration was as good as the corrected one, so "
+			   "deskewing never reached the scan (deskewed error=" << deskewedError
+			<< " m, uncorrected error=" << skewedError << " m)";
+	EXPECT_GT(skewedError, deskewedError * 5.0) << "deskewing barely improved the estimate";
+}
+
+
+// ---------------------------------------------------------------------------
+// IMU: the orientation replaces the rotation of the odometry's prediction.
+//
+// RTAB-Map builds a guess for ICP from a constant-velocity model, and when an IMU with a
+// valid orientation is available it keeps that model's translation but takes the rotation
+// from the IMU ("replace orientation guess with IMU" in Odometry::process). On the second
+// frame there is no velocity yet, so the guess is the IMU's rotation and nothing else --
+// which is exactly what odom_info reports.
+//
+// The IMU topic exists only when wait_imu_to_init is set; without it the node never
+// subscribes and every scan is registered from an identity guess.
+// ---------------------------------------------------------------------------
+
+/**
+ * The sensor turns 0.35 rad between two scans and an IMU says so: the guess handed to ICP
+ * carries that rotation and no translation, and the registration lands on it.
+ */
+TEST_F(IcpOdometryTest, takes_the_rotation_of_its_guess_from_the_imu)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("wait_imu_to_init", true));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishImuTurn(1.0));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "no odometry for the first scan";
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }))
+			<< "no odometry for the second scan";
+
+	// The guess is the IMU's change of orientation, exactly: 0.35 rad.
+	EXPECT_NEAR(kPredictedTurn, rotationAngleOf(info->back().guess.rotation), 0.01)
+			<< "the guess handed to ICP did not come from the IMU";
+	// ...and the constant-velocity model contributes nothing to it yet, there being no
+	// velocity to speak of after a single frame.
+	EXPECT_NEAR(0.0, info->back().guess.translation.x, 1e-6);
+	EXPECT_NEAR(0.0, info->back().guess.translation.y, 1e-6);
+	EXPECT_NEAR(0.0, info->back().guess.translation.z, 1e-6);
+
+	ASSERT_FALSE(isLost(odom->back())) << "the registration did not converge";
+	// The pose also carries the heading the IMU started from: RTAB-Map seeds the first
+	// pose with the IMU orientation, so this is kImuHeading + kPredictedTurn, not kPredictedTurn.
+	EXPECT_NEAR(kImuHeading + kPredictedTurn, rotationAngle(odom->back()), 0.02);
+}
+
+/**
+ * The same two scans with no IMU: the guess carries no rotation at all.
+ *
+ * ICP then fails to find the turn, on either backend: it settles about 0.01 rad from no
+ * motion at all and reports that as a successful registration. How it fails does vary --
+ * at smaller turns libpointmatcher trips Icp/MaxTranslation and returns an unusable pose
+ * while PCL converges correctly -- so the test asserts only that the turn was not
+ * recovered, not the manner of it.
+ */
+TEST_F(IcpOdometryTest, without_an_imu_the_guess_carries_no_rotation)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3DTurned(kPredictedTurn)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	// Null or identity, but in no case the turn: with no IMU there is nothing to predict
+	// a rotation from, and no velocity yet either.
+	EXPECT_GT(std::fabs(rotationAngleOf(info->back().guess.rotation) - kPredictedTurn), 0.1)
+			<< "the guess carried the turn with no IMU to supply it";
+
+	// And without it the registration does not find the turn: measured at 0.009 rad on
+	// PCL and 0.010 on libpointmatcher, against a real 1.05. Neither reports failure --
+	// they settle on "barely moved", which is the quiet way this goes wrong in the field.
+	if(!isLost(odom->back()))
+	{
+		EXPECT_GT(std::fabs(rotationAngle(odom->back()) - kPredictedTurn), 0.5)
+				<< "ICP recovered the turn from an identity guess, which would make the "
+				   "prediction tested above unnecessary";
+	}
+}
+
+
+// ---------------------------------------------------------------------------
+// What the node hands downstream, and two parameters that change it.
+// ---------------------------------------------------------------------------
+
+/**
+ * The scan republished on odom_sensor_data is the one ICP registered -- after
+ * voxelization -- not the sweep the lidar published. See "Reusing the filtered scan
+ * downstream" in doc/icp_odometry.md.
+ */
+TEST_F(IcpOdometryTest, republishes_the_filtered_scan_rather_than_the_input)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_voxel_size", 0.5));   // coarse, on a 4 m corner
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(makeXYZCloud("lidar", 1.0, points));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty() && !data->empty(); }));
+
+	// 1200 points in, 243 out at a 0.5 m voxel on a 4 m corner.
+	EXPECT_GT(data->back().laser_scan.width, 0u) << "no scan was republished at all";
+	EXPECT_LT(data->back().laser_scan.width, points.size() / 2)
+			<< "the republished scan still has the input's density, so it is the input";
+}
+
+/**
+ * scan_cloud_is_2d says a cloud carrying a z field is a planar scan after all, so it is
+ * registered -- and republished -- as 2D. The scan format says which it was: 3
+ * (kXYNormal) against 8 (3D with normals) for the same cloud.
+ */
+TEST_F(IcpOdometryTest, scan_cloud_is_2d_registers_a_cloud_as_a_planar_scan)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> planar =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data_planar/raw");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> volume =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data_volume/raw");
+	std::vector<rclcpp::Parameter> flat = icpTestParameters();
+	std::vector<rclcpp::Parameter> spatial = icpTestParameters();
+	flat.push_back(rclcpp::Parameter("scan_cloud_is_2d", true));
+	spatial.push_back(rclcpp::Parameter("scan_cloud_is_2d", false));
+	makeNode(flat, "planar");
+	makeNode(spatial, "volume");
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub, 2));
+
+	// A flat corner: two walls, every point at z = 0, but the cloud still carries a z field.
+	std::vector<cv::Point3f> points;
+	for(int i=0; i<200; ++i)
+	{
+		points.push_back(cv::Point3f(-2.0f + 0.02f*i, -2.0f, 0.0f));
+		points.push_back(cv::Point3f(-2.0f, -2.0f + 0.02f*i, 0.0f));
+	}
+	pub->publish(makeXYZCloud("lidar", 1.0, points));
+	ASSERT_TRUE(spinUntil([&]() { return !planar->empty() && !volume->empty(); }));
+
+	// LaserScan::Format numbers the 2D layouts 1 to 4 and the 3D ones from 5 up.
+	EXPECT_LT(planar->back().laser_scan_format, 5)
+			<< "the cloud was registered as 3D despite scan_cloud_is_2d";
+	EXPECT_GE(volume->back().laser_scan_format, 5)
+			<< "the same cloud should be 3D without the parameter";
+}
+
+/**
+ * deskewing_slerp interpolates the correction between the ends of the sweep instead of
+ * looking TF up for every point -- cheaper, and per the documentation slightly less
+ * accurate. It has to land in the same place.
+ */
+TEST_F(IcpOdometryTest, deskewing_slerp_gives_the_same_answer_as_the_per_point_lookup)
+{
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> slerp =
+			collect<nav_msgs::msg::Odometry>("odom_slerp");
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> perPoint =
+			collect<nav_msgs::msg::Odometry>("odom_perpoint");
+	const Recording recording = readOusterRecording();
+	ASSERT_TRUE(recording.valid()) << "could not read " << ousterHalfTurnBag();
+
+	const std::vector<rclcpp::Parameter> common = {
+		rclcpp::Parameter("deskewing", true),
+		rclcpp::Parameter("guess_frame_id", "base_link"),
+		rclcpp::Parameter("scan_cloud_max_points", 65536),
+		rclcpp::Parameter("scan_voxel_size", 0.2),
+		rclcpp::Parameter("Icp/MaxCorrespondenceDistance", "2.0"),
+		rclcpp::Parameter("Icp/MaxTranslation", "0.5"),
+		rclcpp::Parameter("wait_for_transform", 2.0)};
+	std::vector<rclcpp::Parameter> a = common, b = common;
+	a.push_back(rclcpp::Parameter("deskewing_slerp", true));
+	b.push_back(rclcpp::Parameter("deskewing_slerp", false));
+	makeNode(a, "slerp");
+	makeNode(b, "perpoint");
+	ASSERT_TRUE(publishRecordedTf(recording));
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub, 2));
+
+	pub->publish(recording.clouds[0]);
+	ASSERT_TRUE(spinUntil([&]() { return !slerp->empty() && !perPoint->empty(); }));
+	pub->publish(recording.clouds[1]);
+	ASSERT_TRUE(spinUntil([&]() {
+		return slerp->size() >= 2 && perPoint->size() >= 2; }));
+
+	ASSERT_FALSE(isLost(slerp->back())) << "the interpolated deskew failed to register";
+	ASSERT_FALSE(isLost(perPoint->back()));
+	// 0.0117 m against 0.0131 m, and the rotations agree to four decimals: "slightly less
+	// accurate", as documented, and nowhere near a different answer.
+	EXPECT_NEAR(translationNorm(perPoint->back()), translationNorm(slerp->back()), 0.01)
+			<< "interpolating the correction moved the estimate";
+	EXPECT_NEAR(rotationAngle(perPoint->back()), rotationAngle(slerp->back()), 0.01);
+}
+
+
+// ---------------------------------------------------------------------------
+// The fields a driver puts in its cloud, and the filters the node runs on them.
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief A cloud carrying intensity, normals, both, or neither.
+ *
+ * Each combination is a different PCL point type inside the node, so a driver that
+ * publishes everything its lidar measured is handled by different code from one that
+ * publishes bare XYZ. All four describe the same corner, so all four have to end at the
+ * same pose. scan_downsampling_step is on throughout: the step is applied in each of the
+ * four, and halving the cloud is visible in what comes back out.
+ */
+class IcpOdometryCloudFieldsTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<std::tuple<bool, bool>>
+{
+};
+
+TEST_P(IcpOdometryCloudFieldsTest, recovers_the_motion_whatever_fields_the_cloud_carries)
+{
+	const bool intensity = std::get<0>(GetParam());
+	const bool normals = std::get<1>(GetParam());
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_downsampling_step", 2));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(makeCloudWithFields("lidar", 1.0, points, intensity, normals));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1 && data->size() >= 1; }));
+
+	const cv::Point3f motion(0.10f, 0.06f, 0.04f);
+	pub->publish(makeCloudWithFields("lidar", 1.1, corner3D(motion), intensity, normals));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & msg = odom->back();
+	EXPECT_NEAR(motion.x, msg.pose.pose.position.x, 0.015);
+	EXPECT_NEAR(motion.y, msg.pose.pose.position.y, 0.015);
+	EXPECT_NEAR(motion.z, msg.pose.pose.position.z, 0.015);
+	EXPECT_EQ(points.size()/2, data->front().laser_scan.width)
+			<< "scan_downsampling_step:=2 should have kept every second point";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		OptionalFields,
+		IcpOdometryCloudFieldsTest,
+		::testing::Combine(::testing::Bool(), ::testing::Bool()),
+		[](const ::testing::TestParamInfo<std::tuple<bool, bool>> & info) {
+			return std::string(std::get<0>(info.param) ? "intensity" : "no_intensity") +
+					(std::get<1>(info.param) ? "_normals" : "_no_normals");
+		});
+
+/**
+ * @brief A cloud that says it is not dense, with and without intensity.
+ *
+ * is_dense:=false is a driver saying some of these points are NaN -- a ray that hit
+ * nothing. The node drops them rather than handing NaNs to ICP.
+ */
+class IcpOdometryDenseTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<bool>
+{
+};
+
+TEST_P(IcpOdometryDenseTest, drops_the_invalid_points_of_a_cloud_that_is_not_dense)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	std::vector<cv::Point3f> points = corner3D();
+	const size_t valid = points.size();
+	const float nan = std::numeric_limits<float>::quiet_NaN();
+	points.insert(points.end(), 100, cv::Point3f(nan, nan, nan));
+	sensor_msgs::msg::PointCloud2 cloud =
+			makeCloudWithFields("lidar", 1.0, points, GetParam(), false);
+	cloud.is_dense = false;
+	pub->publish(cloud);
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_EQ(valid, data->back().laser_scan.width)
+			<< "the 100 NaN points were registered along with the real ones";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		OptionalFields,
+		IcpOdometryDenseTest,
+		::testing::Bool(),
+		[](const ::testing::TestParamInfo<bool> & info) {
+			return info.param ? "intensity" : "no_intensity";
+		});
+
+/**
+ * An organized cloud -- one row per laser ring -- tells the node how many points a full
+ * sweep has, so scan_cloud_max_points does not have to be given at all.
+ */
+TEST_F(IcpOdometryTest, takes_scan_cloud_max_points_from_an_organized_cloud)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(organized(makeXYZCloud("lidar", 1.0, points), 80));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_EQ(int(points.size()), data->back().laser_scan_max_pts);
+}
+
+/// A value too small for the cloud that arrived is raised to it rather than believed.
+TEST_F(IcpOdometryTest, raises_scan_cloud_max_points_to_the_size_of_an_organized_cloud)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_cloud_max_points", 10));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(organized(makeXYZCloud("lidar", 1.0, points), 80));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_EQ(int(points.size()), data->back().laser_scan_max_pts);
+}
+
+/**
+ * scan_range_min and scan_range_max cut the cloud down to a shell around the sensor. The
+ * corner spans 2.0 m to 3.5 m from the origin, so a 2.6-3.0 m window keeps part of it.
+ */
+TEST_F(IcpOdometryTest, keeps_only_the_cloud_points_within_the_range_limits)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_range_min", 2.6));
+	params.push_back(rclcpp::Parameter("scan_range_max", 3.0));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(makeXYZCloud("lidar", 1.0, points));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole cloud was filtered away";
+	EXPECT_LT(data->back().laser_scan.width, points.size())
+			<< "nothing outside 2.6-3.0 m was dropped";
+}
+
+/**
+ * scan_normal_ground_up turns the normals a driver sent toward a viewpoint 10 m above the
+ * sensor, so the ground's point up the way ICP expects rather than into the road. Here
+ * every normal arrives pointing down, and the scan republished on
+ * odom_filtered_input_scan is the one the node registered -- normals included, so it says
+ * which way they ended up. The second node is the control: without the parameter the node
+ * does not touch them at all.
+ */
+TEST_F(IcpOdometryTest, turns_the_normals_of_a_cloud_toward_a_viewpoint_above)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> aligned =
+			collect<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan_aligned");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> asIs =
+			collect<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan_as_is");
+	std::vector<rclcpp::Parameter> up = icpTestParameters();
+	up.push_back(rclcpp::Parameter("scan_normal_ground_up", 0.8));
+	makeNode(up, "aligned");
+	makeNode(icpTestParameters(), "as_is");
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub, 2));
+	ASSERT_TRUE(waitForPublisher(aligned->subscription));
+	ASSERT_TRUE(waitForPublisher(asIs->subscription));
+
+	pub->publish(makeCloudWithFields("lidar", 1.0, corner3D(), false, true,
+			cv::Point3f(0, 0, -1)));
+	ASSERT_TRUE(spinUntil([&]() { return !aligned->empty() && !asIs->empty(); }));
+
+	const std::vector<float> turned = fieldValues(aligned->back(), "normal_z");
+	ASSERT_FALSE(turned.empty()) << "the republished scan carries no normals";
+	EXPECT_EQ(turned.size(), countPositive(turned)) << "some normals still point down";
+
+	const std::vector<float> untouched = fieldValues(asIs->back(), "normal_z");
+	ASSERT_EQ(turned.size(), untouched.size());
+	EXPECT_EQ(0u, countPositive(untouched))
+			<< "the normals were turned over without scan_normal_ground_up being set";
+}
+
+/// What goes out on odom_filtered_input_scan is the filtered scan, not the input.
+TEST_F(IcpOdometryTest, publishes_the_scan_it_registered_on_odom_filtered_input_scan)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> filtered =
+			collect<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_voxel_size", 0.5));   // coarse, on a 4 m corner
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(filtered->subscription));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(makeXYZCloud("lidar", 1.0, points));
+	ASSERT_TRUE(spinUntil([&]() { return !filtered->empty(); }));
+
+	EXPECT_EQ("lidar", filtered->back().header.frame_id);
+	EXPECT_GT(filtered->back().width, 0u) << "nothing was republished at all";
+	EXPECT_LT(filtered->back().width, points.size()/2)
+			<< "the republished scan still has the input's density, so it is the input";
+}
+
+/**
+ * @brief Deskewing without a guess frame, on a cloud already in frame_id and on one that
+ * is not.
+ *
+ * With no fixed frame to look the motion up in, the node deskews against its own constant
+ * velocity model, which lives in frame_id: a cloud published in the sensor's frame has to
+ * be carried into frame_id and back, and one already there is deskewed where it lies. The
+ * velocity only exists once two frames have registered, so the correction starts on the
+ * third.
+ */
+class IcpOdometryDeskewFrameTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<std::string>
+{
+};
+
+TEST_P(IcpOdometryDeskewFrameTest, deskews_against_its_own_velocity)
+{
+	const std::string frame = GetParam();
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("deskewing", true));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	for(int i=0; i<4; ++i)
+	{
+		pub->publish(makeCloudWithFields(frame, 1.0 + 0.1*i,
+				corner3D(cv::Point3f(0.05f*i, 0, 0)), false, false,
+				cv::Point3f(0, 0, 1), true));
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "no odometry for the cloud at " << (1.0 + 0.1*i) << "s";
+	}
+
+	// Four frames 0.05 m apart, and a sweep short enough that deskewing them barely moves
+	// anything: the point is that the correction ran, not that it changed the answer.
+	EXPECT_NEAR(0.15, odom->back().pose.pose.position.x, 0.02);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		CloudFrame,
+		IcpOdometryDeskewFrameTest,
+		::testing::Values("base_link", "lidar"),
+		[](const ::testing::TestParamInfo<std::string> & info) {
+			return "in_" + info.param;
+		});
+
+// ---------------------------------------------------------------------------
+// The same filters, on the 2D scan topic.
+// ---------------------------------------------------------------------------
+
+/**
+ * @brief A LaserScan with and without the intensity channel.
+ *
+ * laser_geometry carries the channel into the projected cloud when the scan has one, and
+ * from there the node takes the PointXYZI path rather than the PointXYZ one -- separate
+ * code for every filter below.
+ */
+class IcpOdometryScanChannelTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<bool>
+{
+};
+
+TEST_P(IcpOdometryScanChannelTest, recovers_the_motion_whatever_channels_the_scan_carries)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(cornerScan(1.0, 0.0, 0.0, GetParam()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; }));
+
+	pub->publish(cornerScan(1.1, 0.05, 0.0, GetParam()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	EXPECT_NEAR(0.05, odom->back().pose.pose.position.x, 0.02);
+}
+
+/// scan_downsampling_step and scan_voxel_size thin the scan before ICP sees it.
+TEST_P(IcpOdometryScanChannelTest, downsamples_and_voxelizes_a_laser_scan)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_downsampling_step", 2));
+	params.push_back(rclcpp::Parameter("scan_voxel_size", 0.05));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const sensor_msgs::msg::LaserScan scan = cornerScan(1.0, 0.0, 0.0, GetParam());
+	pub->publish(scan);
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole scan was filtered away";
+	EXPECT_LE(data->back().laser_scan.width, scan.ranges.size()/2 + 1)
+			<< "scan_downsampling_step:=2 alone should have halved it";
+}
+
+/// With both scan_normal_k and scan_normal_radius off, the scan goes to ICP as bare points.
+TEST_P(IcpOdometryScanChannelTest, registers_a_laser_scan_without_computing_normals)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_normal_k", 0));
+	params.push_back(rclcpp::Parameter("scan_normal_radius", 0.0));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(cornerScan(1.0, 0.0, 0.0, GetParam()));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	// LaserScan::Format 1 is kXY and 2 is kXYI; the layouts with normals are 3 and 4.
+	EXPECT_LT(data->back().laser_scan_format, 3)
+			<< "normals were computed although both scan_normal_* are off";
+}
+
+/// The range limits apply to a 2D scan too: the corner is 3 m away at its nearest.
+TEST_P(IcpOdometryScanChannelTest, keeps_only_the_scan_points_within_the_range_limits)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_range_min", 3.5));
+	params.push_back(rclcpp::Parameter("scan_range_max", 6.0));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const sensor_msgs::msg::LaserScan scan = cornerScan(1.0, 0.0, 0.0, GetParam());
+	pub->publish(scan);
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	EXPECT_GT(data->back().laser_scan.width, 0u) << "the whole scan was filtered away";
+	EXPECT_LT(data->back().laser_scan.width, scan.ranges.size())
+			<< "nothing nearer than 3.5 m was dropped";
+	EXPECT_NEAR(6.0, data->back().laser_scan_max_range, 1e-3)
+			<< "scan_range_max should replace the scan's own 30 m range_max";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		Channels,
+		IcpOdometryScanChannelTest,
+		::testing::Bool(),
+		[](const ::testing::TestParamInfo<bool> & info) {
+			return info.param ? "with_intensities" : "without_intensities";
+		});
+
+// ---------------------------------------------------------------------------
+// One lidar at a time, and the parameters that come from RTAB-Map's own names.
+// ---------------------------------------------------------------------------
+
+/**
+ * The node subscribes to both `scan` and `scan_cloud`, but registering a 2D scan against
+ * a 3D cloud is meaningless, so whichever topic speaks second is dropped for good.
+ */
+TEST_F(IcpOdometryTest, stops_listening_for_scans_once_clouds_are_arriving)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(cloud));
+	ASSERT_TRUE(waitForSubscriber(scan));
+
+	cloud->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; }));
+
+	scan->publish(cornerScan(1.1));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_EQ(1u, odom->size()) << "the scan was registered against the cloud";
+	EXPECT_EQ(0u, scan->get_subscription_count()) << "the scan subscriber is still up";
+}
+
+/// And the other way round: a cloud arriving after scans is dropped instead.
+TEST_F(IcpOdometryTest, stops_listening_for_clouds_once_scans_are_arriving)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(cloud));
+	ASSERT_TRUE(waitForSubscriber(scan));
+
+	scan->publish(cornerScan(1.0));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; }));
+
+	cloud->publish(makeXYZCloud("lidar", 1.1, corner3D()));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_EQ(1u, odom->size()) << "the cloud was registered against the scan";
+	EXPECT_EQ(0u, cloud->get_subscription_count()) << "the cloud subscriber is still up";
+}
+
+/**
+ * Several of the Icp parameters name a filter the node runs itself, before ICP ever sees the
+ * scan. Setting one of those is taken as asking for the node's filter, so the value moves
+ * to the matching ros parameter -- see "Where these defaults come from" in the doc.
+ */
+TEST_F(IcpOdometryTest, takes_the_icp_filter_values_as_its_own_scan_parameters)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node = makeNode({
+			rclcpp::Parameter("Icp/DownsamplingStep", "2"),
+			rclcpp::Parameter("Icp/RangeMin", "0.5"),
+			rclcpp::Parameter("Icp/RangeMax", "20.0"),
+			rclcpp::Parameter("Icp/PointToPlaneRadius", "0.3"),
+			rclcpp::Parameter("Icp/PointToPlaneGroundNormalsUp", "0.8")});
+
+	EXPECT_EQ(2, node->get_parameter("scan_downsampling_step").as_int());
+	EXPECT_NEAR(0.5, node->get_parameter("scan_range_min").as_double(), 1e-6);
+	EXPECT_NEAR(20.0, node->get_parameter("scan_range_max").as_double(), 1e-6);
+	EXPECT_NEAR(0.3, node->get_parameter("scan_normal_radius").as_double(), 1e-6);
+	EXPECT_NEAR(0.8, node->get_parameter("scan_normal_ground_up").as_double(), 1e-6);
+}
+
+/**
+ * Reg/Strategy picks between visual and ICP registration, and this node only has ICP. A
+ * value asking for anything else is overruled rather than obeyed.
+ */
+TEST_F(IcpOdometryTest, registers_with_icp_whatever_reg_strategy_asks_for)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("Reg/Strategy", "0"));   // visual only
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 1; }));
+
+	const cv::Point3f motion(0.10f, 0.06f, 0.04f);
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(motion)));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	// Nothing but ICP could have recovered this: there is no image to register.
+	EXPECT_NEAR(motion.x, odom->back().pose.pose.position.x, 0.01);
+}
+
+
+/**
+ * @brief The four field combinations again, organized and downsampled.
+ *
+ * An organized cloud keeps its rows through the step, so the node counts what a full
+ * sweep holds from the cloud's own dimensions instead of dividing by the step.
+ */
+class IcpOdometryOrganizedFieldsTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<std::tuple<bool, bool>>
+{
+};
+
+TEST_P(IcpOdometryOrganizedFieldsTest, sizes_a_downsampled_organized_cloud_from_its_rows)
+{
+	const bool intensity = std::get<0>(GetParam());
+	const bool normals = std::get<1>(GetParam());
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_downsampling_step", 2));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = corner3D();
+	pub->publish(organized(
+			makeCloudWithFields("lidar", 1.0, points, intensity, normals), 80));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	// 1200 points as 15 rings of 80, every second point of each ring kept: 15 x 40.
+	EXPECT_EQ(int(points.size()/2), data->back().laser_scan_max_pts);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		OptionalFields,
+		IcpOdometryOrganizedFieldsTest,
+		::testing::Combine(::testing::Bool(), ::testing::Bool()),
+		[](const ::testing::TestParamInfo<std::tuple<bool, bool>> & info) {
+			return std::string(std::get<0>(info.param) ? "intensity" : "no_intensity") +
+					(std::get<1>(info.param) ? "_normals" : "_no_normals");
+		});
+
+/// With both scan_normal_* off, a cloud reaches ICP as bare points, intensity or not.
+TEST_P(IcpOdometryDenseTest, registers_a_cloud_without_computing_normals)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("scan_normal_k", 0));
+	params.push_back(rclcpp::Parameter("scan_normal_radius", 0.0));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeCloudWithFields("lidar", 1.0, corner3D(), GetParam(), false));
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	// LaserScan::Format kXYZ and kXYZI; the layouts with normals are 8 and 9.
+	EXPECT_EQ(GetParam() ? 6 : 5, data->back().laser_scan_format)
+			<< "normals were computed although both scan_normal_* are off";
+}
+
+/**
+ * An intensity channel the node cannot read -- anything but float32 -- is dropped rather
+ * than misread, and the cloud is registered without it.
+ */
+TEST_F(IcpOdometryTest, ignores_an_intensity_field_it_cannot_read)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> data =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	sensor_msgs::msg::PointCloud2 cloud =
+			makeCloudWithFields("lidar", 1.0, corner3D(), true, false);
+	for(sensor_msgs::msg::PointField & field : cloud.fields)
+	{
+		if(field.name == "intensity")
+		{
+			field.datatype = sensor_msgs::msg::PointField::UINT8;
+		}
+	}
+	pub->publish(cloud);
+	ASSERT_TRUE(spinUntil([&]() { return !data->empty(); }));
+
+	// kXYZNormal rather than kXYZINormal: the channel was left out.
+	EXPECT_EQ(8, data->back().laser_scan_format)
+			<< "an intensity channel that is not float32 was carried through anyway";
+}
+
+/// A frame TF knows nothing about is an error, not a pose: the frame is dropped.
+TEST_F(IcpOdometryTest, refuses_a_cloud_whose_frame_is_not_in_tf)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("unmounted_lidar", 1.0, corner3D()));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(odom->empty()) << "a cloud from an unknown frame was registered anyway";
+}
+
+/// The same for a 2D scan.
+TEST_F(IcpOdometryTest, refuses_a_scan_whose_frame_is_not_in_tf)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	sensor_msgs::msg::LaserScan scan = cornerScan(1.0);
+	scan.header.frame_id = "unmounted_lidar";
+	pub->publish(scan);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(odom->empty()) << "a scan from an unknown frame was registered anyway";
+}
+
+/**
+ * @brief Deskewing a 2D scan without a guess frame, from the sensor's frame and from
+ * frame_id.
+ *
+ * The cloud version of this is IcpOdometryDeskewFrameTest above; a scan takes a different
+ * route to the same place, because laser_geometry projects it into frame_id first when
+ * the sensor is mounted somewhere else, and leaves it alone when it is not.
+ */
+class IcpOdometryScanDeskewFrameTest :
+		public IcpOdometryTest,
+		public ::testing::WithParamInterface<std::string>
+{
+};
+
+TEST_P(IcpOdometryScanDeskewFrameTest, deskews_a_scan_against_its_own_velocity)
+{
+	const std::string frame = GetParam();
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::vector<rclcpp::Parameter> params = icpTestParameters();
+	params.push_back(rclcpp::Parameter("deskewing", true));
+	makeNode(params);
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	for(int i=0; i<4; ++i)
+	{
+		sensor_msgs::msg::LaserScan scan = cornerScan(1.0 + 0.1*i, 0.05*i, 0.0, false, 0.01f);
+		scan.header.frame_id = frame;
+		pub->publish(scan);
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "no odometry for the scan at " << (1.0 + 0.1*i) << "s";
+	}
+
+	EXPECT_NEAR(0.15, odom->back().pose.pose.position.x, 0.02);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		ScanFrame,
+		IcpOdometryScanDeskewFrameTest,
+		::testing::Values("base_link", "lidar"),
+		[](const ::testing::TestParamInfo<std::string> & info) {
+			return "in_" + info.param;
+		});
+
+
+/**
+ * pause_odom stops the scan topic as well as the cloud one -- the scans keep arriving,
+ * and none of them is registered until resume_odom.
+ */
+TEST_F(IcpOdometryTest, pause_and_resume_stop_and_restart_processing_scans)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node = makeNode(icpTestParameters());
+
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(cornerScan(1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+
+	ASSERT_TRUE(callEmptyService("pause_odom"));
+	const size_t whilePaused = odom->size();
+	pub->publish(cornerScan(1.1, 0.05));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(whilePaused, odom->size()) << "a scan was registered while paused";
+
+	ASSERT_TRUE(callEmptyService("resume_odom"));
+	pub->publish(cornerScan(1.2, 0.10));
+	EXPECT_TRUE(spinUntil([&]() { return odom->size() > whilePaused; }));
+}
+
+
+
+}  // namespace
+}  // namespace rtabmap_odom_test
diff --git a/rtabmap_odom/test/test_odometry_ros.cpp b/rtabmap_odom/test/test_odometry_ros.cpp
new file mode 100644
index 00000000..6694f994
--- /dev/null
+++ b/rtabmap_odom/test/test_odometry_ros.cpp
@@ -0,0 +1,1350 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <cstdlib>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2_ros/static_transform_broadcaster.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <rtabmap_msgs/srv/reset_pose.hpp>
+
+#include <rtabmap_odom/icp_odometry.hpp>
+
+#include "msg_builders.hpp"
+#include "node_test_utils.hpp"
+#include "scan_scenes.hpp"
+
+namespace rtabmap_odom_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/**
+ * OdometryROS is abstract, so these drive it through ICPOdometry -- the cheapest of the
+ * three to feed, since a synthetic point cloud needs no camera calibration. Everything
+ * asserted here lives in the base class and behaves identically on all three nodes.
+ */
+class OdometryRosTest : public NodeTest
+{
+protected:
+	void publishSensorTf()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = helper()->now();
+		tf.header.frame_id = "base_link";
+		tf.child_frame_id = "lidar";
+		tf.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(tf);
+	}
+
+	std::shared_ptr<rtabmap_odom::ICPOdometry> makeNode(
+			std::vector<rclcpp::Parameter> params = {})
+	{
+		std::vector<rclcpp::Parameter> all = icpTestParameters();
+		all.push_back(rclcpp::Parameter("frame_id", "base_link"));
+		// What a sweep holds, as a real lidar reports it. ICP measures its correspondence
+		// ratio against this, and without it the ratio falls back to the scan's own size
+		// -- which lets the three points of degenerateCloud() match themselves perfectly.
+		all.push_back(rclcpp::Parameter("scan_cloud_max_points", int(corner3D().size())));
+		// See the note in test_icp_odometry.cpp: without this the node drops frames that
+		// arrive closer together than their stamps claim, which is what a loaded runner
+		// does to a sequence published back to back.
+		all.push_back(rclcpp::Parameter("always_process_most_recent_frame", false));
+		for(const rclcpp::Parameter & p : params)
+		{
+			all.push_back(p);
+		}
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(all);
+		return addNode(std::make_shared<rtabmap_odom::ICPOdometry>(options));
+	}
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanPublisher()
+	{
+		return helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	}
+
+	/**
+	 * @brief Drives `count` frames through the node, each `step` metres further along x.
+	 *
+	 * `unpublished` is how many of them are expected to produce no message, so the wait
+	 * still knows when each frame is done. It is 1 with `publish_null_when_lost` off,
+	 * where the first frame initialises the odometry and is held back for having no
+	 * velocity behind it, and 0 everywhere else.
+	 */
+	void feedFrames(
+			const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub,
+			const std::shared_ptr<Collector<nav_msgs::msg::Odometry>> & odom,
+			int count, float step = 0.05f, int unpublished = 0)
+	{
+		for(int i=0; i<count; ++i)
+		{
+			pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(step*i, 0, 0))));
+			const int expected = i+1 - unpublished;
+			if(expected > 0)
+			{
+				spinUntil([&]() { return int(odom->size()) >= expected; });
+			}
+			else
+			{
+				spinFor(std::chrono::milliseconds(200));
+			}
+		}
+	}
+
+	/// A scan with nothing in it to register against: three points on a line.
+	sensor_msgs::msg::PointCloud2 degenerateCloud(double stamp)
+	{
+		// A short line off in free space: enough points for the scan_normal_k neighbours
+		// the node asks for, and jittered, because a perfectly straight one has no
+		// second axis for the normals to be fitted against and some PCL versions return
+		// them as NaN -- which empties the scan and has the odometry refuse it for that
+		// rather than for its shape. Its structural complexity stays near 0.016, under
+		// the 0.02 that Icp/PointToPlaneMinComplexity asks of a frame to start a map on.
+		cv::RNG rng(0xDECAF);
+		std::vector<cv::Point3f> line;
+		for(int i=0; i<20; ++i)
+		{
+			line.push_back(cv::Point3f(1.0f + 0.05f*float(i),
+					float(rng.gaussian(0.002)), float(rng.gaussian(0.002))));
+		}
+		return makeXYZCloud("lidar", stamp, line);
+	}
+
+	/// Counts the transforms matching @p parent -> @p child seen so far.
+	static size_t countTransforms(
+			const std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> & tf,
+			const std::string & parent, const std::string & child)
+	{
+		size_t found = 0;
+		for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+		{
+			for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+			{
+				found += (t.header.frame_id == parent && t.child_frame_id == child) ? 1 : 0;
+			}
+		}
+		return found;
+	}
+
+	/**
+	 * @brief Publishes wheel_odom -> base_link driving straight at @p speed m/s.
+	 *
+	 * Sampled at 100 Hz across the whole window and sent in chunks: tf2's listener reads
+	 * /tf on its own thread with a bounded queue, and a burst large enough to overflow it
+	 * drops the oldest transforms -- the ones the first frames need.
+	 */
+	bool publishGuessMotion(double from, double to, double speed)
+	{
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> echo =
+				collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(200));
+		if(!waitForSubscriber(tf, 2))
+		{
+			return false;
+		}
+		size_t published = 0;
+		for(double t = from; t <= to; t += 0.01)
+		{
+			geometry_msgs::msg::TransformStamped pose;
+			pose.header.stamp = stampOf(t);
+			pose.header.frame_id = "wheel_odom";
+			pose.child_frame_id = "base_link";
+			pose.transform.translation.x = speed * (t - from);
+			pose.transform.rotation.w = 1.0;
+			tf2_msgs::msg::TFMessage message;
+			message.transforms.push_back(pose);
+			tf->publish(message);
+			if(++published % 10 == 0)
+			{
+				spinFor(std::chrono::milliseconds(5));
+			}
+		}
+		guessMotionTf_ = tf;
+		return spinUntil([&]() { return echo->size() >= published; });
+	}
+
+	/**
+	 * @brief Publishes odom -> base_link as another node would: a fused estimate.
+	 *
+	 * This is the "sensor fusion is used" case the reset paths look for -- the output of
+	 * robot_localization, say, publishing the same odom frame this node reports in. Static
+	 * here so that the lookup succeeds whatever stamp a frame carries.
+	 */
+	void publishFusedPose(double x)
+	{
+		fusedTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped fused;
+		fused.header.stamp = helper()->now();
+		fused.header.frame_id = "odom";
+		fused.child_frame_id = "base_link";
+		fused.transform.translation.x = x;
+		fused.transform.rotation.w = 1.0;
+		fusedTf_->sendTransform(fused);
+		spinFor(std::chrono::milliseconds(200));
+	}
+
+	/// Calls an Empty service on the node and waits for it to return.
+	///
+	/// The node advertises these under its own name -- /icp_odometry/reset_odom, not
+	/// /reset_odom -- so the bare name a client would resolve against the namespace is
+	/// not the right one.
+	bool callEmptyService(const std::string & name, const std::string & node = "icp_odometry")
+	{
+		rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+				helper()->create_client<std_srvs::srv::Empty>("/" + node + "/" + name);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return false;
+		}
+		std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
+				client->async_send_request(
+						std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
+		return spinUntil([&]() {
+			return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
+	}
+
+protected:
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> guessTf_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr guessMotionTf_;
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> fusedTf_;
+};
+
+/**
+ * always_process_most_recent_frame is a "skip the backlog" policy, and it is on by
+ * default: a frame that arrives while the previous one is still being registered is
+ * dropped rather than queued, so the odometry stays on the newest data instead of falling
+ * further and further behind a sensor it cannot keep up with. Registration holds the data
+ * mutex for its whole duration, and a frame that cannot take that mutex is the one that
+ * gets dropped.
+ *
+ * Every other test in these suites turns the policy off, to be able to account for each
+ * frame; this pair is where the default itself is covered.
+ *
+ * topic_queue_size is raised because icp_odometry defaults to 1: with a queue that deep
+ * the middleware would drop the burst before the node ever saw it, and the test would
+ * pass without exercising anything.
+ */
+TEST_F(OdometryRosTest, drops_frames_that_arrive_while_the_previous_one_is_registering)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("always_process_most_recent_frame", true),
+	          rclcpp::Parameter("topic_queue_size", 20)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	// A burst with no spin in between, so the executor hands the node its second frame
+	// while the worker thread is still inside the first registration.
+	const size_t burst = 10;
+	for(size_t i=0; i<burst; ++i)
+	{
+		pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0))));
+	}
+	spinFor(std::chrono::milliseconds(2000));
+
+	EXPECT_GE(odom->size(), 1u) << "the burst produced no odometry at all";
+	EXPECT_LT(odom->size(), burst)
+			<< "every frame of the burst came back out, so nothing was skipped";
+}
+
+/// With the policy off, the same burst is registered whole, one pose per frame.
+TEST_F(OdometryRosTest, processes_every_frame_of_a_burst_when_the_policy_is_off)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	// always_process_most_recent_frame:=false comes from the fixture.
+	makeNode({rclcpp::Parameter("topic_queue_size", 20)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const size_t burst = 10;
+	for(size_t i=0; i<burst; ++i)
+	{
+		pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.05f*i, 0, 0))));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= burst; }))
+			<< "only " << odom->size() << " of " << burst << " frames came back out";
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(burst, odom->size()) << "more poses than frames";
+}
+
+/// The frames the pose is published in are both configurable.
+TEST_F(OdometryRosTest, publishes_in_the_configured_frames)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("odom_frame_id", "custom_odom")});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 1);
+
+	ASSERT_FALSE(odom->empty());
+	EXPECT_EQ("custom_odom", odom->back().header.frame_id);
+	EXPECT_EQ("base_link", odom->back().child_frame_id);
+}
+
+/// publish_tf broadcasts odom -> base_link, which is how the rest of the system sees the pose.
+TEST_F(OdometryRosTest, broadcasts_the_odom_to_base_transform)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	makeNode({rclcpp::Parameter("publish_tf", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+	bool found = false;
+	for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+	{
+		for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+		{
+			found = found || (t.header.frame_id == "odom" && t.child_frame_id == "base_link");
+		}
+	}
+	EXPECT_TRUE(found);
+}
+
+/**
+ * With publish_tf off nothing is broadcast, which is what you want when another node --
+ * robot_localization, typically -- already owns odom -> base_link.
+ */
+TEST_F(OdometryRosTest, publishes_no_transform_when_publish_tf_is_off)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	makeNode({rclcpp::Parameter("publish_tf", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_FALSE(odom->empty()) << "the node must still publish odometry";
+
+	for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+	{
+		for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+		{
+			EXPECT_FALSE(t.header.frame_id == "odom" && t.child_frame_id == "base_link");
+		}
+	}
+}
+
+/**
+ * With a guess frame configured the node publishes a correction rather than the pose:
+ * odom -> guess_frame_id, leaving guess_frame_id -> base_link to the guess source. This
+ * is what keeps the TF tree connected while registration is lost -- see "It also keeps TF
+ * alive through a failure" in the README.
+ */
+TEST_F(OdometryRosTest, broadcasts_a_correction_to_the_guess_frame_when_one_is_configured)
+{
+	publishSensorTf();
+	// The guess source owns this half of the chain.
+	geometry_msgs::msg::TransformStamped guess;
+	guess.header.stamp = helper()->now();
+	guess.header.frame_id = "odom";
+	guess.child_frame_id = "base_link";
+	guess.transform.rotation.w = 1.0;
+	guessTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+	guessTf_->sendTransform(guess);
+
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	// As the launch files in this repository do it: the guess source keeps the
+	// conventional /odom frame, and this node takes a distinct one.
+	makeNode({rclcpp::Parameter("publish_tf", true),
+	          rclcpp::Parameter("odom_frame_id", "icp_odom"),
+	          rclcpp::Parameter("guess_frame_id", "odom")});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+
+	bool correction = false;
+	bool directPose = false;
+	for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+	{
+		for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+		{
+			correction = correction || (t.header.frame_id == "icp_odom" && t.child_frame_id == "odom");
+			directPose = directPose || (t.header.frame_id == "icp_odom" && t.child_frame_id == "base_link");
+		}
+	}
+	EXPECT_TRUE(correction) << "expected the icp_odom -> odom correction";
+	EXPECT_FALSE(directPose) << "the link to base_link belongs to the guess source, not this node";
+}
+
+/**
+ * Naming the guess frame the same as odom_frame_id would publish a frame as its own
+ * parent, so the node drops the guess and warns rather than doing it. The pose then goes
+ * out the ordinary way, odom -> base_link.
+ */
+TEST_F(OdometryRosTest, disables_the_guess_when_its_frame_matches_odom_frame_id)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node =
+			makeNode({rclcpp::Parameter("publish_tf", true),
+			          rclcpp::Parameter("odom_frame_id", "odom"),
+			          rclcpp::Parameter("guess_frame_id", "odom")});
+
+	EXPECT_TRUE(node->guessFrameId().empty()) << "the guess should have been disabled";
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+
+	bool directPose = false;
+	for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+	{
+		for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+		{
+			directPose = directPose || (t.header.frame_id == "odom" && t.child_frame_id == "base_link");
+		}
+	}
+	EXPECT_TRUE(directPose) << "with the guess disabled the pose is published directly";
+}
+
+/// initial_pose starts the trajectory somewhere other than the origin.
+TEST_F(OdometryRosTest, starts_from_the_configured_initial_pose)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("initial_pose", "1 2 3 0 0 0")});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 1);
+
+	ASSERT_FALSE(odom->empty());
+	EXPECT_NEAR(1.0, odom->back().pose.pose.position.x, 1e-3);
+	EXPECT_NEAR(2.0, odom->back().pose.pose.position.y, 1e-3);
+	EXPECT_NEAR(3.0, odom->back().pose.pose.position.z, 1e-3);
+}
+
+/// reset_odom puts the pose back to the identity and starts the map again.
+TEST_F(OdometryRosTest, reset_odom_returns_the_pose_to_the_origin)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	ASSERT_GE(odom->size(), 3u);
+	ASSERT_GT(odom->back().pose.pose.position.x, 0.01) << "should have travelled before reset";
+
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 2.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	EXPECT_NEAR(0.0, odom->back().pose.pose.position.x, 1e-6);
+}
+
+/// reset_odom_to_pose does the same, to a pose of your choosing.
+TEST_F(OdometryRosTest, reset_odom_to_pose_sets_the_given_pose)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+
+	rclcpp::Client<rtabmap_msgs::srv::ResetPose>::SharedPtr client =
+			helper()->create_client<rtabmap_msgs::srv::ResetPose>("/icp_odometry/reset_odom_to_pose");
+	ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
+	std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request> request =
+			std::make_shared<rtabmap_msgs::srv::ResetPose::Request>();
+	request->x = 5.0;
+	request->y = -2.0;
+	std::shared_future<rtabmap_msgs::srv::ResetPose::Response::SharedPtr> future =
+			client->async_send_request(request).future.share();
+	ASSERT_TRUE(spinUntil([&]() {
+		return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
+
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 3.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	EXPECT_NEAR(5.0, odom->back().pose.pose.position.x, 1e-3);
+	EXPECT_NEAR(-2.0, odom->back().pose.pose.position.y, 1e-3);
+}
+
+/**
+ * A reset is announced to whatever consumes this topic by publishing both covariances
+ * bad: there is a pose, but it is an initialisation rather than a registration and there
+ * is no velocity behind it yet. The same pair of values goes out on the very first frame
+ * of a session, which is the same situation.
+ *
+ * They are bad for different reasons, and do not carry the same number. The pose
+ * covariance is RTAB-Map's registration covariance doubled, and registration reports
+ * BAD_COVARIANCE for a frame it could not link, so the pose comes out at 19998. The twist
+ * covariance is set to BAD_COVARIANCE directly, 9999, whenever no velocity is available.
+ * Tracking normally, both drop to the real values -- around 1e-8 on this synthetic scene.
+ *
+ * rtabmap_slam starts a new mapping session when it sees this, because the new pose
+ * cannot be linked to the previous one.
+ */
+TEST_F(OdometryRosTest, marks_both_covariances_bad_on_the_frame_after_a_reset)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	ASSERT_GE(odom->size(), 3u);
+	// Tracking normally: a velocity is available, so the twist covariance is real.
+	EXPECT_LT(odom->back().twist.covariance[0], 9999.0);
+
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 5.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+
+	EXPECT_GE(odom->back().twist.covariance[0], 9999.0)
+			<< "the first frame after a reset has no velocity, and must say so";
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0)
+			<< "and it is an initialization rather than a registration";
+}
+
+/**
+ * Same after reset_odom_to_pose, where the pose is non-identity and perfectly usable.
+ * rtabmap_slam relies on this: it starts a new mapping session when it sees an identity
+ * pose, or both covariances bad, so a reset to an arbitrary pose is still recognized as
+ * the discontinuity it is.
+ */
+TEST_F(OdometryRosTest, marks_both_covariances_bad_after_reset_to_pose)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+
+	rclcpp::Client<rtabmap_msgs::srv::ResetPose>::SharedPtr client =
+			helper()->create_client<rtabmap_msgs::srv::ResetPose>("/icp_odometry/reset_odom_to_pose");
+	ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
+	std::shared_ptr<rtabmap_msgs::srv::ResetPose::Request> request =
+			std::make_shared<rtabmap_msgs::srv::ResetPose::Request>();
+	request->x = 3.0;
+	std::shared_future<rtabmap_msgs::srv::ResetPose::Response::SharedPtr> future =
+			client->async_send_request(request).future.share();
+	ASSERT_TRUE(spinUntil([&]() {
+		return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
+
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 5.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+
+	// Both covariances are bad, even though the pose itself is perfectly usable: the frame
+	// after a reset is an initialization, not a registration. That pair is what
+	// rtabmap_slam tests for to start a new mapping session.
+	EXPECT_GE(odom->back().twist.covariance[0], 9999.0);
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0);
+	EXPECT_NEAR(3.0, odom->back().pose.pose.position.x, 1e-3);
+}
+
+/// pause_odom stops processing entirely; resume_odom starts it again.
+TEST_F(OdometryRosTest, pause_and_resume_stop_and_restart_processing)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<rtabmap_odom::ICPOdometry> node = makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 1);
+	ASSERT_FALSE(odom->empty());
+
+	ASSERT_TRUE(callEmptyService("pause_odom"));
+	EXPECT_TRUE(node->isPaused());
+
+	const size_t whilePaused = odom->size();
+	pub->publish(makeXYZCloud("lidar", 2.0, corner3D()));
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(whilePaused, odom->size()) << "nothing should be processed while paused";
+
+	ASSERT_TRUE(callEmptyService("resume_odom"));
+	EXPECT_FALSE(node->isPaused());
+
+	pub->publish(makeXYZCloud("lidar", 3.0, corner3D()));
+	EXPECT_TRUE(spinUntil([&]() { return odom->size() > whilePaused; }));
+}
+
+/// The log-level services are advertised; they change RTAB-Map's own verbosity.
+TEST_F(OdometryRosTest, advertises_the_log_level_services)
+{
+	publishSensorTf();
+	makeNode();
+
+	for(const char * const name : {"log_debug", "log_info", "log_warning", "log_error"})
+	{
+		EXPECT_TRUE(callEmptyService(name)) << name << " did not answer";
+	}
+}
+
+/**
+ * publish_null_when_lost:=false also suppresses the *successful* frame after a reset.
+ *
+ * That frame has a pose but no velocity, and the publish is gated on
+ * `setTwist || publish_null_when_lost` -- so with null publishing off, the one message
+ * carrying the reset's bad covariances never goes out. The next frame has a velocity
+ * again and is published with ordinary covariances.
+ *
+ * The consequence is worth knowing: rtabmap never sees the reset, and does not start a
+ * new mapping session -- so this parameter silently changes mapping behaviour as well as
+ * the lost signal it is named for.
+ */
+TEST_F(OdometryRosTest, suppresses_the_post_reset_frame_when_null_publishing_is_off)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("publish_null_when_lost", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 4);
+	ASSERT_FALSE(odom->empty());
+
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+
+	// The frame that would have carried the reset signal is not published at all.
+	size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 6.0, corner3D()));
+	spinFor(std::chrono::milliseconds(1000));
+	EXPECT_EQ(before, odom->size())
+			<< "the post-reset frame has no velocity, so it is gated away";
+
+	// The frame after that has a velocity again, and looks entirely ordinary.
+	before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 6.1, corner3D(cv::Point3f(0.05f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	EXPECT_LT(odom->back().pose.covariance[0], 9999.0);
+	EXPECT_LT(odom->back().twist.covariance[0], 9999.0);
+}
+
+
+// ---------------------------------------------------------------------------
+// What happens while registration is lost, and the rate limits around it.
+// ---------------------------------------------------------------------------
+
+/**
+ * A guess frame keeps TF alive through a failure: while registration is lost the node
+ * goes on publishing the correction, frozen at the last pose it computed and carrying
+ * whatever the guess source has moved since, so base_link keeps moving in TF instead of
+ * stalling. See "It also keeps TF alive through a failure" in the README.
+ */
+TEST_F(OdometryRosTest, keeps_broadcasting_the_correction_while_registration_is_lost)
+{
+	publishSensorTf();
+	geometry_msgs::msg::TransformStamped guess;
+	guess.header.stamp = helper()->now();
+	guess.header.frame_id = "odom";
+	guess.child_frame_id = "base_link";
+	guess.transform.rotation.w = 1.0;
+	guessTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+	guessTf_->sendTransform(guess);
+
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	makeNode({rclcpp::Parameter("publish_tf", true),
+	          rclcpp::Parameter("odom_frame_id", "icp_odom"),
+	          rclcpp::Parameter("guess_frame_id", "odom")});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+	spinFor(std::chrono::milliseconds(300));   // let every transform of the good frames land
+	const size_t beforeLoss = countTransforms(tf, "icp_odom", "odom");
+
+	// Nothing registrable: the pose is lost, but the chain must not go quiet.
+	const size_t odomBefore = odom->size();
+	pub->publish(degenerateCloud(5.0));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > odomBefore; }));
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0) << "the frame was expected to be lost";
+	EXPECT_GT(countTransforms(tf, "icp_odom", "odom"), beforeLoss)
+			<< "the correction stopped while lost, which breaks the TF tree downstream";
+}
+
+/// Without a guess frame there is no correction to publish, so TF stops while lost.
+TEST_F(OdometryRosTest, broadcasts_no_transform_while_lost_without_a_guess_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	makeNode({rclcpp::Parameter("publish_tf", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+	spinFor(std::chrono::milliseconds(300));
+	const size_t beforeLoss = countTransforms(tf, "odom", "base_link");
+
+	const size_t odomBefore = odom->size();
+	pub->publish(degenerateCloud(5.0));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > odomBefore; }));
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0) << "the frame was expected to be lost";
+	EXPECT_EQ(beforeLoss, countTransforms(tf, "odom", "base_link"))
+			<< "a pose was broadcast for a frame that did not register";
+}
+
+/// max_update_rate throttles registration, dropping the frames that arrive too soon.
+TEST_F(OdometryRosTest, max_update_rate_skips_frames_that_arrive_too_soon)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("max_update_rate", 5.0),   // one frame per 0.2 s
+	          rclcpp::Parameter("topic_queue_size", 20)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	// Ten frames a tenth of a second apart: twice the rate the node will accept.
+	for(int i=0; i<10; ++i)
+	{
+		pub->publish(makeXYZCloud("lidar", 1.0 + 0.1*i, corner3D(cv::Point3f(0.02f*i, 0, 0))));
+		spinFor(std::chrono::milliseconds(50));
+	}
+	spinFor(std::chrono::milliseconds(300));
+
+	// Four of the ten got through on this machine; the rate allows about five.
+	EXPECT_GE(odom->size(), 2u) << "the throttle swallowed everything";
+	EXPECT_LE(odom->size(), 7u) << "ten frames at twice the rate should not all register";
+}
+
+/**
+ * min_update_rate is the other end: when the gap between frames grows beyond it the
+ * motion assumption no longer holds, so the odometry is reset rather than continued.
+ */
+TEST_F(OdometryRosTest, min_update_rate_resets_when_a_frame_arrives_too_late)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("min_update_rate", 2.0)});   // half a second
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	ASSERT_GE(odom->size(), 3u);
+
+	// Two seconds later, well past 1/min_update_rate.
+	const double previous = odom->back().pose.pose.position.x;
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 5.0, corner3D(cv::Point3f(0.1f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	// Registered as an initialisation rather than a continuation: both covariances bad,
+	// exactly as after an explicit reset.
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0);
+	EXPECT_GE(odom->back().twist.covariance[0], 9999.0);
+	// The pose itself carries on from where it was -- the reset drops the map, not the
+	// place the robot had reached.
+	EXPECT_NEAR(previous, odom->back().pose.pose.position.x, 1e-3);
+}
+
+/**
+ * Below guess_min_translation the node does not register at all: it forwards the guess as
+ * the pose and labels it with guess_linear_variance, which is how a consumer can tell the
+ * difference from a real registration.
+ */
+TEST_F(OdometryRosTest, skips_registration_when_the_guess_says_it_barely_moved)
+{
+	publishSensorTf();
+	geometry_msgs::msg::TransformStamped guess;
+	guess.header.stamp = helper()->now();
+	guess.header.frame_id = "wheel_odom";
+	guess.child_frame_id = "base_link";
+	guess.transform.rotation.w = 1.0;
+	guessTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+	guessTf_->sendTransform(guess);
+
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("guess_min_translation", 1.0),   // a metre, never reached
+	          rclcpp::Parameter("guess_linear_variance", 0.042)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	ASSERT_GE(odom->size(), 2u);
+	// The published covariance is guess_linear_variance, doubled on the way out as every
+	// pose covariance is -- 0.084 for the 0.042 configured here. A registration would
+	// report its own, far smaller value, so this is what marks the frame as guess-only.
+	EXPECT_NEAR(2.0 * 0.042, odom->back().pose.covariance[0], 1e-6)
+			<< "the frame was registered rather than taken from the guess";
+}
+
+/// Odom/ResetCountdown resets the odometry after that many consecutive lost frames.
+TEST_F(OdometryRosTest, resets_itself_after_the_configured_number_of_lost_frames)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("Odom/ResetCountdown", "2")});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	const double moved = odom->back().pose.pose.position.x;
+
+	for(int i=0; i<2; ++i)
+	{
+		const size_t before = odom->size();
+		pub->publish(degenerateCloud(5.0 + i));
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	}
+	// A registrable frame again, after the countdown has run out.
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 8.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	// The countdown reset the odometry, so this frame initialises a new map from where
+	// the robot had got to -- 0.0999975 m along, with an initialisation's covariance.
+	// With the countdown disabled the same frame comes back null, at the origin: the
+	// stale map is still there and still does not match.
+	EXPECT_NEAR(moved, odom->back().pose.pose.position.x, 1e-3)
+			<< "the recovered pose should continue from the last good one";
+	EXPECT_GT(odom->back().pose.pose.position.x, 0.05)
+			<< "a null pose at the origin means the odometry never recovered";
+}
+
+
+/**
+ * The frame the odometry registered, republished on the auxiliary topics.
+ *
+ * These are not copies of the input: they carry what the registration worked on and what
+ * it produced. `odom_sensor_data/features` is the same message with the images and the
+ * scan stripped out, leaving the features alone. See "Outputting filtered scans and
+ * features" in the README.
+ *
+ * `odom_local_map` and `odom_last_frame` are not among them: both are built from the
+ * frame's visual words, so a lidar-only run publishes neither. `odom_local_scan_map` is
+ * the ICP path's equivalent, and the visual pair is covered in test_rgbd_odometry.cpp.
+ */
+TEST_F(OdometryRosTest, republishes_the_registered_frame_on_the_auxiliary_topics)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> lite =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info_lite");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> scanMap =
+			collect<sensor_msgs::msg::PointCloud2>("odom_local_scan_map");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> raw =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/raw");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> features =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/features");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 3);
+	ASSERT_TRUE(spinUntil([&]() {
+		return !lite->empty() && !scanMap->empty() && !raw->empty() && !features->empty(); }))
+			<< "lite=" << lite->size() << " scan_map=" << scanMap->size()
+			<< " raw=" << raw->size() << " features=" << features->size();
+
+	EXPECT_GT(raw->back().laser_scan.data.size(), 0u)
+			<< "the raw topic should carry the scan the odometry registered";
+	EXPECT_EQ(0u, features->back().laser_scan.data.size())
+			<< "the features topic is the same message with the scan and images removed";
+	EXPECT_GT(scanMap->back().width, 0u) << "the scan map the frame was registered against";
+}
+
+/// publish_compressed_sensor_data swaps the raw images and scan for compressed ones.
+TEST_F(OdometryRosTest, publishes_compressed_sensor_data_when_asked)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> compressed =
+			collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/compressed");
+	makeNode({rclcpp::Parameter("publish_compressed_sensor_data", true)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	feedFrames(pub, odom, 2);
+	ASSERT_TRUE(spinUntil([&]() { return !compressed->empty(); }))
+			<< "nothing was published on odom_sensor_data/compressed";
+	// The scan travels compressed instead of raw: ~25 kB against an empty raw field.
+	EXPECT_GT(compressed->back().laser_scan_compressed.size(), 0u)
+			<< "nothing was compressed into the message";
+	EXPECT_EQ(0u, compressed->back().laser_scan.data.size())
+			<< "the raw scan should have been replaced by the compressed one";
+}
+
+
+/**
+ * The whole recovery story, as a wheeled robot would live it: a guess frame it trusts,
+ * silence while lost, and a pose that is still right when registration comes back.
+ *
+ * The robot drives at 1 m/s. Registration fails on the first degenerate scan and, with
+ * Odom/ResetCountdown at 1, the odometry resets immediately onto the guess. It stays lost
+ * for a second -- ten frames at 10 Hz, a metre of travel -- publishing nothing at all,
+ * because publish_null_when_lost is off. When a registrable scan arrives again the node
+ * reports a usable pose, and that pose is where the wheels say the robot is: a metre on.
+ */
+TEST_F(OdometryRosTest, recovers_on_the_guess_after_a_metre_of_being_lost)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("publish_null_when_lost", false),
+	          rclcpp::Parameter("Odom/ResetCountdown", "1"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	// 1 m/s from t=1.0 to t=2.5, which covers every frame below: x(t) = t - 1.0.
+	const double speed = 1.0;   // m/s, and the frames below are 10 Hz
+	ASSERT_TRUE(publishGuessMotion(1.0, 2.5, speed));
+
+	// Tracking normally at the start. The first frame has no velocity behind it yet, so
+	// with null publishing off it is not published either -- the second one is.
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	spinFor(std::chrono::milliseconds(200));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(float(0.1*speed), 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking";
+	const size_t whileTracking = odom->size();
+
+	// A second of nothing to register against, at 10 Hz, while the wheels carry on.
+	for(int i=2; i<=11; ++i)
+	{
+		pub->publish(degenerateCloud(1.0 + 0.1*i));
+		spinFor(std::chrono::milliseconds(60));
+	}
+	const size_t whileLost = odom->size();
+
+	// The scene comes back, seen from where the wheels say the robot now is. The frame
+	// that restarts the map is published as a continuation of the old one (see
+	// publishes_the_restarting_frame_as_a_continuation_of_the_guess below); a second
+	// frame follows so that what is asserted here is a real registration.
+	pub->publish(makeXYZCloud("lidar", 2.2, corner3D(cv::Point3f(float(1.2*speed), 0, 0))));
+	spinFor(std::chrono::milliseconds(200));
+	pub->publish(makeXYZCloud("lidar", 2.3, corner3D(cv::Point3f(float(1.3*speed), 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > whileLost; }))
+			<< "never recovered once there was something to register again";
+	spinFor(std::chrono::milliseconds(500));   // give any second message time to arrive
+
+	// Nothing goes missing while lost either. With a guess to fall back on, a frame that
+	// re-initialises the map is published rather than dropped, carrying the guess's pose
+	// and its confidence. Whether any of the degenerate frames manages to initialise a map
+	// at all differs between builds, so what is checked is what such a frame carries
+	// rather than how many of them there are.
+	for(size_t i=whileTracking; i<whileLost; ++i)
+	{
+		EXPECT_DOUBLE_EQ(0.002, odom->messages[i]->pose.covariance[0])
+				<< "a pose published while lost should carry the guess's confidence, "
+				<< "message " << i << " of " << whileLost;
+	}
+
+	// Tracking again, on a real registration rather than the guess.
+	EXPECT_LT(odom->back().pose.covariance[0], 9999.0) << "still lost after recovering";
+
+	// And the pose is where the wheels say the robot is: 1.3 m, to the millimetre.
+	//
+	// This used to come back 0.1 m short, and the shortfall accumulated -- 0.1, 0.2, 0.3 m
+	// after one, two and three loss-and-recovery cycles. The guess for the frame that
+	// re-initialises the map is now folded into the pose before that frame is processed,
+	// so the new map is anchored where the robot actually is.
+	const double truth = 1.3 * speed;
+	EXPECT_NEAR(truth, odom->back().pose.pose.position.x, 0.02)
+			<< "the recovered pose does not match where the guess says the robot is";
+	EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.02)
+			<< "the robot drove straight";
+}
+
+
+/**
+ * The same recovery contract on the other reset path: min_update_rate.
+ *
+ * The robot drives at 1 m/s and a second passes without a frame -- twice the configured
+ * limit -- so the odometry is reset. A guess frame is available throughout, and the pose
+ * after the gap should be where the wheels say the robot is.
+ *
+ * This used to report 0.2 m against a true 1.2, losing the whole gap rather than one
+ * frame of it: tooOldPreviousData was handled before the guess for the frame was worked
+ * out, so guess_ was still null from the last successful frame, the "reset based on
+ * latest guess available from TF" branch never ran, and the node carried on from where
+ * it was when it went quiet. The reset now happens after the guess is computed.
+ */
+TEST_F(OdometryRosTest, recovers_on_the_guess_after_min_update_rate_resets_it)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("min_update_rate", 2.0),   // half a second
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishGuessMotion(1.0, 2.5, 1.0));   // 1 m/s, x(t) = t - 1.0
+
+	const auto drive = [&](double stamp) {
+		pub->publish(makeXYZCloud("lidar", stamp,
+				corner3D(cv::Point3f(float(stamp - 1.0), 0, 0))));
+		spinFor(std::chrono::milliseconds(120));
+	};
+
+	drive(1.0);
+	drive(1.1);              // tracking, a tenth of a metre along
+	drive(2.1);              // a second later: the gap resets the odometry
+	drive(2.2);
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+	spinFor(std::chrono::milliseconds(200));
+
+	EXPECT_LT(odom->back().pose.covariance[0], 9999.0) << "still lost after the gap";
+	EXPECT_NEAR(1.2, odom->back().pose.pose.position.x, 0.02)
+			<< "the pose after the gap does not match the guess";
+}
+
+/**
+ * @brief The same metre of being lost, in the default configuration.
+ *
+ * `publish_null_when_lost` on is what most robots run: every lost frame is announced with
+ * a null pose, and the frame that restarts the map says it cannot be linked to what came
+ * before. What must not differ is *where* the odometry restarts -- the guess is folded
+ * into the pose either way, so the trajectory has to come back where the wheels say it is.
+ */
+TEST_F(OdometryRosTest, recovers_on_the_guess_after_a_metre_of_being_lost_announcing_each_loss)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("Odom/ResetCountdown", "1"),
+	          // Point to plane, so the scan's own shape is what decides whether a frame
+	          // is good enough to start a map on, rather than how ICP happened to fail.
+	          // The threshold sits between the two scenes this test feeds it: the corner
+	          // measures 0.016 and the line of degenerateCloud() 0.0001, and the 0.02
+	          // default would turn away both.
+	          rclcpp::Parameter("Icp/PointToPlane", "true"),
+	          rclcpp::Parameter("Icp/PointToPlaneMinComplexity", "0.005"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	// 1 m/s from t=1.0 to t=2.5, which covers every frame below: x(t) = t - 1.0.
+	const double speed = 1.0;
+	ASSERT_TRUE(publishGuessMotion(1.0, 2.5, speed));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking";
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(float(0.1*speed), 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+	const size_t whileTracking = odom->size();
+
+	// A second of nothing to register against, at 10 Hz, while the wheels carry on.
+	for(int i=2; i<=11; ++i)
+	{
+		pub->publish(degenerateCloud(1.0 + 0.1*i));
+		spinFor(std::chrono::milliseconds(60));
+	}
+
+	// Every one of them is announced, which is the whole point of the default.
+	ASSERT_GT(odom->size(), whileTracking) << "being lost was never announced";
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0)
+			<< "a usable pose while there was nothing to register";
+
+	// The scene comes back, seen from where the wheels say the robot now is. Two frames:
+	// the line was never good enough to start a map on, so the first corner does that
+	// and the second is the first that has something to be registered against.
+	pub->publish(makeXYZCloud("lidar", 2.2, corner3D(cv::Point3f(float(1.2*speed), 0, 0))));
+	spinFor(std::chrono::milliseconds(200));
+	pub->publish(makeXYZCloud("lidar", 2.3, corner3D(cv::Point3f(float(1.3*speed), 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->back().pose.covariance[0] < 9999.0; }))
+			<< "never recovered once there was something to register again";
+
+	// And the trajectory resumes where the wheels say, exactly as with null publishing off.
+	EXPECT_NEAR(1.3*speed, odom->back().pose.pose.position.x, 0.02)
+			<< "the recovered pose does not match where the guess says the robot is";
+	EXPECT_NEAR(0.0, odom->back().pose.pose.position.y, 0.02) << "the robot drove straight";
+}
+
+
+/**
+ * The same service with a guess frame configured restarts the pose there instead.
+ *
+ * Resetting to the identity would put the odometry somewhere the robot has not been, and
+ * the guess source knows better: it has been tracking the whole time. So the pose picks up
+ * the guess frame's current pose and the map is rebuilt around it.
+ *
+ * reset_odom_to_pose is not affected -- a pose asked for explicitly is left alone.
+ */
+TEST_F(OdometryRosTest, reset_odom_restarts_from_the_guess_frame_when_one_is_configured)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishGuessMotion(1.0, 3.0, 1.0));   // 1 m/s, so the frame is at x = t - 1.0
+
+	const auto drive = [&](double stamp) {
+		pub->publish(makeXYZCloud("lidar", stamp,
+				corner3D(cv::Point3f(float(stamp - 1.0), 0, 0))));
+		spinFor(std::chrono::milliseconds(120));
+	};
+
+	drive(1.0);
+	drive(1.1);
+	drive(1.2);
+	ASSERT_FALSE(odom->empty());
+	EXPECT_NEAR(0.2, odom->back().pose.pose.position.x, 0.02) << "not tracking before the reset";
+
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+
+	const size_t before = odom->size();
+	drive(1.3);   // seeds the pose from the guess frame and initialises the map there
+	drive(1.4);   // registers against it
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }));
+	spinFor(std::chrono::milliseconds(200));
+
+	// 0.4 m: where the guess frame is at that stamp, not the origin the service name
+	// suggests. Without a guess frame the same call does return to the origin, which is
+	// what reset_odom_returns_the_pose_to_the_origin covers.
+	EXPECT_NEAR(0.4, odom->back().pose.pose.position.x, 0.02)
+			<< "the reset did not restart from the guess frame";
+}
+
+
+/**
+ * With no guess frame, a reset looks for a fused pose in TF before falling back.
+ *
+ * If something else publishes odom -> base_link -- an EKF fusing wheels and IMU, say --
+ * then that is a better answer than the pose this node had when it lost tracking, and the
+ * automatic reset adopts it. Without it, the reset keeps the latest computed pose, which
+ * the other reset tests cover.
+ */
+TEST_F(OdometryRosTest, reset_countdown_adopts_a_fused_pose_from_tf_when_there_is_one)
+{
+	publishSensorTf();
+	publishFusedPose(3.0);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	// The configuration this branch is written for: another node owns odom -> base_link,
+	// so this one does not publish it, and a filter is consuming the odom topic, so null
+	// poses are suppressed rather than fed to it.
+	makeNode({rclcpp::Parameter("Odom/ResetCountdown", "1"),
+	          rclcpp::Parameter("publish_tf", false),
+	          rclcpp::Parameter("publish_null_when_lost", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	// One less message than frames: the first one initialises the odometry.
+	feedFrames(pub, odom, 3, 0.05f, 1);
+	ASSERT_GE(odom->size(), 2u);
+
+	// Lose it: the countdown fires and goes looking for a pose in TF. Nothing is published
+	// for this frame, nor for the one that initialises the map after it -- that one has no
+	// velocity behind it yet.
+	const size_t whileTracking = odom->size();
+	pub->publish(degenerateCloud(5.0));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_EQ(whileTracking, odom->size()) << "a null pose went out with null publishing off";
+
+	// Something to register against again: the first initialises the map at the adopted
+	// pose, the second registers against it and is published.
+	pub->publish(makeXYZCloud("lidar", 6.0, corner3D()));
+	spinFor(std::chrono::milliseconds(300));
+	pub->publish(makeXYZCloud("lidar", 6.1, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > whileTracking; }))
+			<< "nothing came back after the odometry recovered";
+
+	// The robot has not moved since, so the pose is the one the reset adopted.
+	EXPECT_NEAR(3.0, odom->back().pose.pose.position.x, 0.02)
+			<< "the reset did not adopt the fused pose published on odom -> base_link";
+}
+
+/// The same fallback on the other reset path, the one min_update_rate triggers.
+TEST_F(OdometryRosTest, min_update_rate_adopts_a_fused_pose_from_tf_when_there_is_one)
+{
+	publishSensorTf();
+	publishFusedPose(2.0);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	// Same configuration as above: the fused source owns odom -> base_link, and a filter
+	// downstream means null poses are suppressed.
+	makeNode({rclcpp::Parameter("min_update_rate", 2.0),   // half a second
+	          rclcpp::Parameter("publish_tf", false),
+	          rclcpp::Parameter("publish_null_when_lost", false)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	// One less message than frames: the first one initialises the odometry.
+	feedFrames(pub, odom, 3, 0.05f, 1);
+	ASSERT_GE(odom->size(), 2u);
+
+	// A frame well past the limit: the odometry is reset, and TF has a better pose. That
+	// frame initialises the map and is not published, having no velocity behind it; the
+	// next one registers against it and is.
+	const size_t before = odom->size();
+	pub->publish(makeXYZCloud("lidar", 5.0, corner3D()));
+	spinFor(std::chrono::milliseconds(300));
+	pub->publish(makeXYZCloud("lidar", 5.1, corner3D()));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > before; }))
+			<< "nothing came back after the gap";
+
+	EXPECT_NEAR(2.0, odom->back().pose.pose.position.x, 0.02)
+			<< "the reset did not adopt the fused pose published on odom -> base_link";
+}
+
+
+/**
+ * @brief With the guess frame vouching for it, a reset is published rather than swallowed.
+ *
+ * The frame that restarts the map has nothing to measure against, so it normally carries
+ * 9999 on both covariances -- rtabmap's signal to start a new map rather than link across
+ * a jump. With `publish_null_when_lost:=false` that frame was not published at all: the
+ * map stayed in one piece, but its consumer lost a pose. It now goes out carrying the
+ * guess frame's own confidence and velocity, which is what makes the poses *and* the
+ * covariances on this topic continuous for as long as the guess is there.
+ */
+TEST_F(OdometryRosTest, publishes_the_restarting_frame_as_a_continuation_of_the_guess)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("publish_null_when_lost", false),
+	          rclcpp::Parameter("Odom/ResetCountdown", "1"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	const double speed = 1.0;   // m/s, frames at 10 Hz: x(t) = t - 1.0
+	ASSERT_TRUE(publishGuessMotion(1.0, 2.0, speed));
+
+	// Tracking, then one frame with nothing in it, which the countdown turns into a reset.
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	spinFor(std::chrono::milliseconds(200));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })) << "never started tracking";
+	pub->publish(degenerateCloud(1.2));
+	spinFor(std::chrono::milliseconds(200));
+	const size_t beforeRestart = odom->size();
+
+	// The scene comes back: this is the frame that restarts the map.
+	pub->publish(makeXYZCloud("lidar", 1.3, corner3D(cv::Point3f(0.3f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart; }))
+			<< "the frame that restarts the map was not published";
+
+	const nav_msgs::msg::Odometry restart = odom->back();
+	EXPECT_LT(restart.pose.covariance[0], 9999.0)
+			<< "the restarting frame still says it cannot be linked to what came before";
+	EXPECT_LT(restart.twist.covariance[0], 9999.0)
+			<< "rtabmap starts a new map when both covariances say 9999";
+	// The guess's own confidence, doubled into the pose as everywhere else on this topic.
+	EXPECT_DOUBLE_EQ(0.002, restart.pose.covariance[0]);
+	EXPECT_DOUBLE_EQ(0.001, restart.twist.covariance[0]);
+	EXPECT_DOUBLE_EQ(0.001, restart.twist.covariance[21]);
+	// And the guess's own velocity, there being no registration to take one from.
+	EXPECT_NEAR(speed, restart.twist.twist.linear.x, 0.05)
+			<< "the twist of the restarting frame does not match the guess it came from";
+	EXPECT_NEAR(0.3*speed, restart.pose.pose.position.x, 0.02)
+			<< "the map restarted somewhere other than where the guess says the robot is";
+
+	// From the next frame on it is an ordinary registration again, with its own covariance.
+	pub->publish(makeXYZCloud("lidar", 1.4, corner3D(cv::Point3f(0.4f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart + 1; }));
+	EXPECT_NE(0.002, odom->back().pose.covariance[0])
+			<< "still reporting the guess's confidence after the registration resumed";
+	EXPECT_LT(odom->back().pose.covariance[0], 9999.0);
+}
+
+/// Unchanged by default: the restart still announces itself, which is what starts a new map.
+TEST_F(OdometryRosTest, marks_the_restarting_frame_as_unlinked_when_null_publishing_is_on)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("guess_frame_id", "wheel_odom"),
+	          rclcpp::Parameter("Odom/ResetCountdown", "1"),
+	          rclcpp::Parameter("wait_for_transform", 2.0)});
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(publishGuessMotion(1.0, 2.0, 1.0));
+
+	pub->publish(makeXYZCloud("lidar", 1.0, corner3D()));
+	spinFor(std::chrono::milliseconds(200));
+	pub->publish(makeXYZCloud("lidar", 1.1, corner3D(cv::Point3f(0.1f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(degenerateCloud(1.2));
+	spinFor(std::chrono::milliseconds(200));
+	const size_t beforeRestart = odom->size();
+
+	pub->publish(makeXYZCloud("lidar", 1.3, corner3D(cv::Point3f(0.3f, 0, 0))));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() > beforeRestart; }));
+
+	// The pose is the recovered one, but both covariances say it is not linked to the
+	// trajectory before it, which is how rtabmap knows to start a new map.
+	EXPECT_NEAR(0.3, odom->back().pose.pose.position.x, 0.02);
+	EXPECT_GE(odom->back().pose.covariance[0], 9999.0);
+	EXPECT_GE(odom->back().twist.covariance[0], 9999.0);
+}
+
+}  // namespace
+}  // namespace rtabmap_odom_test
diff --git a/rtabmap_odom/test/test_rgbd_odometry.cpp b/rtabmap_odom/test/test_rgbd_odometry.cpp
new file mode 100644
index 00000000..d3f7fb00
--- /dev/null
+++ b/rtabmap_odom/test/test_rgbd_odometry.cpp
@@ -0,0 +1,800 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <tf2_ros/static_transform_broadcaster.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap_odom/rgbd_odometry.hpp>
+
+#include <cmath>
+
+#include <rtabmap/core/Version.h>
+
+#include "camera_rig.hpp"
+#include "msg_builders.hpp"
+#include "node_test_utils.hpp"
+#include "test_data.hpp"
+
+namespace rtabmap_odom_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/**
+ * The frames that carry a scene come from test/data/rgbd -- the same two RGB-D frames
+ * RTAB-Map registers in corelib/test/test_odometry.cpp. These tests assert the ROS-level
+ * contract (which topics are subscribed, what is published, how the parameters wire up)
+ * on real input rather than the accuracy of the registration, which is that test's
+ * business.
+ */
+const char * const kFrame = "17";
+const char * const kLaterFrame = "154";   // much further along the sequence
+
+/// The blank scene the "lost tracking" tests need is synthetic: there is nothing to see in it.
+const int kBlankWidth = 160;
+const int kBlankHeight = 120;
+
+double translationNorm(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Point & p = odom.pose.pose.position;
+	return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
+}
+
+/// The angle of the pose's rotation, in radians.
+double rotationAngle(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
+	return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
+}
+
+/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
+bool isLost(const nav_msgs::msg::Odometry & odom)
+{
+	return odom.pose.covariance[0] >= 9999.0;
+}
+
+class RgbdOdometryTest : public NodeTest
+{
+protected:
+	void publishSensorTf()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = helper()->now();
+		tf.header.frame_id = "base_link";
+		tf.child_frame_id = "camera";
+		tf.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(tf);
+	}
+
+	/**
+	 * @brief Waits until the node under test has subscribed to /tf_static.
+	 *
+	 * The fixture sends the sensor transform before the node exists, so the node's TF
+	 * listener only sees it as the retained transient-local message it gets on discovery.
+	 * Publishing an image before that arrives makes the node drop the frame after its
+	 * 100 ms wait_for_transform, for no reason the test can see.
+	 */
+	bool waitForTfListener()
+	{
+		if(!staticTf_)
+		{
+			return true;
+		}
+		return spinUntil([&]() { return helper()->count_subscribers("/tf_static") >= 1; });
+	}
+
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> makeNode(
+			std::vector<rclcpp::Parameter> params = {})
+	{
+		// Defaults first, so a test that passes the same parameter overrides them.
+		//
+		// always_process_most_recent_frame:=false is what the node itself recommends for
+		// data that arrives faster than its stamps: these tests publish a whole sequence
+		// back to back with stamps a tenth of a second apart, and when the executor is
+		// slow enough that two of them land in the same spin -- a loaded CI runner, a
+		// single core -- the node drops the second as a replay glitch and the test waits
+		// for a message that will never come. It also keeps processing on the calling
+		// thread instead of the node's worker, which is what makes these tests observable
+		// at all: the odometry is finished by the time the publish returns.
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("frame_id", "base_link"),
+			rclcpp::Parameter("publish_tf", false),
+			rclcpp::Parameter("always_process_most_recent_frame", false),
+		};
+		all.insert(all.end(), params.begin(), params.end());
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(all);
+		std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
+				addNode(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
+		waitForTfListener();
+		return node;
+	}
+
+	/// Calls one of the node's std_srvs/Empty services and waits for the answer.
+	bool callEmptyService(const std::string & name)
+	{
+		rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+				helper()->create_client<std_srvs::srv::Empty>("/rgbd_odometry/" + name);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return false;
+		}
+		std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
+				client->async_send_request(
+						std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
+		return spinUntil([&]() {
+			return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
+	}
+
+	/// Where each camera of a rig is mounted, as its driver would publish it once.
+	void publishRigTf(const CameraRig & rig)
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now()));
+	}
+
+	/// One frame from test/data/rgbd, as rgbd_sync would deliver it: bgr8 plus 16UC1 millimetres.
+	rtabmap_msgs::msg::RGBDImage makeFrame(const std::string & name, double stamp)
+	{
+		const cv::Mat rgb = rgbdColorImage(name);
+		const cv::Mat depth = rgbdDepthImage(name);
+		EXPECT_FALSE(rgb.empty()) << "test/data/rgbd/rgb/" << name << ".jpg missing";
+		EXPECT_FALSE(depth.empty()) << "test/data/rgbd/depth/" << name << ".png missing";
+
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
+		msg.depth = makeImage("camera", stamp, depth, "16UC1");
+		msg.rgb_camera_info = rgbdInfo(name, "camera", stamp);
+		msg.depth_camera_info = msg.rgb_camera_info;
+		return msg;
+	}
+
+	/// A scene with nothing in it: no features to detect, no motion to recover.
+	rtabmap_msgs::msg::RGBDImage makeBlankFrame(double stamp)
+	{
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeImage("camera", stamp,
+				cv::Mat::zeros(kBlankHeight, kBlankWidth, CV_8UC1), "mono8");
+		msg.depth = makeImage("camera", stamp,
+				cv::Mat(kBlankHeight, kBlankWidth, CV_32FC1, cv::Scalar(2.0f)), "32FC1");
+		msg.rgb_camera_info = makeCameraInfo("camera", stamp, kBlankWidth, kBlankHeight);
+		msg.depth_camera_info = msg.rgb_camera_info;
+		return msg;
+	}
+
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+};
+
+/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything.
+TEST_F(RgbdOdometryTest, the_test_calibration_describes_the_camera)
+{
+	const sensor_msgs::msg::CameraInfo info = rgbdInfo(kFrame, "camera", 1.0);
+
+	ASSERT_EQ(640u, info.width);
+	ASSERT_EQ(480u, info.height);
+	EXPECT_DOUBLE_EQ(525.0, info.k[0]);
+	EXPECT_DOUBLE_EQ(525.0, info.k[4]);
+	// No projection_matrix in the file: P falls back to [K|0], an already-rectified camera.
+	EXPECT_DOUBLE_EQ(info.k[0], info.p[0]);
+	EXPECT_DOUBLE_EQ(0.0, info.p[3]);
+}
+
+/// By default the node takes the three raw camera topics.
+TEST_F(RgbdOdometryTest, subscribes_to_the_raw_camera_topics_by_default)
+{
+	publishSensorTf();
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+
+	EXPECT_TRUE(waitForSubscriber(rgb));
+	EXPECT_TRUE(waitForSubscriber(depth));
+	EXPECT_TRUE(waitForSubscriber(info));
+}
+
+/// A synchronized set of the three raw topics produces one odometry message, at the origin.
+TEST_F(RgbdOdometryTest, publishes_odom_for_a_synchronized_raw_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgb =
+			helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth =
+			helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(rgb));
+	ASSERT_TRUE(waitForSubscriber(depth));
+	ASSERT_TRUE(waitForSubscriber(info));
+
+	// Identical stamps, so this works under either synchronization policy.
+	const rtabmap_msgs::msg::RGBDImage frame = makeFrame(kFrame, 1.0);
+	rgb->publish(frame.rgb);
+	depth->publish(frame.depth);
+	info->publish(frame.rgb_camera_info);
+
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_EQ("odom", odom->back().header.frame_id);
+	EXPECT_EQ("base_link", odom->back().child_frame_id);
+	// The first frame has nothing to register against: it defines the origin.
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9);
+}
+
+/// subscribe_rgbd swaps the three topics for one pre-synchronized RGBDImage.
+TEST_F(RgbdOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+}
+
+/**
+ * The same frame twice: the registration runs on real features and depth, and the only
+ * answer consistent with the input is "I have not moved". A node that mangles the depth
+ * units or the calibration on the way into RTAB-Map fails here, where the textureless
+ * scenes below cannot tell the difference.
+ */
+TEST_F(RgbdOdometryTest, registers_a_repeated_frame_as_no_motion)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kFrame, 1.1));
+	// Both collectors: odom and odom_info are published separately, and the assertions
+	// below compare the second of each.
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking on a frame identical to the previous one";
+	EXPECT_GT(info->back().features, 20) << "no features found in a real scene";
+	EXPECT_GT(info->back().inliers, 20)
+			<< "too few inliers (matches=" << info->back().matches << ")";
+	// Exactly zero on this build, in both translation and rotation; a millimetre and a
+	// milliradian leave room for a backend that answers with rounding noise instead.
+	EXPECT_LT(translationNorm(second), 0.001) << "motion reported between identical frames";
+	EXPECT_LT(rotationAngle(second), 0.001) << "rotation reported between identical frames";
+}
+
+/**
+ * Frames 17 and 154 are far apart in the sequence, so losing tracking is a legitimate
+ * outcome; what must hold is that the node's answer agrees with itself -- either a pose
+ * it stands behind, of a plausible size, or one flagged as unusable.
+ */
+TEST_F(RgbdOdometryTest, stays_consistent_between_two_distant_frames)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kLaterFrame, 1.1));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	if(!isLost(second))
+	{
+		// 0.41 to 0.46 m over ten runs here. The bound stays a plausibility check rather
+		// than a fit: how far apart these two frames land is the registration's business,
+		// and this test's claim is only that the answer is not nonsense.
+		EXPECT_LT(translationNorm(second), 2.0)
+				<< "implausible jump of " << translationNorm(second) << " m";
+	}
+}
+
+/**
+ * @brief Two to six cameras, each on its own numbered topic.
+ *
+ * Above six the node has no synchronizer for it and says to use rgbd_cameras:=0 with the
+ * rgbd_images topic instead, so six is where this stops. Only the subscriptions are
+ * checked: one numbered topic per camera, none left behind.
+ */
+class RgbdOdometryCamerasTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(RgbdOdometryCamerasTest, subscribes_to_one_numbered_topic_per_camera)
+{
+	const int cameras = GetParam();
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras)});
+
+	// All of them first, so they are discovered together rather than one wait after another.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+
+	for(int i=0; i<cameras; ++i)
+	{
+		EXPECT_TRUE(waitForSubscriber(publishers[i]))
+				<< "rgbd_cameras:=" << cameras << " left rgbd_image" << i << " unsubscribed";
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		RgbdCameras,
+		RgbdOdometryCamerasTest,
+		::testing::Range(2, 7),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return std::to_string(info.param) + "_cameras";
+		});
+
+/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message.
+TEST_F(RgbdOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic)
+{
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+
+	EXPECT_TRUE(waitForSubscriber(pub));
+}
+
+/// This node matches by nearest stamp unless told otherwise; stereo_odometry does not.
+TEST_F(RgbdOdometryTest, approx_sync_is_on_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> node = makeNode();
+
+	EXPECT_TRUE(node->get_parameter("approx_sync").as_bool());
+}
+
+/**
+ * A textureless scene is the documented failure: there is nothing to match, so the frame
+ * is lost and the node says so with a null pose rather than publishing nothing.
+ * See "When it loses track" in doc/rgbd_odometry.md.
+ */
+TEST_F(RgbdOdometryTest, reports_lost_with_a_null_pose_on_a_textureless_scene)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeBlankFrame(1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeBlankFrame(1.1));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	// Nothing to register against: no features, and the pose carries the "do not use me"
+	// covariance rather than the node going silent.
+	EXPECT_EQ(0, info->back().features);
+	EXPECT_TRUE(isLost(odom->back()));
+}
+
+/// publish_null_when_lost:=false makes the node go silent instead.
+TEST_F(RgbdOdometryTest, publishes_nothing_when_lost_if_null_publishing_is_off)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("publish_null_when_lost", false)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeBlankFrame(1.0));
+	pub->publish(makeBlankFrame(1.1));
+	spinFor(std::chrono::milliseconds(1500));
+
+	EXPECT_TRUE(odom->empty());
+}
+
+
+/**
+ * keep_color decides what reaches RTAB-Map from a color image, and therefore what the
+ * node republishes: the matcher works in grayscale, so the color is dropped unless asked
+ * for. Same contract as stereo_odometry, checked here because the doc states it of this
+ * node too.
+ */
+TEST_F(RgbdOdometryTest, keeps_the_image_in_color_when_asked)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("keep_color", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("bgr8", frames->back().rgb.encoding);
+}
+
+/// Off by default: what reaches RTAB-Map, and comes back out, is grayscale.
+TEST_F(RgbdOdometryTest, converts_the_image_to_grayscale_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	std::shared_ptr<rtabmap_odom::RGBDOdometry> node =
+			makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+	EXPECT_FALSE(node->get_parameter("keep_color").as_bool());
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("mono8", frames->back().rgb.encoding);
+}
+
+/**
+ * The two feature topics the lidar node cannot fill: both are built from the frame's
+ * visual words, so they carry content only on the visual paths. `odom_local_map` is the
+ * map the frame was registered against, `odom_last_frame` the frame's own features, both
+ * in the odom frame.
+ */
+TEST_F(RgbdOdometryTest, publishes_the_feature_map_and_the_frame_that_registered_against_it)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> localMap =
+			collect<sensor_msgs::msg::PointCloud2>("odom_local_map");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> lastFrame =
+			collect<sensor_msgs::msg::PointCloud2>("odom_last_frame");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(localMap->subscription));
+	ASSERT_TRUE(waitForPublisher(lastFrame->subscription));
+
+	pub->publish(makeFrame(kFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	pub->publish(makeFrame(kFrame, 1.1));
+	// All three collectors: the clouds are published after the odometry of the same frame,
+	// so waiting for odom alone leaves them one spin behind.
+	ASSERT_TRUE(spinUntil([&]() {
+				return odom->size() >= 2 && !localMap->empty() && !lastFrame->empty(); }));
+
+	// 534 features on this frame, in both, expressed in the odometry frame.
+	ASSERT_FALSE(localMap->empty()) << "no feature map was published";
+	ASSERT_FALSE(lastFrame->empty()) << "no frame features were published";
+	EXPECT_GT(localMap->back().width, 0u);
+	EXPECT_GT(lastFrame->back().width, 0u);
+	EXPECT_EQ("odom", lastFrame->back().header.frame_id)
+			<< "these are published in the odometry frame, not the sensor's";
+}
+
+
+/**
+ * @brief A four-camera rig, driven a metre through a world of points.
+ *
+ * The frames carry their features -- keypoints, 3D points, descriptors -- and no image at
+ * all, as a driver that does its own extraction publishes them. So the trajectory below
+ * can only come from the features: the control test that follows runs the same frames
+ * with them stripped off, and it finds nothing.
+ *
+ * Both estimation types the multi-camera case supports are run. Vis/EstimationType=0
+ * aligns the two sets of 3D points, which needs nothing extra; =1 solves a PnP across
+ * all four cameras at once, which RTAB-Map hands to OpenGV and cannot do without it.
+ */
+class RgbdOdometryRigTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(RgbdOdometryRigTest, recovers_the_trajectory_of_a_rig_from_the_features_it_is_given)
+{
+	const int estimationType = GetParam();
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	// A metre forward, ten centimetres at a time.
+	const int frames = 11;
+	rtabmap_msgs::msg::RGBDImages lastFrame;
+	for(int i=0; i<frames; ++i)
+	{
+		lastFrame = cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(rig.cameras(), lastFrame.rgbd_images.size());
+		pub->publish(lastFrame);
+		// Both collectors: odom and odom_info are published separately, and the feature
+		// count asserted below is read from the odom_info of this same frame.
+		ASSERT_TRUE(spinUntil([&]() {
+					return odom->size() >= size_t(i+1) && info->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking on a rig that sees the whole scene";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05)
+			<< "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+	EXPECT_NEAR(0.0, rotationAngle(last), 0.05) << "the rig never turned";
+
+	// The frame's own features, reassembled from the four cameras and used as they are.
+	// A couple can go missing on the way: RTAB-Map drops a feature whose descriptor lands
+	// on the same visual word as another one of the same frame, both being ambiguous then
+	// (the `count(*iter) == 1` guards in RegistrationVis). What matters here is that the
+	// number is the frame's own and not zero, which is all a blank image could give.
+	const int sent = int(cameraRigFeatureCount(lastFrame));
+	EXPECT_LE(info->back().features, sent);
+	EXPECT_GT(info->back().features, sent - 10)
+			<< "the node did not use the features the frame came with";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		EstimationTypes,
+		RgbdOdometryRigTest,
+		::testing::Values(0, 1),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return info.param == 0 ? std::string("3d_to_3d") : std::string("pnp_across_cameras");
+		});
+
+/**
+ * The control for the test above: the same frames with the features stripped off. What is
+ * left is four calibrations and nothing to see, which the node is right to process -- an
+ * empty scene is still a frame -- and right to report lost.
+ */
+TEST_F(RgbdOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track)
+{
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	for(int i=0; i<2; ++i)
+	{
+		rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		for(size_t c=0; c<frame.rgbd_images.size(); ++c)
+		{
+			frame.rgbd_images[c].key_points.clear();
+			frame.rgbd_images[c].points.clear();
+			frame.rgbd_images[c].descriptors.clear();
+		}
+		pub->publish(frame);
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
+	EXPECT_EQ(0, info->back().features) << "features appeared from a frame that has none";
+	EXPECT_TRUE(isLost(odom->back()));
+}
+
+/**
+ * Odom/ImageDecimation shrinks the image before registering it, and scales the
+ * calibration to match. Features that arrived with the frame are placed in the full size
+ * image, so they have to be brought down with it: read against a calibration half their
+ * scale, a rig's keypoints land in the wrong camera altogether.
+ *
+ * The frames here carry a blank image for the decimation to have something to work on,
+ * and the trajectory has to come out the same as without it.
+ */
+TEST_F(RgbdOdometryRigTest, decimation_brings_the_given_features_down_with_the_image)
+{
+#ifndef RTABMAP_OPENGV
+	// Only a PnP reads the keypoints this test is about; a 3D-to-3D registration would
+	// come out right even with every one of them misplaced.
+	GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+#endif
+
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0),
+	          rclcpp::Parameter("Odom/ImageDecimation", "2")});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const int frames = 11;
+	for(int i=0; i<frames; ++i)
+	{
+		pub->publish(cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0),
+				1.0 + 0.1*i, /*withImages=*/true));
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "the features were lost on the way into the decimated frame";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+}
+
+/**
+ * @brief The same rig on the numbered topics, one to six cameras.
+ *
+ * `rgbd_cameras:=N` subscribes to N topics and synchronizes them with a callback of its
+ * own per N, six of them in all. The test above drives the `rgbd_cameras:=0` one; these
+ * drive the rest, by publishing each camera of the rig on its own topic and asking for
+ * the same metre back.
+ *
+ * Each of them runs both estimation types, except that a multi-camera PnP needs OpenGV
+ * and is skipped when RTAB-Map was built without it. A single camera does not, so that
+ * one is run either way.
+ */
+class RgbdOdometryRigCamerasTest :
+		public RgbdOdometryTest,
+		public ::testing::WithParamInterface<std::tuple<int, bool, int>>
+{
+};
+
+TEST_P(RgbdOdometryRigCamerasTest, recovers_the_trajectory_from_the_numbered_topics)
+{
+	const int cameras = std::get<0>(GetParam());
+	const bool approxSync = std::get<1>(GetParam());
+	const int estimationType = std::get<2>(GetParam());
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1 && cameras > 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig(cameras);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras),
+	          rclcpp::Parameter("approx_sync", approxSync),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	// One camera listens on rgbd_image, more than one on rgbd_image0..N-1.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10));
+	}
+	for(int i=0; i<cameras; ++i)
+	{
+		ASSERT_TRUE(waitForSubscriber(publishers[i])) << "camera " << i << " has no subscriber";
+	}
+
+	const int frames = 11;
+	for(int i=0; i<frames; ++i)
+	{
+		const rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(size_t(cameras), frame.rgbd_images.size());
+		for(int c=0; c<cameras; ++c)
+		{
+			publishers[c]->publish(frame.rgbd_images[c]);
+		}
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking with " << cameras << " camera(s)";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05) << "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+
+	// A reset tears the synchronizer down and builds it again -- one per camera count,
+	// and a different one for each of the two sync policies. Frames have to keep arriving
+	// through the new one.
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+	const size_t beforeReset = odom->size();
+	const rtabmap_msgs::msg::RGBDImages frame =
+			cameraRigFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1);
+	for(int c=0; c<cameras; ++c)
+	{
+		publishers[c]->publish(frame.rgbd_images[c]);
+	}
+	EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; }))
+			<< "nothing came back after the reset rebuilt the synchronizer";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		RgbdCameras,
+		RgbdOdometryRigCamerasTest,
+		::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)),
+		[](const ::testing::TestParamInfo<std::tuple<int, bool, int>> & info) {
+			const int cameras = std::get<0>(info.param);
+			return std::to_string(cameras) + (cameras == 1 ? "_camera_" : "_cameras_") +
+					(std::get<1>(info.param) ? "approx_sync_" : "exact_sync_") +
+					(std::get<2>(info.param) == 0 ? "3d_to_3d" : "pnp");
+		});
+
+}  // namespace
+}  // namespace rtabmap_odom_test
diff --git a/rtabmap_odom/test/test_stereo_odometry.cpp b/rtabmap_odom/test/test_stereo_odometry.cpp
new file mode 100644
index 00000000..d400cb21
--- /dev/null
+++ b/rtabmap_odom/test/test_stereo_odometry.cpp
@@ -0,0 +1,1272 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <tf2_ros/static_transform_broadcaster.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+
+#include <rtabmap_odom/stereo_odometry.hpp>
+
+#include <cmath>
+
+#include <rtabmap/core/Version.h>
+
+#include "camera_rig.hpp"
+#include "msg_builders.hpp"
+#include "node_test_utils.hpp"
+#include "test_data.hpp"
+
+namespace rtabmap_odom_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/**
+ * These assert the ROS-level contract -- topics, parameters, what gets published -- on
+ * real input: the pairs in test/data/stereo, rectified ones from the set RTAB-Map
+ * registers in corelib/test/test_odometry.cpp and unrectified ones for the paths the
+ * documentation describes around Rtabmap/ImagesAlreadyRectified. The accuracy of the
+ * registration is that test's business; what is checked here is that a frame published on
+ * the four raw topics comes out of this node as a plausible, non-degenerate odometry
+ * message.
+ */
+const char * const kFirstFrame = "50";
+const char * const kSecondFrame = "60";   // ~15 cm of motion from the first
+
+/// Straight off the camera, still distorted: test/data/stereo/raw, 21.00 s and 21.25 s.
+const char * const kFirstRawFrame = "420";
+const char * const kSecondRawFrame = "425";
+
+double translationNorm(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Point & p = odom.pose.pose.position;
+	return std::sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
+}
+
+/// The angle of the pose's rotation, in radians.
+double rotationAngle(const nav_msgs::msg::Odometry & odom)
+{
+	const geometry_msgs::msg::Quaternion & q = odom.pose.pose.orientation;
+	return 2.0 * std::acos(std::min(1.0, std::fabs(q.w)));
+}
+
+/// rtabmap_odom marks a pose it does not trust with a 9999 covariance rather than staying silent.
+bool isLost(const nav_msgs::msg::Odometry & odom)
+{
+	return odom.pose.covariance[0] >= 9999.0;
+}
+
+class StereoOdometryTest : public NodeTest
+{
+protected:
+	void publishSensorTf()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		geometry_msgs::msg::TransformStamped tf;
+		tf.header.stamp = helper()->now();
+		tf.header.frame_id = "base_link";
+		tf.child_frame_id = "camera";
+		tf.transform.rotation.w = 1.0;
+		staticTf_->sendTransform(tf);
+	}
+
+	/**
+	 * @brief Waits until the node under test has subscribed to /tf_static.
+	 *
+	 * The fixture sends the sensor transform before the node exists, so the node's TF
+	 * listener only sees it as the retained transient-local message it gets on discovery.
+	 * Publishing an image before that arrives makes the node drop the frame after its
+	 * 100 ms wait_for_transform, for no reason the test can see.
+	 */
+	bool waitForTfListener()
+	{
+		if(!staticTf_)
+		{
+			return true;
+		}
+		return spinUntil([&]() { return helper()->count_subscribers("/tf_static") >= 1; });
+	}
+
+	/// Calls one of the node's std_srvs/Empty services and waits for the answer.
+	bool callEmptyService(const std::string & name)
+	{
+		rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+				helper()->create_client<std_srvs::srv::Empty>("/stereo_odometry/" + name);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return false;
+		}
+		std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> future =
+				client->async_send_request(
+						std::make_shared<std_srvs::srv::Empty::Request>()).future.share();
+		return spinUntil([&]() {
+			return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; });
+	}
+
+	/// Where each camera of a rig is mounted, as its driver would publish it once.
+	void publishRigTf(const CameraRig & rig)
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		staticTf_->sendTransform(cameraRigTransforms(rig, helper()->now()));
+	}
+
+	std::shared_ptr<rtabmap_odom::StereoOdometry> makeNode(
+			std::vector<rclcpp::Parameter> params = {})
+	{
+		// Defaults first, so a test that passes the same parameter overrides them.
+		//
+		// always_process_most_recent_frame:=false is what the node itself recommends for
+		// data that arrives faster than its stamps: these tests publish a whole sequence
+		// back to back with stamps a tenth of a second apart, and when the executor is
+		// slow enough that two of them land in the same spin -- a loaded CI runner, a
+		// single core -- the node drops the second as a replay glitch and the test waits
+		// for a message that will never come. It also keeps processing on the calling
+		// thread instead of the node's worker, which is what makes these tests observable
+		// at all: the odometry is finished by the time the publish returns.
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("frame_id", "base_link"),
+			rclcpp::Parameter("publish_tf", false),
+			rclcpp::Parameter("always_process_most_recent_frame", false),
+		};
+		all.insert(all.end(), params.begin(), params.end());
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(all);
+		std::shared_ptr<rtabmap_odom::StereoOdometry> node =
+				addNode(std::make_shared<rtabmap_odom::StereoOdometry>(options));
+		waitForTfListener();
+		return node;
+	}
+
+	/// The four raw topics a stereo driver publishes, which this node takes by default.
+	struct StereoPublishers
+	{
+		rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr left;
+		rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr right;
+		rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfo;
+		rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfo;
+	};
+
+	StereoPublishers makeStereoPublishers()
+	{
+		StereoPublishers pubs;
+		pubs.left = helper()->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
+		pubs.right = helper()->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
+		pubs.leftInfo = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
+		pubs.rightInfo = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 10);
+		return pubs;
+	}
+
+	bool waitForStereoSubscribers(const StereoPublishers & pubs)
+	{
+		return waitForSubscriber(pubs.left) && waitForSubscriber(pubs.right) &&
+				waitForSubscriber(pubs.leftInfo) && waitForSubscriber(pubs.rightInfo);
+	}
+
+	/**
+	 * @brief Publishes one pair from test/data/stereo as the four raw topics.
+	 * @param rightOffset added to the right camera's stamp, to break an exact pairing.
+	 *
+	 * Both cameras report the same frame_id, as a rectified rig does: the pair is already
+	 * in a common frame, so there is nothing for TF to say about the two of them.
+	 */
+	void publishStereoFrame(
+			const StereoPublishers & pubs, const std::string & name, double stamp,
+			double rightOffset = 0.0, StereoSet set = kRectified)
+	{
+		const cv::Mat left = stereoLeftImage(name, set);
+		const cv::Mat right = stereoRightImage(name, set);
+		const std::string dir = set == kRaw ? "raw" : "rect";
+		ASSERT_FALSE(left.empty()) << "test/data/stereo/" << dir << "/left/" << name << ".jpg missing";
+		ASSERT_FALSE(right.empty()) << "test/data/stereo/" << dir << "/right/" << name << ".jpg missing";
+
+		pubs.left->publish(makeImage("camera", stamp, left, "bgr8"));
+		pubs.right->publish(makeImage("camera", stamp + rightOffset, right, "mono8"));
+		pubs.leftInfo->publish(stereoLeftInfo("camera", stamp, set));
+		pubs.rightInfo->publish(stereoRightInfo("camera", stamp + rightOffset, set));
+	}
+
+	/**
+	 * @brief Publishes an unrectified pair with one frame_id per camera.
+	 *
+	 * Which is what an unrectified rig looks like: the two images are in different frames,
+	 * and Rtabmap/ImagesAlreadyRectified:=false makes the node ask TF for the transform
+	 * between them rather than reading the baseline out of the right camera's P.
+	 */
+	void publishRawStereoFrameInSplitFrames(
+			const StereoPublishers & pubs, const std::string & name, double stamp)
+	{
+		const cv::Mat left = stereoLeftImage(name, kRaw);
+		const cv::Mat right = stereoRightImage(name, kRaw);
+		ASSERT_FALSE(left.empty()) << "test/data/stereo/raw/left/" << name << ".jpg missing";
+		ASSERT_FALSE(right.empty()) << "test/data/stereo/raw/right/" << name << ".jpg missing";
+
+		pubs.left->publish(makeImage("camera_left", stamp, left, "bgr8"));
+		pubs.right->publish(makeImage("camera_right", stamp, right, "mono8"));
+		pubs.leftInfo->publish(stereoLeftInfo("camera_left", stamp, kRaw));
+		pubs.rightInfo->publish(stereoRightInfo("camera_right", stamp, kRaw));
+	}
+
+	/**
+	 * @brief One pair packed the way stereo_sync publishes it.
+	 *
+	 * The RGB-D message is reused for stereo: the left image and its calibration travel in
+	 * the rgb fields, the right ones in the depth fields. See stereo_sync.cpp.
+	 */
+	rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage(const std::string & name, double stamp)
+	{
+		const cv::Mat left = stereoLeftImage(name);
+		const cv::Mat right = stereoRightImage(name);
+		EXPECT_FALSE(left.empty()) << "test/data/stereo/rect/left/" << name << ".jpg missing";
+		EXPECT_FALSE(right.empty()) << "test/data/stereo/rect/right/" << name << ".jpg missing";
+
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeImage("camera", stamp, left, "bgr8");
+		msg.depth = makeImage("camera", stamp, right, "mono8");
+		msg.rgb_camera_info = stereoLeftInfo("camera", stamp);
+		msg.depth_camera_info = stereoRightInfo("camera", stamp);
+		return msg;
+	}
+
+	/**
+	 * @brief Publishes an unrectified pair whose camera_info messages carry no frame_id.
+	 *
+	 * A driver that leaves frame_id empty gives the node nothing to ask TF about. The
+	 * baseline in the right camera's P is then the only thing left to work from, which the
+	 * node falls back to -- unless @p zeroBaseline strips that too, and nothing remains.
+	 */
+	void publishRawStereoFrameWithUnnamedCameras(
+			const StereoPublishers & pubs, const std::string & name, double stamp,
+			bool zeroBaseline = false)
+	{
+		const cv::Mat left = stereoLeftImage(name, kRaw);
+		const cv::Mat right = stereoRightImage(name, kRaw);
+		ASSERT_FALSE(left.empty()) << "test/data/stereo/raw/left/" << name << ".jpg missing";
+		ASSERT_FALSE(right.empty()) << "test/data/stereo/raw/right/" << name << ".jpg missing";
+
+		// The images keep their frame: base_link -> camera is what localTransform needs,
+		// and it is a different lookup from the one between the two cameras.
+		sensor_msgs::msg::CameraInfo leftInfo = stereoLeftInfo("", stamp, kRaw);
+		sensor_msgs::msg::CameraInfo rightInfo = stereoRightInfo("", stamp, kRaw);
+		if(zeroBaseline)
+		{
+			rightInfo.p[3] = 0.0;
+		}
+
+		pubs.left->publish(makeImage("camera", stamp, left, "bgr8"));
+		pubs.right->publish(makeImage("camera", stamp, right, "mono8"));
+		pubs.leftInfo->publish(leftInfo);
+		pubs.rightInfo->publish(rightInfo);
+	}
+
+	/**
+	 * @brief base_link -> camera_left -> camera_right, the rig an unrectified pair needs in TF.
+	 *
+	 * The second link is the rig's measured extrinsics rather than an assumed baseline:
+	 * ~12 cm along x, plus the few milliradians the two cameras are really off by. That is
+	 * the transform the node looks up to rectify the pair for itself.
+	 */
+	void publishSplitSensorTf()
+	{
+		staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		std::vector<geometry_msgs::msg::TransformStamped> transforms(2);
+		transforms[0].header.stamp = helper()->now();
+		transforms[0].header.frame_id = "base_link";
+		transforms[0].child_frame_id = "camera_left";
+		transforms[0].transform.rotation.w = 1.0;
+		transforms[1] = transforms[0];
+		transforms[1].header.frame_id = "camera_left";
+		transforms[1].child_frame_id = "camera_right";
+		transforms[1].transform = stereoRightInLeftFrame();
+		staticTf_->sendTransform(transforms);
+	}
+
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+};
+
+/// The vendored calibration has to survive the trip through CameraInfo, or nothing below means anything.
+TEST_F(StereoOdometryTest, the_test_calibration_describes_the_stereo_rig)
+{
+	const sensor_msgs::msg::CameraInfo left = stereoLeftInfo("camera", 1.0);
+	const sensor_msgs::msg::CameraInfo right = stereoRightInfo("camera", 1.0);
+
+	ASSERT_EQ(640u, left.width);
+	ASSERT_EQ(480u, left.height);
+	EXPECT_NEAR(487.6, left.k[0], 0.1);
+	EXPECT_DOUBLE_EQ(0.0, left.p[3]);
+
+	// P(0,3) = -fx * baseline: ~12 cm, the scale every stereo estimate below rests on.
+	const double baseline = -right.p[3] / right.p[0];
+	EXPECT_NEAR(0.1197, baseline, 0.001);
+
+	// Rectified: nothing left to undistort, and no rotation left to apply.
+	EXPECT_DOUBLE_EQ(0.0, left.d[0]);
+	EXPECT_DOUBLE_EQ(1.0, left.r[0]);
+}
+
+/**
+ * The raw set is a different calibration of the same rig, and must not be confused with
+ * the rectified one: it carries the lens's distortion and the rotation into the rectified
+ * frame, which is what Rtabmap/ImagesAlreadyRectified:=false needs to do the rectification
+ * the pipeline has not done.
+ */
+TEST_F(StereoOdometryTest, the_raw_calibration_carries_distortion_and_rectification)
+{
+	const sensor_msgs::msg::CameraInfo left = stereoLeftInfo("camera", 1.0, kRaw);
+	const sensor_msgs::msg::CameraInfo right = stereoRightInfo("camera", 1.0, kRaw);
+
+	ASSERT_EQ(640u, left.width);
+	ASSERT_EQ(480u, left.height);
+	EXPECT_LT(left.d[0], -0.1) << "a raw pair needs real distortion coefficients";
+	EXPECT_NE(1.0, left.r[0]) << "a raw pair needs the rotation into the rectified frame";
+
+	// The same physical rig, so the same ~12 cm baseline as the rectified calibration.
+	EXPECT_NEAR(0.1197, -right.p[3] / right.p[0], 0.001);
+}
+
+/**
+ * The rig's extrinsics, which the unrectified path takes from TF rather than from P.
+ * Stereo calibration stores left-to-right; TF publishes right-in-left, so the sign of the
+ * baseline flips on the way through, and getting that backwards would put the right camera
+ * on the wrong side of the left one.
+ */
+TEST_F(StereoOdometryTest, the_stereo_pose_puts_the_right_camera_beside_the_left_one)
+{
+	const geometry_msgs::msg::Transform transform = stereoRightInLeftFrame();
+
+	// In the optical frame x points right, so the right camera sits at +baseline.
+	EXPECT_NEAR(0.1194, transform.translation.x, 0.001);
+	EXPECT_NEAR(0.0, transform.translation.y, 0.01);
+	EXPECT_NEAR(0.0, transform.translation.z, 0.01);
+	// The two cameras are nearly parallel: a few milliradians, not a few degrees.
+	EXPECT_NEAR(1.0, std::fabs(transform.rotation.w), 0.001);
+}
+
+/// By default the node takes the four raw stereo topics.
+TEST_F(StereoOdometryTest, subscribes_to_the_raw_stereo_topics_by_default)
+{
+	publishSensorTf();
+	makeNode();
+
+	StereoPublishers pubs = makeStereoPublishers();
+
+	EXPECT_TRUE(waitForSubscriber(pubs.left));
+	EXPECT_TRUE(waitForSubscriber(pubs.right));
+	EXPECT_TRUE(waitForSubscriber(pubs.leftInfo));
+	EXPECT_TRUE(waitForSubscriber(pubs.rightInfo));
+}
+
+/// A synchronized set of the four topics produces one odometry message, at the origin.
+TEST_F(StereoOdometryTest, publishes_odom_for_a_synchronized_stereo_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	// Identical stamps, which is what this node's exact-by-default policy requires.
+	publishStereoFrame(pubs, kFirstFrame, 1.0);
+
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_EQ("odom", odom->back().header.frame_id);
+	EXPECT_EQ("base_link", odom->back().child_frame_id);
+	// The first frame has nothing to register against: it defines the origin.
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9);
+}
+
+/**
+ * The point of feeding real pairs: a second frame is registered against the first and a
+ * motion comes out. The bounds are loose on purpose -- the claim is "a plausible,
+ * non-degenerate transform from real correspondences", not a specific value, which
+ * depends on the odometry strategy RTAB-Map was built with.
+ */
+TEST_F(StereoOdometryTest, recovers_motion_between_two_real_stereo_pairs)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode();
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	publishStereoFrame(pubs, kFirstFrame, 1.0);
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	publishStereoFrame(pubs, kSecondFrame, 1.1);
+	// Both collectors: odom and odom_info are published separately, and the assertions
+	// below compare the second of each.
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking between frames "
+			<< kFirstFrame << " and " << kSecondFrame;
+	EXPECT_GT(info->back().inliers, 20)
+			<< "too few inliers (matches=" << info->back().matches << ")";
+	EXPECT_LE(info->back().inliers, info->back().matches);
+
+	// 0.1710 to 0.1740 m and 0.1312 to 0.1315 rad over ten runs, on this build.
+	EXPECT_NEAR(0.172, translationNorm(second), 0.035)
+			<< "the pair is ~17 cm apart; this estimate is not that";
+	EXPECT_NEAR(0.131, rotationAngle(second), 0.030)
+			<< "the pair turns ~0.13 rad; this estimate is not that";
+}
+
+/**
+ * Unlike rgbd_odometry, this node requires identical stamps by default, because a stereo
+ * pair is normally hardware-triggered. See "Synchronization" in doc/stereo_odometry.md.
+ */
+TEST_F(StereoOdometryTest, approx_sync_is_off_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<rtabmap_odom::StereoOdometry> node = makeNode();
+
+	EXPECT_FALSE(node->get_parameter("approx_sync").as_bool());
+}
+
+/// With the exact policy, stamps that differ never pair and nothing is published at all.
+TEST_F(StereoOdometryTest, publishes_nothing_when_stamps_differ_under_exact_sync)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("approx_sync", false)});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	publishStereoFrame(pubs, kFirstFrame, 1.0, /*rightOffset=*/0.001);   // a millisecond apart
+	spinFor(std::chrono::milliseconds(1500));
+
+	EXPECT_TRUE(odom->empty())
+			<< "the exact policy must not pair frames whose stamps differ";
+}
+
+/// Approximate matching pairs them anyway, which is the fix when a rig is not triggered.
+TEST_F(StereoOdometryTest, approx_sync_pairs_frames_whose_stamps_differ)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("approx_sync", true)});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	// Two frames: the approximate policy needs a following message before it can settle
+	// on the best pairing for the first one.
+	publishStereoFrame(pubs, kFirstFrame, 1.0, /*rightOffset=*/0.001);
+	spinFor(std::chrono::milliseconds(100));
+	publishStereoFrame(pubs, kSecondFrame, 1.1, /*rightOffset=*/0.001);
+
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+}
+
+/**
+ * keep_color decides what reaches RTAB-Map from a color left image: the matcher only ever
+ * works in grayscale, so the color is dropped by default and kept only when asked for,
+ * which is what a downstream consumer of odom_rgbd_image or odom_sensor_data needs.
+ */
+TEST_F(StereoOdometryTest, keeps_the_left_image_in_color_when_asked)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	makeNode({rclcpp::Parameter("keep_color", true)});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+	// The node republishes the frame only when something is listening for it.
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	publishStereoFrame(pubs, kFirstFrame, 1.0);
+
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("bgr8", frames->back().rgb.encoding);
+	// The right image is the matcher's other input and is grayscale either way.
+	EXPECT_EQ("mono8", frames->back().depth.encoding);
+}
+
+/// Off by default: RTAB-Map converts the pair to grayscale on the way in.
+TEST_F(StereoOdometryTest, converts_the_left_image_to_grayscale_by_default)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
+			collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
+	std::shared_ptr<rtabmap_odom::StereoOdometry> node = makeNode();
+	EXPECT_FALSE(node->get_parameter("keep_color").as_bool());
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+	ASSERT_TRUE(waitForPublisher(frames->subscription));
+
+	// The same color pair as above: what differs is only what the node does with it.
+	publishStereoFrame(pubs, kFirstFrame, 1.0);
+
+	ASSERT_TRUE(spinUntil([&]() { return !frames->empty(); }));
+	EXPECT_EQ("mono8", frames->back().rgb.encoding);
+}
+
+/**
+ * The unrectified path the documentation offers as the alternative to stereo_image_proc:
+ * Rtabmap/ImagesAlreadyRectified:=false, raw images, and the transform between the two
+ * cameras taken from TF. See "The images are normally rectified" in doc/stereo_odometry.md.
+ */
+TEST_F(StereoOdometryTest, rectifies_a_raw_pair_itself_when_told_the_images_are_not_rectified)
+{
+	publishSplitSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	publishRawStereoFrameInSplitFrames(pubs, kFirstRawFrame, 1.0);
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_EQ("odom", odom->back().header.frame_id);
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin";
+
+	publishRawStereoFrameInSplitFrames(pubs, kSecondRawFrame, 1.25);
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	// The rectification RTAB-Map did for itself has to be good enough to register the two
+	// frames, with the scale taken from TF. A quarter second of walking forward, which is
+	// 0.2329 to 0.2351 m and 0.0298 to 0.0303 rad over ten runs on this build.
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking on a pair it rectified itself";
+	EXPECT_NEAR(0.234, translationNorm(second), 0.047)
+			<< "a quarter second of walking is ~0.23 m; this estimate is not that";
+	EXPECT_NEAR(0.030, rotationAngle(second), 0.015)
+			<< "the pair turns ~0.03 rad; this estimate is not that";
+}
+
+/**
+ * Same parameter, but both camera_info messages name the same frame: TF then answers with
+ * the identity, which is no baseline at all. The node refuses the frame rather than
+ * estimating a trajectory at an arbitrary scale.
+ */
+TEST_F(StereoOdometryTest, publishes_nothing_for_a_raw_pair_when_the_cameras_share_a_frame)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	// Both images on "camera", which is right for a rectified pair and wrong for this one.
+	publishStereoFrame(pubs, kFirstRawFrame, 1.0, /*rightOffset=*/0.0, kRaw);
+	spinFor(std::chrono::milliseconds(1500));
+
+	EXPECT_TRUE(odom->empty())
+			<< "an identity transform between the cameras cannot give a baseline";
+}
+
+/**
+ * Unrectified images from a driver that leaves camera_info's frame_id empty: there is no
+ * TF query to make, so the node falls back to the baseline in the right camera's P rather
+ * than refusing the frame.
+ */
+TEST_F(StereoOdometryTest, uses_the_calibration_baseline_for_a_raw_pair_with_unnamed_cameras)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	publishRawStereoFrameWithUnnamedCameras(pubs, kFirstRawFrame, 1.0);
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin";
+
+	publishRawStereoFrameWithUnnamedCameras(pubs, kSecondRawFrame, 1.25);
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	// The same motion as above, held to the same tolerance: the baseline came from the file
+	// instead of TF, and the estimate has to come out the same size either way.
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking on a pair rectified from the calibration alone";
+	EXPECT_NEAR(0.234, translationNorm(second), 0.047)
+			<< "a quarter second of walking is ~0.23 m; this estimate is not that";
+	EXPECT_NEAR(0.030, rotationAngle(second), 0.015)
+			<< "the pair turns ~0.03 rad; this estimate is not that";
+}
+
+/// No frame_id and no baseline in P either: nothing left to derive the scale from.
+TEST_F(StereoOdometryTest, publishes_nothing_for_a_raw_pair_with_neither_frame_nor_baseline)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")});
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	publishRawStereoFrameWithUnnamedCameras(pubs, kFirstRawFrame, 1.0, /*zeroBaseline=*/true);
+	spinFor(std::chrono::milliseconds(1500));
+
+	EXPECT_TRUE(odom->empty())
+			<< "with no TF and no Tx there is no baseline, so no estimate to publish";
+}
+
+/**
+ * The silent middle case doc/stereo_odometry.md warns about: an unrectified pair while the
+ * node is left believing it is rectified. Nothing fails -- odometry is published, and its
+ * covariance says the node stands behind it -- which is exactly why the warning is there
+ * and why this test pins the behaviour rather than an accuracy bound.
+ */
+TEST_F(StereoOdometryTest, accepts_a_raw_pair_silently_when_it_believes_it_is_rectified)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode();
+
+	StereoPublishers pubs = makeStereoPublishers();
+	ASSERT_TRUE(waitForStereoSubscribers(pubs));
+
+	publishStereoFrame(pubs, kFirstRawFrame, 1.0, /*rightOffset=*/0.0, kRaw);
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	publishStereoFrame(pubs, kSecondRawFrame, 1.25, /*rightOffset=*/0.0, kRaw);
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2; }));
+
+	EXPECT_FALSE(isLost(odom->back()))
+			<< "the node has no way to notice the images are distorted";
+}
+
+/// subscribe_rgbd swaps the four topics for one pre-synchronized message from stereo_sync.
+TEST_F(StereoOdometryTest, subscribe_rgbd_takes_a_single_rgbd_image_topic)
+{
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+
+	EXPECT_TRUE(waitForSubscriber(pub));
+}
+
+/**
+ * The same pairs through the other entry point. Both callbacks hand the same four vectors
+ * to commonCallback, so what this covers is the unpacking on the way in: left out of rgb,
+ * right out of depth, and the baseline out of depth_camera_info -- swap the two infos and
+ * the scale of the whole trajectory goes with them.
+ */
+TEST_F(StereoOdometryTest, recovers_motion_from_a_single_rgbd_image_topic)
+{
+	publishSensorTf();
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	pub->publish(makeStereoRGBDImage(kFirstFrame, 1.0));
+	ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); }));
+	EXPECT_NEAR(0.0, translationNorm(odom->back()), 1e-9) << "the first frame is the origin";
+
+	pub->publish(makeStereoRGBDImage(kSecondFrame, 1.1));
+	ASSERT_TRUE(spinUntil([&]() { return odom->size() >= 2 && info->size() >= 2; }));
+
+	const nav_msgs::msg::Odometry & second = odom->back();
+	ASSERT_FALSE(isLost(second)) << "lost tracking between frames "
+			<< kFirstFrame << " and " << kSecondFrame;
+	EXPECT_GT(info->back().inliers, 20)
+			<< "too few inliers (matches=" << info->back().matches << ")";
+
+	// The same motion the four-topic test sees, held to the same tolerance: the two paths
+	// differ only in packaging, so an estimate that differs is a packing bug.
+	EXPECT_NEAR(0.172, translationNorm(second), 0.035)
+			<< "the pair is ~17 cm apart; this estimate is not that";
+	EXPECT_NEAR(0.131, rotationAngle(second), 0.030)
+			<< "the pair turns ~0.13 rad; this estimate is not that";
+}
+
+/**
+ * @brief Two to six cameras, each on its own numbered topic.
+ *
+ * Above six the node has no synchronizer for it and says to use rgbd_cameras:=0 with the
+ * rgbd_images topic instead, so six is where this stops. Only the subscriptions are
+ * checked: one numbered topic per camera, none left behind.
+ */
+class StereoOdometryCamerasTest :
+		public StereoOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(StereoOdometryCamerasTest, subscribes_to_one_numbered_topic_per_camera)
+{
+	const int cameras = GetParam();
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras)});
+
+	// All of them first, so they are discovered together rather than one wait after another.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+
+	for(int i=0; i<cameras; ++i)
+	{
+		EXPECT_TRUE(waitForSubscriber(publishers[i]))
+				<< "rgbd_cameras:=" << cameras << " left rgbd_image" << i << " unsubscribed";
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		RgbdCameras,
+		StereoOdometryCamerasTest,
+		::testing::Range(2, 7),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return std::to_string(info.param) + "_cameras";
+		});
+
+/// rgbd_cameras:=0 takes any number of cameras in one RGBDImages message.
+TEST_F(StereoOdometryTest, rgbd_cameras_zero_takes_an_rgbd_images_topic)
+{
+	publishSensorTf();
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+
+	EXPECT_TRUE(waitForSubscriber(pub));
+}
+
+/**
+ * @brief Four stereo pairs on one rig, driven a metre through a world of points.
+ *
+ * The frames carry their features -- keypoints, 3D points, descriptors -- and no image at
+ * all, as a driver that does its own extraction publishes them. What differs from the
+ * RGB-D rig is the second calibration of each camera: the node has to read the pairs as
+ * stereo, and the features still belong to the left image of each one.
+ *
+ * Both estimation types the multi-camera case supports are run. Vis/EstimationType=0
+ * aligns the two sets of 3D points, which needs nothing extra; =1 solves a PnP across all
+ * four cameras at once, which RTAB-Map hands to OpenGV and cannot do without it.
+ */
+class StereoOdometryRigTest :
+		public StereoOdometryTest,
+		public ::testing::WithParamInterface<int>
+{
+};
+
+TEST_P(StereoOdometryRigTest, recovers_the_trajectory_of_a_rig_from_the_features_it_is_given)
+{
+	const int estimationType = GetParam();
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	// A metre forward, ten centimetres at a time.
+	const int frames = 11;
+	rtabmap_msgs::msg::RGBDImages lastFrame;
+	for(int i=0; i<frames; ++i)
+	{
+		lastFrame = cameraRigStereoFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(rig.cameras(), lastFrame.rgbd_images.size());
+		pub->publish(lastFrame);
+		// Both collectors: odom and odom_info are published separately, and the feature
+		// count asserted below is read from the odom_info of this same frame.
+		ASSERT_TRUE(spinUntil([&]() {
+					return odom->size() >= size_t(i+1) && info->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking on a rig that sees the whole scene";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05)
+			<< "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+	EXPECT_NEAR(0.0, rotationAngle(last), 0.05) << "the rig never turned";
+
+	// The frame's own features, reassembled from the four cameras and used as they are.
+	// A couple can go missing on the way: RTAB-Map drops a feature whose descriptor lands
+	// on the same visual word as another one of the same frame, both being ambiguous then.
+	const int sent = int(cameraRigFeatureCount(lastFrame));
+	EXPECT_LE(info->back().features, sent);
+	EXPECT_GT(info->back().features, sent - 10)
+			<< "the node did not use the features the frame came with";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		EstimationTypes,
+		StereoOdometryRigTest,
+		::testing::Values(0, 1),
+		[](const ::testing::TestParamInfo<int> & info) {
+			return info.param == 0 ? std::string("3d_to_3d") : std::string("pnp_across_cameras");
+		});
+
+/**
+ * The control for the test above: the same frames with the features stripped off. What is
+ * left is four calibrations and nothing to see, which the node is right to process -- an
+ * empty scene is still a frame -- and right to report lost.
+ */
+TEST_F(StereoOdometryRigTest, the_same_frames_without_their_features_have_nothing_to_track)
+{
+	const CameraRig rig = makeCameraRig();
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 0)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	for(int i=0; i<2; ++i)
+	{
+		rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigStereoFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		for(size_t c=0; c<frame.rgbd_images.size(); ++c)
+		{
+			frame.rgbd_images[c].key_points.clear();
+			frame.rgbd_images[c].points.clear();
+			frame.rgbd_images[c].descriptors.clear();
+		}
+		pub->publish(frame);
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return info->size() >= 2; }));
+	EXPECT_EQ(0, info->back().features) << "features appeared from a frame that has none";
+	EXPECT_TRUE(isLost(odom->back()));
+}
+
+/**
+ * @brief The same rig on the numbered topics, one to six stereo pairs.
+ *
+ * `rgbd_cameras:=N` subscribes to N topics and synchronizes them with a callback of its
+ * own per N, six of them in all. The test above drives the `rgbd_cameras:=0` one; these
+ * drive the rest, by publishing each camera of the rig on its own topic and asking for
+ * the same metre back.
+ *
+ * Each of them runs both estimation types, except that a multi-camera PnP needs OpenGV
+ * and is skipped when RTAB-Map was built without it. A single camera does not, so that
+ * one is run either way.
+ */
+class StereoOdometryRigCamerasTest :
+		public StereoOdometryTest,
+		public ::testing::WithParamInterface<std::tuple<int, bool, int>>
+{
+};
+
+TEST_P(StereoOdometryRigCamerasTest, recovers_the_trajectory_from_the_numbered_topics)
+{
+	const int cameras = std::get<0>(GetParam());
+	const bool approxSync = std::get<1>(GetParam());
+	const int estimationType = std::get<2>(GetParam());
+#ifndef RTABMAP_OPENGV
+	if(estimationType == 1 && cameras > 1)
+	{
+		GTEST_SKIP() << "a multi-camera PnP is solved by OpenGV, which RTAB-Map was built without";
+	}
+#endif
+
+	const CameraRig rig = makeCameraRig(cameras);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", cameras),
+	          rclcpp::Parameter("approx_sync", approxSync),
+	          rclcpp::Parameter("Vis/EstimationType", std::to_string(estimationType))});
+
+	// One camera listens on rgbd_image, more than one on rgbd_image0..N-1.
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<cameras; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				cameras == 1 ? "rgbd_image" : "rgbd_image" + std::to_string(i), 10));
+	}
+	for(int i=0; i<cameras; ++i)
+	{
+		ASSERT_TRUE(waitForSubscriber(publishers[i])) << "camera " << i << " has no subscriber";
+	}
+
+	const int frames = 11;
+	for(int i=0; i<frames; ++i)
+	{
+		const rtabmap_msgs::msg::RGBDImages frame =
+				cameraRigStereoFrame(rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i);
+		ASSERT_EQ(size_t(cameras), frame.rgbd_images.size());
+		for(int c=0; c<cameras; ++c)
+		{
+			publishers[c]->publish(frame.rgbd_images[c]);
+		}
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	const nav_msgs::msg::Odometry & last = odom->back();
+	ASSERT_FALSE(isLost(last)) << "lost tracking with " << cameras << " camera(s)";
+	EXPECT_NEAR(1.0, last.pose.pose.position.x, 0.05) << "the rig travelled a metre along x";
+	EXPECT_NEAR(0.0, last.pose.pose.position.y, 0.05);
+	EXPECT_NEAR(0.0, last.pose.pose.position.z, 0.05);
+
+	// A reset tears the synchronizer down and builds it again -- one per camera count,
+	// and a different one for each of the two sync policies. Frames have to keep arriving
+	// through the new one.
+	ASSERT_TRUE(callEmptyService("reset_odom"));
+	const size_t beforeReset = odom->size();
+	const rtabmap_msgs::msg::RGBDImages frame =
+			cameraRigStereoFrame(rig, rtabmap::Transform(1.1f, 0, 0, 0, 0, 0), 2.1);
+	for(int c=0; c<cameras; ++c)
+	{
+		publishers[c]->publish(frame.rgbd_images[c]);
+	}
+	EXPECT_TRUE(spinUntil([&]() { return odom->size() > beforeReset; }))
+			<< "nothing came back after the reset rebuilt the synchronizer";
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		StereoCameras,
+		StereoOdometryRigCamerasTest,
+		::testing::Combine(::testing::Range(1, 7), ::testing::Bool(), ::testing::Values(0, 1)),
+		[](const ::testing::TestParamInfo<std::tuple<int, bool, int>> & info) {
+			const int cameras = std::get<0>(info.param);
+			return std::to_string(cameras) + (cameras == 1 ? "_camera_" : "_cameras_") +
+					(std::get<1>(info.param) ? "approx_sync_" : "exact_sync_") +
+					(std::get<2>(info.param) == 0 ? "3d_to_3d" : "pnp");
+		});
+
+/**
+ * A frame whose cameras disagree about carrying images is refused rather than
+ * half-processed: the images are stitched side by side and the keypoints indexed into
+ * that strip, so one camera short of images would put everything after it in the wrong
+ * place.
+ */
+TEST_F(StereoOdometryTest, refuses_a_frame_whose_cameras_disagree_about_carrying_images)
+{
+	const CameraRig rig = makeCameraRig(2);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 2)});
+
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<2; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+	ASSERT_TRUE(waitForSubscriber(publishers[0]));
+	ASSERT_TRUE(waitForSubscriber(publishers[1]));
+
+	rtabmap_msgs::msg::RGBDImages frame =
+			cameraRigStereoFrame(rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true);
+	// The second camera sends its features and its calibration, but no images.
+	frame.rgbd_images[1].rgb = sensor_msgs::msg::Image();
+	frame.rgbd_images[1].depth = sensor_msgs::msg::Image();
+	publishers[0]->publish(frame.rgbd_images[0]);
+	publishers[1]->publish(frame.rgbd_images[1]);
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(odom->empty()) << "a frame with images on one camera only was processed anyway";
+}
+
+/**
+ * Features whose three parts disagree are dropped rather than used out of step: a
+ * keypoint read against the wrong descriptor, or given another keypoint's 3D point,
+ * would register the frame confidently and wrongly.
+ */
+TEST_F(StereoOdometryTest, ignores_features_whose_counts_disagree)
+{
+	const CameraRig rig = makeCameraRig(1);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<rtabmap_msgs::msg::OdomInfo>> info =
+			collect<rtabmap_msgs::msg::OdomInfo>("odom_info");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 1)});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(info->subscription));
+
+	rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0);
+	ASSERT_GT(frame.rgbd_images[0].key_points.size(), 1u);
+	// One keypoint fewer than there are 3D points and descriptor rows.
+	frame.rgbd_images[0].key_points.pop_back();
+	pub->publish(frame.rgbd_images[0]);
+	ASSERT_TRUE(spinUntil([&]() { return !info->empty(); }));
+
+	EXPECT_EQ(0, info->back().features)
+			<< "features that do not line up with each other were used anyway";
+}
+
+/**
+ * @brief The calibration paths around `Rtabmap/ImagesAlreadyRectified`, on synthetic pairs.
+ *
+ * A stereo pair is only usable if the node can work out how far apart the two cameras
+ * are. It has two ways -- `P(0,3)` in the right `camera_info`, or the transform between
+ * the two camera frames in TF -- and refuses the frame rather than guessing when neither
+ * answers. These drive each of those outcomes.
+ */
+class StereoOdometryCalibrationTest : public StereoOdometryTest
+{
+protected:
+	/// A one-camera rig and its publisher, with the node already listening.
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr start(
+			const CameraRig & rig, std::vector<rclcpp::Parameter> params = {})
+	{
+		publishRigTf(rig);
+		params.push_back(rclcpp::Parameter("subscribe_rgbd", true));
+		params.push_back(rclcpp::Parameter("rgbd_cameras", 1));
+		makeNode(params);
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+				helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+		return pub;
+	}
+};
+
+/// No `P(0,3)` and nothing in TF to make up for it: there is no scale, so no pose.
+TEST_F(StereoOdometryCalibrationTest, refuses_a_pair_whose_calibration_has_no_baseline)
+{
+	const CameraRig rig = makeCameraRig(1);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = start(rig);
+
+	// Both calibrations describe the same camera, so TF between them is the identity.
+	rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+			rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, /*baseline=*/0.0, /*withImages=*/true);
+	pub->publish(frame.rgbd_images[0]);
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(odom->empty()) << "a pair with no baseline was registered anyway";
+}
+
+/// The D400 case the node warns about: no `P(0,3)`, but the two frames are in TF.
+TEST_F(StereoOdometryCalibrationTest, takes_the_baseline_from_tf_when_the_calibration_has_none)
+{
+	const CameraRig rig = makeCameraRig(1);
+	const double baseline = 0.12;
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+
+	// The rig's TF, plus the right camera beside the left one.
+	staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+	std::vector<geometry_msgs::msg::TransformStamped> transforms =
+			cameraRigTransforms(rig, helper()->now());
+	geometry_msgs::msg::TransformStamped right;
+	right.header.stamp = helper()->now();
+	right.header.frame_id = rig.frameIds[0];
+	right.child_frame_id = rig.frameIds[0] + "_right";
+	right.transform.translation.x = baseline;
+	right.transform.rotation.w = 1.0;
+	transforms.push_back(right);
+	staticTf_->sendTransform(transforms);
+
+	// makeNode() waits for the node's TF listener to pick the static transforms up.
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true), rclcpp::Parameter("rgbd_cameras", 1)});
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	// A metre forward, with the baseline reachable only through TF.
+	for(int i=0; i<11; ++i)
+	{
+		rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+				rig, rtabmap::Transform(0.1f*i, 0, 0, 0, 0, 0), 1.0 + 0.1*i,
+				/*baseline=*/0.0, /*withImages=*/true);
+		frame.rgbd_images[0].depth_camera_info.header.frame_id = rig.frameIds[0] + "_right";
+		pub->publish(frame.rgbd_images[0]);
+		ASSERT_TRUE(spinUntil([&]() { return odom->size() >= size_t(i+1); }))
+				<< "nothing came back for frame " << i;
+	}
+
+	EXPECT_FALSE(isLost(odom->back())) << "the baseline from TF did not make the pair usable";
+	EXPECT_NEAR(1.0, odom->back().pose.pose.position.x, 0.05)
+			<< "the rig travelled a metre along x";
+}
+
+/// Unrectified images the node is asked to rectify, with no transform between the cameras.
+TEST_F(StereoOdometryCalibrationTest, refuses_unrectified_images_when_the_cameras_are_not_in_tf)
+{
+	const CameraRig rig = makeCameraRig(1);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			start(rig, {rclcpp::Parameter("Rtabmap/ImagesAlreadyRectified", "false")});
+
+	rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+			rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true);
+	// A right camera whose frame nothing in TF knows about.
+	frame.rgbd_images[0].depth_camera_info.header.frame_id = "right_camera_nobody_publishes";
+	pub->publish(frame.rgbd_images[0]);
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(odom->empty())
+			<< "rectification was attempted without knowing where the two cameras are";
+}
+
+/// An encoding the node cannot read is refused rather than reinterpreted.
+TEST_F(StereoOdometryCalibrationTest, refuses_an_image_encoding_it_cannot_use)
+{
+	const CameraRig rig = makeCameraRig(1);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = start(rig);
+
+	rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+			rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true);
+	frame.rgbd_images[0].rgb = makeImage(rig.frameIds[0], 1.0,
+			cv::Mat::zeros(rig.height, rig.width, CV_32FC1), "32FC1");
+	pub->publish(frame.rgbd_images[0]);
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(odom->empty()) << "a 32FC1 left image was taken as a stereo frame";
+}
+
+/// The images of every camera are stitched into one strip, so they have to share a type.
+TEST_F(StereoOdometryTest, refuses_cameras_whose_images_are_of_different_types)
+{
+	const CameraRig rig = makeCameraRig(2);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	// keep_color leaves a color image in color, so the two cameras below stay different;
+	// converted to grayscale they would both end up 8UC1 and agree.
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 2),
+	          rclcpp::Parameter("keep_color", true)});
+
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<2; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+	ASSERT_TRUE(waitForSubscriber(publishers[0]));
+	ASSERT_TRUE(waitForSubscriber(publishers[1]));
+
+	rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+			rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, 0.12, /*withImages=*/true);
+	// The rig sends mono8; this camera sends color.
+	frame.rgbd_images[1].rgb = makeImage(rig.frameIds[1], 1.0,
+			cv::Mat::zeros(rig.height, rig.width, CV_8UC3), "bgr8");
+	publishers[0]->publish(frame.rgbd_images[0]);
+	publishers[1]->publish(frame.rgbd_images[1]);
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(odom->empty()) << "images of two different types were stitched together";
+}
+
+/**
+ * Cameras of one rig are meant to fire together. When their stamps are far apart the node
+ * says so once and carries on -- the frame is still registered, since refusing it would
+ * be worse than registering a slightly stale one.
+ */
+TEST_F(StereoOdometryTest, warns_but_carries_on_when_the_cameras_are_far_apart_in_time)
+{
+	const CameraRig rig = makeCameraRig(2);
+	publishRigTf(rig);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	// Exact matching would never pair frames this far apart, so there would be nothing
+	// to warn about.
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+	          rclcpp::Parameter("rgbd_cameras", 2),
+	          rclcpp::Parameter("approx_sync", true)});
+
+	std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> publishers;
+	for(int i=0; i<2; ++i)
+	{
+		publishers.push_back(helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+				"rgbd_image" + std::to_string(i), 10));
+	}
+	ASSERT_TRUE(waitForSubscriber(publishers[0]));
+	ASSERT_TRUE(waitForSubscriber(publishers[1]));
+
+	// Each camera 60 ms behind the other, against the 1/60 s the node considers high.
+	// Several pairs: the approximate policy needs more than one message per topic before
+	// it commits to a pairing.
+	for(int i=0; i<4; ++i)
+	{
+		const double stamp = 1.0 + 0.1*i;
+		const rtabmap::Transform pose(0.1f*i, 0, 0, 0, 0, 0);
+		publishers[0]->publish(
+				cameraRigStereoFrame(rig, pose, stamp, 0.12, true).rgbd_images[0]);
+		publishers[1]->publish(
+				cameraRigStereoFrame(rig, pose, stamp + 0.06, 0.12, true).rgbd_images[1]);
+		spinFor(std::chrono::milliseconds(100));
+	}
+
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }))
+			<< "a pair whose cameras disagree about the time was dropped, not warned about";
+}
+
+/// A baseline that cannot be real is called out, and the frame is registered regardless.
+TEST_F(StereoOdometryCalibrationTest, warns_about_an_implausible_baseline)
+{
+	const CameraRig rig = makeCameraRig(1);
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = start(rig);
+
+	// 20 m between the two cameras of one rig: possible to write down, not to build.
+	const rtabmap_msgs::msg::RGBDImages frame = cameraRigStereoFrame(
+			rig, rtabmap::Transform(0, 0, 0, 0, 0, 0), 1.0, /*baseline=*/20.0, /*withImages=*/true);
+	pub->publish(frame.rgbd_images[0]);
+
+	EXPECT_TRUE(spinUntil([&]() { return !odom->empty(); }))
+			<< "the frame was dropped rather than registered with a warning";
+}
+
+}  // namespace
+}  // namespace rtabmap_odom_test
diff --git a/rtabmap_python/README.md b/rtabmap_python/README.md
new file mode 100644
index 00000000..b29ef440
--- /dev/null
+++ b/rtabmap_python/README.md
@@ -0,0 +1,45 @@
+# rtabmap_python
+
+Python helpers for reading and writing the binary formats [RTAB-Map](https://github.com/introlab/rtabmap) uses.
+
+RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS types. Several `rtabmap_msgs` fields — and every blob in an `.db` database — carry a matrix in RTAB-Map's own compressed encoding rather than as a `sensor_msgs/Image` or an array. This package is the Python side of that encoding, for scripts that read those fields without going through the C++ library.
+
+There are no nodes here. It is an `ament_python` package that installs one importable module.
+
+## Contents
+
+- [Module](#module)
+- [Things worth knowing](#things-worth-knowing)
+- [License](#license)
+
+## Module
+
+`rtabmap_python.cv_compression` — a single-channel `cv::Mat` to and from bytes.
+
+| Function | Description |
+|---|---|
+| `compress(data)` | 1-D or 2-D numpy array → `bytearray`. |
+| `uncompress(data)` | those bytes → 2-D numpy array. |
+
+```python
+import numpy as np
+from rtabmap_python.cv_compression import compress, uncompress
+
+scan = np.zeros((360, 2), dtype=np.float32)
+blob = compress(scan)          # what the message field carries
+restored = uncompress(blob)    # (360, 2) float32
+```
+
+The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols and the element type as three `int32`. It matches `compressData()` and `uncompressData()` in RTAB-Map's `corelib/src/Compression.cpp` byte for byte, so either side can read what the other wrote. The [module docstring](rtabmap_python/cv_compression.py) has the exact layout, and the generated [Python API reference](https://docs.ros.org/en/jazzy/p/rtabmap_python/) renders it alongside the two functions.
+
+## Things worth knowing
+
+**The result is read-only.** `uncompress` views the decompressed buffer instead of copying it, so the array it returns has `writeable=False` and assigning into it raises. Call `.copy()` if you need to modify it.
+
+**Single-channel only.** The C++ encoder packs the channel count into the type code; the tables here cover the single-channel depths `CV_8U` through `CV_64F`. A multi-channel matrix written by the C++ side raises `KeyError` rather than decoding wrongly. So does an unsupported dtype on the way in — `int64` and `float16` have no encoding.
+
+**The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml
index 4a5d7e84..590cdc7c 100644
--- a/rtabmap_python/package.xml
+++ b/rtabmap_python/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_python</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's python package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -10,6 +10,8 @@
   <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
   <url type="repository">https://github.com/introlab/rtabmap_ros</url>
 
+  <depend>python3-numpy</depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
@@ -17,5 +19,6 @@
 
   <export>
     <build_type>ament_python</build_type>
+    <rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>
diff --git a/rtabmap_python/rosdoc2.yaml b/rtabmap_python/rosdoc2.yaml
new file mode 100644
index 00000000..ae1e28c9
--- /dev/null
+++ b/rtabmap_python/rosdoc2.yaml
@@ -0,0 +1,29 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_python
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_python --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_python package: there are no C/C++ headers to parse, and the
+    ## API reference comes from the module docstrings via sphinx-apidoc.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: true
+
+builders:
+    ## Sphinx renders the landing page and the autodoc pages sphinx-apidoc produces
+    ## from rtabmap_python/.
+    - sphinx: {
+        name: 'rtabmap_python',
+        output_dir: ''
+    }
diff --git a/rtabmap_python/rtabmap_python/cv_compression.py b/rtabmap_python/rtabmap_python/cv_compression.py
index 0b8e1604..82769d89 100644
--- a/rtabmap_python/rtabmap_python/cv_compression.py
+++ b/rtabmap_python/rtabmap_python/cv_compression.py
@@ -27,6 +27,35 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 
+"""
+Compress numpy arrays into RTAB-Map's ``cv::Mat`` wire format.
+
+RTAB-Map stores and transmits matrices -- images, laser scans, descriptors -- as a zlib
+payload followed by a 12-byte trailer recording the shape and the element type. Database
+blobs and the compressed fields of ``rtabmap_msgs`` messages both use it.
+
+The layout is::
+
+    [ zlib stream of the elements in C order ][ rows ][ cols ][ type ]
+                                               int32   int32   int32
+
+The three trailer fields are written with ``struct`` format ``'iii'`` -- native byte order
+and size, matching the C++ side's raw ``int`` writes. That makes the encoding
+**host-endian**, so a blob does not travel between machines of opposite endianness.
+
+``type`` is the OpenCV depth of the elements: 0 ``CV_8U``, 1 ``CV_8S``, 2 ``CV_16U``,
+3 ``CV_16S``, 4 ``CV_32S``, 5 ``CV_32F``, 6 ``CV_64F``.
+
+These two functions are the Python side of that format. They match ``compressData()`` and
+``uncompressData()`` in RTAB-Map's ``corelib/src/Compression.cpp`` byte for byte, so a
+matrix written by either side can be read by the other.
+
+Single-channel matrices only. The C++ encoder packs the channel count into the type code
+alongside the depth; the tables here cover the single-channel depths ``CV_8U`` through
+``CV_64F``, which is what the codes 0 to 6 mean.
+"""
+
+
 import struct
 import zlib
 
@@ -34,6 +63,20 @@ import numpy as np
 
 
 def compress(data):
+    """
+    Compress a 1-D or 2-D array into RTAB-Map's format.
+
+    :param data: a single-channel array whose dtype is one of ``uint8``, ``int8``,
+        ``uint16``, ``int16``, ``int32``, ``float32`` or ``float64``. A 1-D array of
+        length ``n`` is recorded as a 1-by-``n`` matrix, which is the shape
+        :func:`uncompress` gives back. Any memory layout is accepted; the bytes are
+        always written in C order.
+    :returns: a ``bytearray`` holding the zlib payload followed by the trailer described
+        in the module docstring.
+    :raises AssertionError: if ``data`` has more than two dimensions.
+    :raises KeyError: if its dtype is not one of the seven above -- ``int64`` and
+        ``float16`` have no encoding in this format.
+    """
     assert data.ndim == 1 or data.ndim == 2
 
     dim1 = 1
@@ -63,6 +106,16 @@ def compress(data):
 
 
 def uncompress(data):
+    """
+    Restore an array written by :func:`compress` or by RTAB-Map's C++ side.
+
+    :param data: a bytes-like object laid out as :func:`compress` returns.
+    :returns: a 2-D array of the recorded shape and dtype. It is 2-D even when
+        :func:`compress` was handed a 1-D array, and it is **read-only**: it views the
+        decompressed buffer instead of copying it, so call ``.copy()`` before writing.
+    :raises KeyError: if the trailer's type code is not a single-channel depth 0 to 6,
+        which is what a multi-channel matrix from the C++ side encodes to.
+    """
     cvtype_to_numpy_type = {
         0: 'uint8',
         1: 'int8',
diff --git a/rtabmap_python/test/test_cv_compression.py b/rtabmap_python/test/test_cv_compression.py
new file mode 100644
index 00000000..55d46c47
--- /dev/null
+++ b/rtabmap_python/test/test_cv_compression.py
@@ -0,0 +1,178 @@
+# Copyright 2025 matlabbe
+#
+# 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 matlabbe 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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
+
+
+"""Tests for :mod:`rtabmap_python.cv_compression`."""
+
+import struct
+import zlib
+
+import numpy as np
+import pytest
+
+from rtabmap_python.cv_compression import compress, uncompress
+
+
+# The single-channel depths the format encodes, with the codes RTAB-Map's
+# serializeMatType() gives them: CV_8U, CV_8S, CV_16U, CV_16S, CV_32S, CV_32F, CV_64F.
+SUPPORTED_TYPES = [
+    ('uint8', 0),
+    ('int8', 1),
+    ('uint16', 2),
+    ('int16', 3),
+    ('int32', 4),
+    ('float32', 5),
+    ('float64', 6),
+]
+
+# Three int32: rows, cols, type code.
+TRAILER_SIZE = 3 * 4
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_roundtrip_preserves_shape_dtype_and_values(dtype, code):
+    """Every supported depth survives a compress/uncompress cycle unchanged."""
+    data = np.arange(12, dtype=dtype).reshape(3, 4)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (3, 4)
+    assert result.dtype == np.dtype(dtype)
+    assert np.array_equal(result, data)
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_trailer_records_rows_cols_and_type_code(dtype, code):
+    """The last 12 bytes are rows, cols and the type code, as the C++ side writes them."""
+    data = np.zeros((3, 4), dtype=dtype)
+
+    trailer = bytes(compress(data)[-TRAILER_SIZE:])
+
+    assert trailer == struct.pack('iii', 3, 4, code)
+
+
+def test_payload_is_plain_zlib_of_the_c_order_bytes():
+    """Everything before the trailer is a zlib stream, so the C++ side can inflate it."""
+    data = np.arange(6, dtype=np.uint8)
+
+    payload = bytes(compress(data)[:-TRAILER_SIZE])
+
+    assert zlib.decompress(payload) == data.tobytes()
+
+
+def test_compress_returns_a_bytearray():
+    """The return type is a bytearray, which is what the message fields expect."""
+    assert isinstance(compress(np.zeros(4, dtype=np.uint8)), bytearray)
+
+
+def test_one_dimensional_input_comes_back_as_a_single_row():
+    """A 1-D array is recorded as 1-by-n, so the roundtrip is not shape-preserving."""
+    data = np.arange(5, dtype=np.float32)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (1, 5)
+    assert np.array_equal(result.ravel(), data)
+
+
+@pytest.mark.parametrize('shape', [(1, 5), (5, 1), (2, 3)])
+def test_two_dimensional_shapes_are_preserved_exactly(shape):
+    """Rows and cols are recorded separately, so no 2-D shape is transposed or flattened."""
+    data = np.arange(5 if 1 in shape else 6, dtype=np.uint8).reshape(shape)
+
+    assert uncompress(compress(data)).shape == shape
+
+
+def test_non_contiguous_input_roundtrips():
+    """A transposed view is written in C order, so it reads back as the same matrix."""
+    data = np.arange(12, dtype=np.int16).reshape(3, 4).T
+    assert not data.flags.c_contiguous
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (4, 3)
+    assert np.array_equal(result, data)
+
+
+def test_empty_array_roundtrips_as_an_empty_row():
+    """An empty array is not a special case; it comes back as a 1-by-0 matrix."""
+    result = uncompress(compress(np.array([], dtype=np.uint8)))
+
+    assert result.shape == (1, 0)
+    assert result.dtype == np.uint8
+
+
+def test_uncompressed_array_is_read_only():
+    """uncompress() views the decompressed buffer rather than copying it."""
+    result = uncompress(compress(np.arange(4, dtype=np.uint8)))
+
+    assert not result.flags.writeable
+    with pytest.raises(ValueError):
+        result[0, 0] = 1
+    # .copy() is the way out, as the docstring says.
+    assert result.copy().flags.writeable
+
+
+def test_accepts_bytes_as_well_as_bytearray():
+    """uncompress() reads whatever compress() produced, converted or not."""
+    data = np.arange(8, dtype=np.uint16).reshape(2, 4)
+
+    result = uncompress(bytes(compress(data)))
+
+    assert np.array_equal(result, data)
+
+
+def test_larger_matrix_roundtrips():
+    """A matrix big enough to actually exercise zlib, with non-trivial content."""
+    rng = np.random.default_rng(42)
+    data = rng.integers(0, 255, size=(120, 160), dtype=np.uint8)
+
+    assert np.array_equal(uncompress(compress(data)), data)
+
+
+def test_rejects_more_than_two_dimensions():
+    """The format has no encoding for a third dimension, so compress() refuses one."""
+    with pytest.raises(AssertionError):
+        compress(np.zeros((2, 2, 2), dtype=np.uint8))
+
+
+@pytest.mark.parametrize('dtype', ['int64', 'uint64', 'float16'])
+def test_rejects_unsupported_dtype(dtype):
+    """Depths outside CV_8U..CV_64F have no type code and raise rather than truncate."""
+    with pytest.raises(KeyError):
+        compress(np.zeros(4, dtype=dtype))
+
+
+def test_rejects_unknown_type_code():
+    """A trailer from a multi-channel C++ matrix encodes a code this table lacks."""
+    payload = bytes(compress(np.zeros((2, 2), dtype=np.uint8))[:-TRAILER_SIZE])
+    # What serializeMatType() returns for a 3-channel CV_8U matrix: depth + ((cn - 1) << 3).
+    forged = bytearray(payload) + struct.pack('iii', 2, 2, 0 + ((3 - 1) << 3))
+
+    with pytest.raises(KeyError):
+        uncompress(forged)
diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml
index 0af27a67..90a279ae 100644
--- a/rtabmap_ros/package.xml
+++ b/rtabmap_ros/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_ros</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>
     RTAB-Map Stack
   </description>
@@ -26,7 +26,7 @@
   <exec_depend>rtabmap_sync</exec_depend>
   <exec_depend>rtabmap_util</exec_depend>
   <exec_depend>rtabmap_viz</exec_depend>
-  <!-- <exec_depend>rtabmap_costmap_plugins</exec_depend> Not available on Lyrical -->
+  <exec_depend>rtabmap_costmap_plugins</exec_depend>
   
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml
index b5b67c82..8dc26861 100644
--- a/rtabmap_rviz_plugins/package.xml
+++ b/rtabmap_rviz_plugins/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_rviz_plugins</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's rviz plugins.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
index b66b6b31..a68ccfff 100644
--- a/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
+++ b/rtabmap_rviz_plugins/src/MapCloudDisplay.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_rviz_plugins/MapCloudDisplay.h"
 
 #include <QApplication>
@@ -325,7 +326,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
 
 					if(!cloud->empty())
 					{
-						pcl::toROSMsg(*cloud, *cloudMsg);
+						rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
 					}
 				}
 			}
@@ -352,7 +353,7 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
 
 				if(!cloud->empty())
 				{
-					pcl::toROSMsg(*cloud, *cloudMsg);
+					rtabmap_conversions::toPointCloud2Msg(*cloud, *cloudMsg);
 				}
 			}
 
diff --git a/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp b/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp
index 81d065c6..17463412 100644
--- a/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp
+++ b/rtabmap_rviz_plugins/src/MapGraphDisplay.cpp
@@ -26,6 +26,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap_rviz_plugins/MapGraphDisplay.h"
+
+// rviz's headers forward declare these, so the ones actually used here are included
+// directly rather than counting on what rviz happens to pull in.
+#include <OgreColourValue.h>
+#include <OgreManualObject.h>
+#include <OgreMatrix4.h>
+#include <OgreQuaternion.h>
+#include <OgreRenderOperation.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector.h>
+
 #include <rviz_common/display_context.hpp>
 #include "rviz_common/properties/color_property.hpp"
 #include "rviz_common/properties/float_property.hpp"
diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt
index 6107b23a..86e168a6 100644
--- a/rtabmap_slam/CMakeLists.txt
+++ b/rtabmap_slam/CMakeLists.txt
@@ -240,4 +240,43 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(rtabmap_conversions REQUIRED)
+
+  # Each test binary drives its own rtabmap node: a crash or a stuck executor in one
+  # cannot take the others down, and each starts from 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 -- odom,
+  # scan, info -- with the other packages' suites. On a shared domain they discover each
+  # other's publishers and assertions then see traffic the test never sent. rtabmap_util
+  # numbers from 30, rtabmap_sync from 50 and rtabmap_odom from 70; keep the ranges apart.
+  set(rtabmap_slam_test_domain_id 90)
+  macro(rtabmap_slam_add_node_test test_name)
+    ament_add_gtest(${test_name} test/${test_name}.cpp
+      ENV ROS_DOMAIN_ID=${rtabmap_slam_test_domain_id}
+      TIMEOUT 300)
+    math(EXPR rtabmap_slam_test_domain_id "${rtabmap_slam_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_slam_plugins)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries} rtabmap_conversions)
+      else()
+        target_link_libraries(${test_name} ${Libraries} rtabmap_conversions::rtabmap_conversions)
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_slam_add_node_test(test_core_wrapper_parameters)
+  rtabmap_slam_add_node_test(test_core_wrapper_mapping)
+  rtabmap_slam_add_node_test(test_core_wrapper_services)
+  rtabmap_slam_add_node_test(test_core_wrapper_planning)
+  rtabmap_slam_add_node_test(test_core_wrapper_inputs)
+endif()
+
 ament_package()
diff --git a/rtabmap_slam/README.md b/rtabmap_slam/README.md
new file mode 100644
index 00000000..2c6ee36d
--- /dev/null
+++ b/rtabmap_slam/README.md
@@ -0,0 +1,111 @@
+# rtabmap_slam
+
+The SLAM node of [RTAB-Map](https://github.com/introlab/rtabmap): it takes a pose from odometry and data from the sensors, builds a graph of where the robot has been, and corrects that graph whenever it recognizes a place it has seen before — in the current run or in a previous one, so a map can be extended over [several sessions](#the-database).
+
+## Contents
+
+- [Nodes](#nodes)
+- [Conventions](#conventions)
+  - [Frames and TF](#frames-and-tf)
+  - [The database](#the-database)
+  - [Update rate and dropped updates](#update-rate-and-dropped-updates)
+  - [Odometry, covariance and new maps](#odometry-covariance-and-new-maps)
+  - [Mapping and localization](#mapping-and-localization)
+- [License](#license)
+
+## Nodes
+
+| Node | Description |
+|---|---|
+| [rtabmap](doc/rtabmap.md) | Graph SLAM with appearance- and proximity-based loop closure detection, memory management, map assembly and planning on the graph. |
+
+```mermaid
+flowchart LR
+    SYNC["<div style='text-align:left'><b>synchronized</b><br>RGB-D camera(s)<br>Stereo camera(s)<br>2D LiDAR<br>3D LiDAR<br>Odometry</div>"]
+    ASYNC["<div style='text-align:left'><b>asynchronous</b><br>IMU<br>GPS<br>Landmarks (markers, tags, fiducials)</div>"]
+    RTAB(["<b>rtabmap</b>"])
+    GRAPH["Graph"]
+    INFO["Info"]
+    MAPS["<div style='text-align:left'><b>maps</b><br>2D occupancy grid<br>OctoMap<br>Elevation map<br>3D point cloud</div>"]
+    TF["TF map → odom"]
+    SYNC --> RTAB
+    ASYNC --> RTAB
+    RTAB --> GRAPH
+    RTAB --> INFO
+    RTAB --> MAPS
+    RTAB --> TF
+```
+
+## Conventions
+
+### Frames and TF
+
+The node publishes `map` → `odom`, the correction from the optimized graph; odometry publishes `odom` → `base_link`, and the sensors are attached to `base_link`. See [Frames and TF](doc/rtabmap.md#frames-and-tf) for the parameters.
+
+```mermaid
+flowchart TB
+    MAP(["map<br><i>map_frame_id</i>"])
+    ODOM(["odom<br><i>odometry frame</i>"])
+    BASE(["base_link<br><i>frame_id</i>"])
+    SENSOR(["camera, lidar, imu..."])
+    MAP -->|this node| ODOM
+    ODOM -->|odometry| BASE
+    BASE -->|static, URDF| SENSOR
+```
+
+### The database
+
+**The map is stored in `database_path`**, `~/.ros/rtabmap.db` by default (under `$ROS_HOME` if that is set). Set `delete_db_on_start`, or pass `-d` as an argument, to start from an empty one. Otherwise restarting on an existing database continues it: the map is reloaded, the next update starts a **new session**, and a loop closure between the new session and an old one merges the two. That is how a map is extended over several runs.
+
+**The database is saved on shutdown**. A node that is killed rather than shut down loses whatever had not been written yet.
+
+**The database also remembers the parameters it was built with**, and reopening it without setting them again brings them back. For example, a map made with ICP registration keeps using ICP, which is what makes the new session compatible with the old ones. Anything set explicitly still wins, and `delete_db_on_start` forgets them along with the map.
+
+### Update rate and dropped updates
+
+`Rtabmap/DetectionRate` is how many updates per second are processed. It is 1 Hz by default because SLAM does not need more: odometry carries the pose between nodes, and each node costs memory, loop closure detection and optimization time for as long as the map exists.
+
+> **Warning: `Rtabmap/DetectionRate` at `0` with sensor updates faster than about 2 Hz makes loop closure detection, graph optimization and map generation intractable fast.** Every update then becomes a node, and each node is compared against all the nodes in working memory, adds a pose to optimize and data to assemble into the maps, so the map, and the time each update takes, grow at the sensors' rate until the node cannot keep up. Keep a detection rate of 1 to 2 Hz, or bound working memory with `Rtabmap/TimeThr` or `Rtabmap/MemoryThr`.
+
+**A robot standing still does not grow the map.** An update that moved less than both `RGBD/LinearUpdate` and `RGBD/AngularUpdate` since the last node is still used to detect loop closures, and then dropped. Set both to `0` to add a node every time.
+
+**An update arriving while the previous one is still being processed is dropped**, not queued. SLAM time grows with the map, so a queue would only fall further behind; dropping keeps the node on the newest data. `info` shows how long each update took (`RtabmapROS/TimeTotal/ms`), and `/diagnostics` how many arrived versus how many were processed.
+
+### Odometry, covariance and new maps
+
+The link between two consecutive nodes is the odometry between them, weighted by its covariance — its inverse becomes the link's information matrix, so the optimizer knows how far to trust each one.
+
+Which covariance is used:
+
+- **The twist covariance, if it is set.** It is the uncertainty of the motion since the previous message, which is what a link between two nodes is.
+- **Otherwise half the pose covariance**, for odometry sources that only fill that one. This assumes it is the error of the motion since the previous message, as visual odometry often publishes it, not the unbounded uncertainty of the pose estimated by a filtered odometry.
+- **Otherwise `odom_tf_linear_variance` and `odom_tf_angular_variance`** (`0.001` by default), for a covariance that is zero, not finite, or exactly `1` — which is what several drivers publish to mean "not set". Many do publish zeros, and taking those at face value would make each link infinitely confident.
+
+Between two nodes, the largest covariance seen is kept, so updates dropped by the rate do not make the link look more certain than any of the motions that made it up.
+
+**An odometry reset starts a new map**, in the same database, rather than deforming the graph across a jump the robot never made:
+
+```
+Odometry is reset (identity pose or high variance detected). Increment map id!
+```
+
+A reset is an identity pose after a non-identity one, or `9999` on both the pose and the twist covariance diagonals — which is what the [odometry nodes publish](../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) when they lose track or restart. Odometry read from TF has no covariance, so only the identity pose counts there. The new map is merged back into the old one on the first loop closure between them.
+
+A consequence worth knowing: an odometry that returns to *exactly* the identity is taken for a reset. Real odometry never does, but a simulator or a test that drives back to the origin will.
+
+**`staleness_factor`** treats a long silence the same way. With `Rtabmap/DetectionRate` at 1 Hz and a factor of 2, an update more than 2 seconds after the previous one starts a new map. It is for odometry sources that go quiet instead of reporting a reset — a gap that long means the motion across it is not known, even if the next pose looks plausible.
+
+### Mapping and localization
+
+`Mem/IncrementalMemory` chooses between the two:
+
+- **`true`, mapping (SLAM)**, the default. Updates become nodes and the map grows. This is the mode to create a map of the environment.
+- **`false`, localization.** The map is loaded and not extended: each update is compared against it, localizes the robot if it matches, and is not added to the database. This is the mode to localize in a map already recorded, without increasing CPU and RAM usage, since the map is kept fixed.
+
+`set_mode_localization` and `set_mode_mapping` switch at runtime. Going back to mapping starts a new session, since nothing links where the robot is now to where it left the map — until a loop closure does.
+
+See [Localization](doc/rtabmap.md#localization) for where the robot starts on the map in localization mode.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
diff --git a/rtabmap_slam/doc/rtabmap.md b/rtabmap_slam/doc/rtabmap.md
new file mode 100644
index 00000000..9a04db83
--- /dev/null
+++ b/rtabmap_slam/doc/rtabmap.md
@@ -0,0 +1,623 @@
+# rtabmap
+
+Graph SLAM: each update that moved far enough becomes a node, linked to the previous one by odometry and to earlier ones by the loop closures found, and the graph is optimized every time a loop closure is added.
+
+Loop closures are found two ways:
+
+- **Appearance-based**: the node's visual words are compared against every node in working memory with an incremental bag-of-words (BoW) approach, which is independent of the odometry pose, and so of its drift.
+- **Proximity-based**: the node is registered against the nodes the graph says are nearby, based on the previous localization and the current odometry pose. This is what a lidar-only setup relies on.
+
+**Memory management**: working memory can be bounded, by update time (`Rtabmap/TimeThr`, in ms) or by node count (`Rtabmap/MemoryThr`): older nodes are then moved to the database and brought back when the robot returns near them, so the update time stays flat on large maps. Both are `0` by default, which leaves working memory unbounded: every node stays in it, and the update time grows with the map. Before enabling memory management, we strongly recommend reading [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050), which explains how it works and what it implies for mapping, localization and planning.
+
+## Contents
+
+- [Usage](#usage)
+- [Choosing the inputs](#choosing-the-inputs)
+  - [RGB-D camera (RGB-D visual SLAM)](#rgb-d-camera-rgb-d-visual-slam)
+  - [Stereo camera (stereo visual SLAM)](#stereo-camera-stereo-visual-slam)
+  - [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar)
+  - [Several RGB-D or stereo cameras](#several-rgb-d-or-stereo-cameras)
+  - [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar)
+  - [Lidar alone](#lidar-alone)
+  - [RGB camera with odometry](#rgb-camera-with-odometry)
+  - [RGB camera alone (appearance-based loop closure detection)](#rgb-camera-alone-appearance-based-loop-closure-detection)
+- [Odometry from TF](#odometry-from-tf)
+- [Automatic adjustments](#automatic-adjustments)
+- [Sensors not stamped together](#sensors-not-stamped-together)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Services](#services)
+- [Parameters](#parameters)
+  - [RTAB-Map's own parameters](#rtab-maps-own-parameters)
+- [Frames and TF](#frames-and-tf)
+- [Asynchronous inputs](#asynchronous-inputs)
+  - [Landmarks](#landmarks)
+  - [GPS and global pose](#gps-and-global-pose)
+  - [IMU](#imu)
+  - [User data and environment sensors](#user-data-and-environment-sensors)
+  - [Intermediate odometry](#intermediate-odometry)
+- [Deriving missing data](#deriving-missing-data)
+- [Localization](#localization)
+- [Planning](#planning)
+- [Diagnostics](#diagnostics)
+
+## Usage
+
+RGB-D camera, with odometry from [rgbd_odometry](../../rtabmap_odom/doc/rgbd_odometry.md) or any other source on `odom`, and the camera synchronized by [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md):
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_rgbd:=true \
+  -p frame_id:=base_link \
+  -r rgbd_image:=/camera/rgbd_image \
+  -r odom:=/odom
+```
+
+2D lidar, with odometry from TF:
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p subscribe_depth:=false -p subscribe_rgb:=false -p subscribe_scan:=true \
+  -p frame_id:=base_link \
+  -p odom_frame_id:=odom \
+  -p "Reg/Force3DoF:='true'" \
+  -r scan:=/scan
+```
+
+```python
+ComposableNode(
+    package='rtabmap_slam',
+    plugin='rtabmap_slam::CoreWrapper',
+    name='rtabmap',
+    parameters=[{'frame_id': 'base_link',
+                 'subscribe_depth': False,
+                 'subscribe_rgb': False,
+                 'subscribe_rgbd': True,
+                 'subscribe_scan': True,
+                 'approx_sync': True,
+                 'RGBD/LinearUpdate': '0.1',
+                 'Reg/Force3DoF': 'true'}],
+    remappings=[('rgbd_image', '/camera/rgbd_image'),
+                ('scan', '/scan'),
+                ('odom', '/odom')])
+```
+
+The executable runs the node on a **multi-threaded** executor, and the node relies on it. SLAM runs in its own callback group, so the synchronized inputs keep arriving while an update is being processed; the asynchronous inputs (GPS, IMU, landmarks, user data...) have groups of their own, so they are buffered rather than blocked. Loaded into a single-threaded component container, it still works, but everything is serialized behind the SLAM update.
+
+In a component container with intra-process communication enabled (`use_intra_process_comms`), the latched publishers (`mapGraph` and the [`MapsManager`](../../rtabmap_util/README.md#mapsmanager) maps, with `latch` on, the default) automatically opt out of it, since intra-process communication does not support transient local durability. The other publishers keep the container's setting, and with `latch` off, all of them do.
+
+[`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) wraps all of this, together with odometry and `rtabmap_viz`, and is where most setups should start.
+
+## Choosing the inputs
+
+The common setups are below. How the input topics are synchronized — `approx_sync`, `topic_queue_size`, `sync_queue_size` and the `qos*` parameters — is documented in [rtabmap_sync](../../rtabmap_sync/README.md#conventions).
+
+### RGB-D camera (RGB-D visual SLAM)
+
+```yaml
+subscribe_depth: true   # default
+subscribe_rgb: true   # default
+```
+
+This is the legacy default. The recommended way is instead to synchronize the camera topics together with [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), and subscribe to its `rgbd_image`, as in [RGB-D or stereo camera and lidar](#rgb-d-or-stereo-camera-and-lidar) without the lidar.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["rgb/image<br>depth/image<br>rgb/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### Stereo camera (stereo visual SLAM)
+
+```yaml
+subscribe_stereo: true
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+`approx_sync` defaults to `false` here: the left and right images, and the odometry, are expected with exactly the same stamp, as when the odometry comes from [stereo_odometry](../../rtabmap_odom/doc/stereo_odometry.md) on the same camera. The images are assumed to be already rectified; if they are not, set `Rtabmap/ImagesAlreadyRectified` to `false` to rectify them here, at rtabmap's update rate.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["left/image_rect<br>left/camera_info<br>right/image_rect<br>right/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### RGB-D or stereo camera and lidar
+
+```yaml
+subscribe_rgbd: true
+subscribe_scan: true   # for a 2D lidar
+#subscribe_scan_cloud: true   # for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+The camera comes as one `rgbd_image`, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) for an RGB-D camera or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) for a stereo camera. Loop closures are still detected visually; with `Reg/Strategy` set to `1`, they are then refined with the lidar (ICP). `RGBD/NeighborLinkRefining` set to `true` also refines, with that registration, the link between each new node and the previous one, which corrects the odometry.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    SYNC["rgbd_sync<br>or stereo_sync"]
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    SYNC -->|rgbd_image| R
+    SCAN --> R
+    ODOM --> R
+```
+
+### Several RGB-D or stereo cameras
+
+```yaml
+subscribe_rgbd: true
+rgbd_cameras: 4
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+Each camera is synchronized by its own [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md) or [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md), and rtabmap subscribes to their `rgbd_image0`, `rgbd_image1`... directly. This requires `rtabmap_sync` built with [`RTABMAP_SYNC_MULTI_RGBD`](../../rtabmap_sync/README.md#build-options), which is off by default; otherwise, see [Several RGB-D or stereo cameras and lidar](#several-rgb-d-or-stereo-cameras-and-lidar).
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    S0["rgbd_sync<br>or stereo_sync"]
+    S1["rgbd_sync<br>or stereo_sync"]
+    S2["rgbd_sync<br>or stereo_sync"]
+    S3["rgbd_sync<br>or stereo_sync"]
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    S0 -->|rgbd_image0| R
+    S1 -->|rgbd_image1| R
+    S2 -->|rgbd_image2| R
+    S3 -->|rgbd_image3| R
+    ODOM --> R
+```
+
+### Several RGB-D or stereo cameras and lidar
+
+```yaml
+subscribe_rgbd: true
+rgbd_cameras: 0
+subscribe_scan: true   # optional, for a 2D lidar
+#subscribe_scan_cloud: true   # optional, for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+[rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md) combines the cameras' `rgbd_image` into one `rgbd_images`. This works without any build option.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    S0["rgbd_sync<br>or stereo_sync"]
+    S1["rgbd_sync<br>or stereo_sync"]
+    S2["rgbd_sync<br>or stereo_sync"]
+    S3["rgbd_sync<br>or stereo_sync"]
+    X["rgbdx_sync"]
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    S0 -->|rgbd_image0| X
+    S1 -->|rgbd_image1| X
+    S2 -->|rgbd_image2| X
+    S3 -->|rgbd_image3| X
+    X -->|rgbd_images| R
+    SCAN --> R
+    ODOM --> R
+```
+
+### Lidar alone
+
+```yaml
+subscribe_scan: true   # for a 2D lidar
+#subscribe_scan_cloud: true   # for a 3D lidar
+subscribe_depth: false
+subscribe_rgb: false
+```
+
+There are no images: bag-of-words is disabled, and loop closures are found by proximity alone.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    SCAN(["scan or scan_cloud"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    SCAN --> R
+    ODOM --> R
+```
+
+### RGB camera with odometry
+
+```yaml
+subscribe_depth: false
+subscribe_rgb: true   # default
+```
+
+Without depth, the images cannot build a metric map, so this is mainly useful in localization mode, to localize a single camera on a map built with a depth camera. [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md) can also be used to synchronize the image with its camera_info, and rtabmap then subscribes to its `rgbd_image` with `subscribe_rgbd:=true` and `subscribe_rgb:=false`.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["rgb/image<br>rgb/camera_info"])
+    ODOM(["odom<br><i>or TF's odom_frame_id</i>"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+    ODOM --> R
+```
+
+### RGB camera alone (appearance-based loop closure detection)
+
+```yaml
+subscribe_depth: false
+subscribe_rgb: false
+subscribe_odom: false
+RGBD/Enabled: "false"
+```
+
+The node then subscribes to `image` and only detects loop closures between images: no odometry, no graph optimization, no metric map.
+
+```mermaid
+%%{init: {'flowchart': {'nodeSpacing': 15, 'rankSpacing': 30}}}%%
+flowchart LR
+    CAM(["image"])
+    R["<b>rtabmap</b>"]
+    CAM --> R
+```
+
+## Odometry from TF
+
+Setting `odom_frame_id` reads odometry from TF instead: `odom_frame_id` → `frame_id` is looked up at the stamp of each sensor message, and `subscribe_odom` is turned off. This is the natural arrangement when the odometry source publishes only TF, and it saves synchronizing one more topic.
+
+The trade-off is covariance: TF has none, so every link gets `odom_tf_linear_variance` and `odom_tf_angular_variance`, and an odometry reset can only be recognized by an identity pose — not by the `9999` covariance the [odometry nodes](../../rtabmap_odom/README.md#lost-frames-resets-and-new-maps) publish when they lose track. Prefer the topic when the source provides a meaningful covariance.
+
+A sensor message whose stamp cannot be found in TF within `wait_for_transform` is dropped. The same goes for a sensor frame that is not connected to `frame_id`.
+
+## Automatic adjustments
+
+Some RTAB-Map defaults only make sense for a camera. With a lidar, or without a camera, the node adjusts them — for example, the occupancy grid built from the scan, and loop closures registered with ICP — unless they were set explicitly, and the log says what it changed.
+
+## Sensors not stamped together
+
+On a real robot the sensors are rarely stamped together: odometry, a lidar and cameras run at their own rates and are triggered independently. The synchronizer (`approx_sync`) groups the closest messages into one update, and the node then makes them consistent in time:
+
+- **The node's stamp is the lidar's**, when there is one, or else the first camera's.
+- **The odometry is taken at that stamp**, interpolated in TF between odometry samples. Without odometry in TF, the synchronized odometry message is used as it is, pose and stamp: the node then takes the odometry's stamp rather than the lidar's.
+- **The link's covariance is the synchronized odometry message's**, not the last one received: the largest among the updates merged into the node.
+
+`odom_sensor_sync`, on by default, uses the odometry in TF to correct each sensor for the robot's motion:
+
+- **Each camera** is moved by the motion between its own stamp and the node's: an image taken 15 ms after the lidar is placed where the robot was 15 ms later. With several cameras triggered one after the other -- in one `rgbd_images` message or on separate topics -- each keeps its own stamp and is placed separately.
+- **A 3D cloud** (`scan_cloud`) is assumed already deskewed, and is moved as a whole, the same way. Deskew it upstream, with [`lidar_deskewing`](../../rtabmap_util/doc/lidar_deskewing.md) or [`icp_odometry`'s deskewing](../../rtabmap_odom/doc/icp_odometry.md#deskewing).
+- **A 2D scan** (`scan`) is deskewed ray by ray, when its `time_increment` is set: each ray is placed where the robot was when it was measured. That needs the odometry in TF across the whole sweep, within `wait_for_transform`.
+
+**Without odometry in TF** -- odometry published as a topic only -- none of this is possible, and the sensors are used as they are: cameras at their mount with a warning, 2D scans without deskewing with a warning shown once. Nothing is dropped.
+
+With `odom_sensor_sync` off, every sensor is placed at its mount, as if it had been stamped with the lidar, and 2D scans are not deskewed. On a moving robot, that costs centimeters: a camera triggered 15 ms late on a robot turning at 0.5 rad/s misplaces what it sees 3 m away by 2 cm, and a 0.1 s lidar sweep at 1 m/s bends the scan by 10 cm.
+
+## Subscribed Topics
+
+**Synchronized** — see [Choosing the inputs](#choosing-the-inputs).
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image`, `depth/image`, `rgb/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | RGB-D camera, depth registered to color. |
+| `left/image_rect`, `right/image_rect`, `left/camera_info`, `right/camera_info` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Stereo camera. |
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | One camera, from [rgbd_sync](../../rtabmap_sync/doc/rgbd_sync.md), [stereo_sync](../../rtabmap_sync/doc/stereo_sync.md) or [rgb_sync](../../rtabmap_sync/doc/rgb_sync.md). |
+| `rgbd_images` | [`rtabmap_msgs/msg/RGBDImages`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImages.html) | Several cameras, from [rgbdx_sync](../../rtabmap_sync/doc/rgbdx_sync.md). |
+| `scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | 2D lidar. |
+| `scan_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | 3D lidar, or [`icp_odometry`](../../rtabmap_odom/doc/icp_odometry.md)'s [filtered scan](../../rtabmap_odom/doc/icp_odometry.md#reusing-the-filtered-scan-downstream). |
+| `scan_descriptor` | [`rtabmap_msgs/msg/ScanDescriptor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/ScanDescriptor.html) | A scan with a global descriptor for loop closure detection. |
+| `sensor_data` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | Everything a node holds in one message, as the odometry nodes republish it on `odom_sensor_data/*`. |
+| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry. Mainly used for its covariance, which weights the link between consecutive nodes in the graph (see [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps)). The pose is taken from TF at the sensors' stamp when available; the message's pose is used otherwise. |
+| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | From an `rtabmap_odom` node: its statistics are stored with the node, and its measured motion gives the velocity. |
+| `user_data` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data stored with the node. |
+
+**Asynchronous** — buffered, and attached to the next node. See [Asynchronous inputs](#asynchronous-inputs).
+
+| Topic | Type | Description |
+|---|---|---|
+| `user_data_async` | [`rtabmap_msgs/msg/UserData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/UserData.html) | Arbitrary data for the next node. See [User data and environment sensors](#user-data-and-environment-sensors). |
+| `gps/fix` | [`sensor_msgs/msg/NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS. See [GPS and global pose](#gps-and-global-pose). |
+| `global_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | An absolute pose from outside, added as a prior. See [GPS and global pose](#gps-and-global-pose). |
+| `imu` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only the orientation is used, for gravity constraints: it must already be estimated, by [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools) for example. See [IMU](#imu). |
+| `landmark_detection`, `landmark_detections` | [`rtabmap_msgs/msg/LandmarkDetection`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetection.html), [`rtabmap_msgs/msg/LandmarkDetections`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/LandmarkDetections.html) | Fiducials or any other identified landmark. See [Landmarks](#landmarks). |
+| `apriltag/detections` | [`apriltag_msgs/msg/AprilTagDetectionArray`](https://github.com/christianrauch/apriltag_msgs/blob/master/msg/AprilTagDetectionArray.msg) | Landmarks straight from [apriltag_ros](https://github.com/christianrauch/apriltag_ros). `tag_detections` is its deprecated name. |
+| `aruco/detections` | [`aruco_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_ros](https://github.com/pal-robotics/aruco_ros). |
+| `aruco_opencv/detections` | [`aruco_opencv_msgs/msg/ArucoDetection`](https://docs.ros.org/en/jazzy/p/aruco_opencv_msgs/msg/ArucoDetection.html) | Landmarks straight from [ros_aruco_opencv](https://github.com/fictionlab/ros_aruco_opencv). |
+| `aruco_markers/detections` | [`aruco_markers_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/aruco_markers_msgs/msg/MarkerArray.html) | Landmarks straight from [aruco_markers](https://github.com/namo-robotics/aruco_markers). |
+| `aruco_interfaces/detections` | [`ros2_aruco_interfaces/msg/ArucoMarkers`](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco/blob/main/ros2_aruco_interfaces/msg/ArucoMarkers.msg) | Landmarks straight from [ros2_aruco](https://github.com/JMU-ROBOTICS-VIVA/ros2_aruco). |
+| `env_sensor` | [`rtabmap_msgs/msg/EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | A scalar reading stored with the next node: WiFi signal strength, or one of the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment). See [User data and environment sensors](#user-data-and-environment-sensors). |
+| `inter_odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | A faster odometry, to fill the gaps between nodes with intermediate nodes. See [Intermediate odometry](#intermediate-odometry). |
+| `inter_odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Its statistics, with `subscribe_inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
+
+Each detector topic (`apriltag/detections` to `aruco_interfaces/detections`) exists only when this package was built with that detector's messages package. They all feed the same landmarks as `landmark_detection`; see [Landmarks](#landmarks).
+
+**Commands**
+
+| Topic | Type | Description |
+|---|---|---|
+| `initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | Where the robot is, in localization mode. |
+| `goal` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html) | A goal pose to plan to. |
+| `goal_node` | [`rtabmap_msgs/msg/Goal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Goal.html) | A goal node, by id or label. |
+| `~/republish_node_data` | [`std_msgs/msg/Int32MultiArray`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Int32MultiArray.html) | Node ids whose data to include in the next `mapData`, for a visualizer catching up on a map it joined late. |
+
+## Published Topics
+
+**Every topic is published only when something is subscribed** — the work of building each message is skipped otherwise. The TF broadcast is not gated this way.
+
+| Topic | Type | Description |
+|---|---|---|
+| `info` | [`rtabmap_msgs/msg/Info`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Info.html) | Everything about the last update: the node id, loop closure and proximity detection results, and all of RTAB-Map's statistics with their timings. One per processed update — intermediate nodes excepted. The first thing to look at when the map misbehaves. |
+| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The optimized graph, plus the data of the node just added. What `rtabmap_viz` and [map_assembler](../../rtabmap_util/doc/map_assembler.md) consume. |
+| `mapGraph` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | The optimized graph alone: poses, links and the map → odom correction. Latched when `latch` is on. |
+| `mapPath` | [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html) | The optimized trajectory, for display. |
+| `mapOdomCache` | [`rtabmap_msgs/msg/MapGraph`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapGraph.html) | In localization mode, the recent odometry poses kept to localize against, with their links to the map. |
+| `localization_pose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | The robot in the map frame after each update, with RTAB-Map's covariance — in mapping mode, the odometry's accumulated along the graph. See [Localization](#localization). |
+| `landmarks` | [`geometry_msgs/msg/PoseArray`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseArray.html) | The optimized landmark poses. |
+| `labels` | [`visualization_msgs/msg/MarkerArray`](https://docs.ros.org/en/jazzy/p/visualization_msgs/msg/MarkerArray.html) | Node ids, labels and landmark ids as text, for RViz. |
+| `local_grid_obstacle`, `local_grid_empty`, `local_grid_ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The local occupancy grid of the node just added, in `frame_id`. |
+| `map`, `grid_prob_map`, `cloud_map`, `cloud_obstacles`, `cloud_ground`, `octomap_*`, `elevation_map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html), [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), [`octomap_msgs/msg/Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html), [`grid_map_msgs/msg/GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | The assembled maps, from [`MapsManager`](../../rtabmap_util/README.md#mapsmanager), which lists them. |
+| `goal_out`, `goal_reached`, `global_path`, `local_path`, `global_path_nodes`, `local_path_nodes` | [`geometry_msgs/msg/PoseStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseStamped.html), [`std_msgs/msg/Bool`](https://docs.ros.org/en/jazzy/p/std_msgs/msg/Bool.html), [`nav_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Path.html), [`rtabmap_msgs/msg/Path`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/Path.html) | Planning. See [Planning](#planning). |
+
+## Services
+
+All under the node's name: `/rtabmap/reset`, not `/reset`. They run in the same callback group as SLAM, so a call waits for the current update to finish, and no update runs while a service does.
+
+| Service | Type | Description |
+|---|---|---|
+| `reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | **Erase the map**, in memory and in the database. Node ids start over from 1. |
+| `trigger_new_map` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Start a new session in the same database; the old one is kept. |
+| `pause`, `resume` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Stop taking input, and start again. Input received while paused is dropped, not queued. Mirrored in the `is_rtabmap_paused` parameter, which can also start the node paused. |
+| `set_mode_localization`, `set_mode_mapping` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | See [Mapping and localization](../README.md#mapping-and-localization). |
+| `backup` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Save the database now, copy it to `<database_path>.back`, and carry on in a new session. |
+| `load_database` | [`rtabmap_msgs/srv/LoadDatabase`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/LoadDatabase.html) | Save the current map and switch to another database; `clear` empties the target first. The current parameters are kept — a warning lists those the target database was built with differently. |
+| `update_parameters` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Re-read every RTAB-Map ROS parameter and apply it. |
+| `get_map_data` | [`rtabmap_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap.html) | The graph and its nodes. `graph_only` leaves out their images, scans and user data, which are most of the size; `global_map` includes the nodes not in working memory; `optimized` returns optimized poses rather than odometry ones. |
+| `get_map_data2` | [`rtabmap_msgs/srv/GetMap2`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetMap2.html) | The same, choosing each kind of node data separately. |
+| `get_node_data` | [`rtabmap_msgs/srv/GetNodeData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodeData.html) | Given nodes, with the data asked for. No id means the latest node. |
+| `get_map`, `get_prob_map` | [`nav_msgs/srv/GetMap`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetMap.html) | The occupancy grid, as trinary or as probabilities. Empty if the map has no grid. |
+| `publish_map` | [`rtabmap_msgs/srv/PublishMap`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/PublishMap.html) | Republish the map on the topics that have subscribers, with the same `global_map`, `optimized` and `graph_only` options. |
+| `get_nodes_in_radius` | [`rtabmap_msgs/srv/GetNodesInRadius`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetNodesInRadius.html) | Nodes within a radius of a node (not counting it) or of a position, which is used when `node_id` is 0 and it is not the origin. |
+| `set_label`, `list_labels`, `remove_label` | [`rtabmap_msgs/srv/SetLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetLabel.html), [`rtabmap_msgs/srv/ListLabels`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/ListLabels.html), [`rtabmap_msgs/srv/RemoveLabel`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/RemoveLabel.html) | Name nodes, so a goal can be `"kitchen"` rather than an id. Node 0 means the latest node. A label is unique in the map. |
+| `add_link` | [`rtabmap_msgs/srv/AddLink`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/AddLink.html) | Add a constraint found outside the node — a loop closure from another process, for instance. |
+| `detect_more_loop_closures` | [`rtabmap_msgs/srv/DetectMoreLoopClosures`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/DetectMoreLoopClosures.html) | Post-processing: look for loop closures between nodes close to each other in the optimized graph. |
+| `global_bundle_adjustment` | [`rtabmap_msgs/srv/GlobalBundleAdjustment`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GlobalBundleAdjustment.html) | Post-processing: refine the graph with bundle adjustment on the visual features. |
+| `cleanup_local_grids` | [`rtabmap_msgs/srv/CleanupLocalGrids`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/CleanupLocalGrids.html) | Post-processing: remove from each node's local grid the obstacles the global map says are free — people who walked through, for instance. |
+| `set_goal`, `cancel_goal`, `get_plan`, `get_plan_nodes` | [`rtabmap_msgs/srv/SetGoal`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/SetGoal.html), [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html), [`nav_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/nav_msgs/srv/GetPlan.html), [`rtabmap_msgs/srv/GetPlan`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/srv/GetPlan.html) | Planning. See [Planning](#planning). |
+| `octomap_binary`, `octomap_full` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | The octomap. Only with RTAB-Map built with OctoMap and this package built with `octomap_msgs`. |
+| `log_debug`, `log_info`, `log_warning`, `log_error` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Set RTAB-Map's own log level, independently from ROS's. |
+
+## Parameters
+
+The node's own ROS parameters, with their real types. The ones about frames are in [Frames and TF](#frames-and-tf); the input ones in [Choosing the inputs](#choosing-the-inputs); map assembly (`map_*`, `cloud_*`, `octomap_*`, `latch`) with [`MapsManager`](../../rtabmap_util/README.md#mapsmanager).
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `database_path` | `string` | `"~/.ros/rtabmap.db"` | The map. `~` is expanded, and a relative path is taken from the working directory of the process. Under `$ROS_HOME` if that is set. |
+| `delete_db_on_start` | `bool` | `false` | Start from an empty map. `-d` or `--delete_db_on_start` as an argument does the same. |
+| `use_saved_map` | `bool` | `true` | Load the occupancy grid saved in the database at startup, instead of reassembling it from the nodes. |
+| `config_path` | `string` | `""` | INI file of RTAB-Map parameters, read at startup and written on shutdown. |
+| `is_rtabmap_paused` | `bool` | `false` | Start paused, waiting for the `resume` service. |
+| `initial_pose` | `string` | `""` | `"x y z roll pitch yaw"` to start from in localization mode. See [Localization](#localization). |
+| `pub_loc_pose_only_when_localizing` | `bool` | `false` | Publish `localization_pose` only on updates that found a loop closure, a proximity detection or a landmark. |
+| `loc_thr` | `double` | `0.0` | Localization error, in meters, above which diagnostics report an error. Localization mode only; `0` disables. |
+| `odom_tf_linear_variance` | `double` | `0.001` | Translational variance used when the odometry carries no usable covariance. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
+| `odom_tf_angular_variance` | `double` | `0.001` | Rotational variance used when the odometry carries no usable covariance. |
+| `staleness_factor` | `double` | `0.0` | Start a new map after a gap longer than this many detection periods. `0` disables; values under `1` are refused and disable it too. See [Odometry, covariance and new maps](../README.md#odometry-covariance-and-new-maps). |
+| `landmark_linear_variance` | `double` | `0.001` | Translational variance of a landmark detection that carries no covariance. |
+| `landmark_angular_variance` | `double` | `0.001` | Rotational variance of a landmark detection that carries no covariance. |
+| `use_action_for_goal` | `bool` | `false` | Send goals to nav2's `navigate_to_pose` action instead of publishing them on `goal_out`. Requires the package built with `nav2_msgs`. |
+| `gen_scan` | `bool` | `false` | Derive a 2D scan from the depth image(s) when no scan is subscribed. See [Deriving missing data](#deriving-missing-data). |
+| `gen_scan_max_depth` | `double` | `4.0` | Farthest depth used for it, in meters. |
+| `gen_scan_min_depth` | `double` | `0.0` | Nearest. |
+| `gen_depth` | `bool` | `false` | Project `scan_cloud` into the camera to make a depth image, for an RGB camera with a lidar. |
+| `gen_depth_decimation` | `int` | `1` | Resolution divider for it; must divide the image size. |
+| `gen_depth_fill_holes_size` | `int` | `0` | Fill holes up to this many pixels. `0` disables. |
+| `gen_depth_fill_iterations` | `int` | `1` | Hole-filling passes. |
+| `gen_depth_fill_holes_error` | `double` | `0.1` | Maximum depth difference, in meters, across a hole for it to be filled. |
+| `stereo_to_depth` | `bool` | `false` | Compute a depth image from the stereo pair (with the `StereoBM/*` parameters) and map it as RGB-D. |
+| `scan_cloud_max_points` | `int` | `0` | Points in a full `scan_cloud` sweep, for an organized or fixed-size cloud; used by ICP as the reference for its correspondence ratio. `0` takes each cloud's own size. |
+| `scan_cloud_is_2d` | `bool` | `false` | `scan_cloud` is a 2D lidar published as a cloud. |
+| `odom_sensor_sync` | `bool` | `true` | Place each sensor where the robot was at that sensor's stamp, and deskew 2D scans ray by ray, using the odometry in TF. See [Sensors not stamped together](#sensors-not-stamped-together). |
+| `subscribe_inter_odom_info` | `bool` | `false` | Synchronize `inter_odom` with `inter_odom_info`. See [Intermediate odometry](#intermediate-odometry). |
+| `log_to_rosout_level` | `int` | `4` | RTAB-Map's own log messages at or above this level (`0` debug to `4` fatal) are forwarded to `/rosout`. |
+| `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | `0` | Reliability of those subscriptions: `0` system default, `1` reliable, `2` best effort. |
+
+And every RTAB-Map parameter, as strings, as described below.
+
+### RTAB-Map's own parameters
+
+Everything in RTAB-Map's parameter set is exposed as a ROS parameter **under its RTAB-Map name**, except the odometry ones (`Odom/*`, `OdomF2M/*`...), which belong to the [odometry nodes](../../rtabmap_odom/README.md):
+
+```bash
+ros2 run rtabmap_slam rtabmap --ros-args \
+  -p "Rtabmap/DetectionRate:='2'" \
+  -p "RGBD/LinearUpdate:='0.2'" \
+  -p "Mem/IncrementalMemory:='false'"
+```
+
+**Every RTAB-Map parameter is declared as a string**, whatever it looks like, because that is how RTAB-Map's own parameter map stores them. `-p RGBD/LinearUpdate:=0.2` makes ROS infer a double, and the node throws on startup. The inner quotes are what keep it a string; in a launch file, `{'RGBD/LinearUpdate': '0.2'}`. The node's own ROS parameters — `frame_id`, `publish_tf`, `subscribe_scan` — have their real types and take plain values.
+
+`rtabmap --params` prints them all with their defaults and descriptions, and so does [RTAB-Map's parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). Two defaults differ from RTAB-Map's own: **`RGBD/CreateOccupancyGrid` is `true`**, since a robot's map is usually meant for navigation, and **`Rtabmap/WorkingDirectory` is `$ROS_HOME`**, or `~/.ros`.
+
+A value can come from several places. From the highest priority to the lowest:
+
+1. **Arguments**, `--Param/Name value` after the executable name, or in a launch file's `arguments=[...]`.
+2. **ROS parameters.**
+3. **`config_path`**, an INI file of RTAB-Map parameters. The node writes its parameters back to it on shutdown.
+4. **The node's own adjustments to its inputs** — ICP registration for a lidar with no camera, for example. See [Automatic adjustments](#automatic-adjustments).
+5. **The database**, which remembers the parameters it was built with. See [The database](../README.md#the-database).
+6. The defaults.
+
+A parameter changed while the node runs, with `ros2 param set`, is applied straight away. `update_parameters` re-reads them all, for a change the node might have missed.
+
+Parameters RTAB-Map has renamed are still accepted under their old name, with a warning naming the new one — worth heeding, since the old names are not declared and so do not show up in `ros2 param list`.
+
+The ones that set how often a node is added, explained in [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates):
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `Rtabmap/DetectionRate` | `string` | `"1"` | Updates per second, in Hz. `0` processes every one; see the [warning](../README.md#update-rate-and-dropped-updates). |
+| `Rtabmap/CreateIntermediateNodes` | `string` | `"false"` | Keep the updates that `Rtabmap/DetectionRate` would skip, as intermediate nodes instead. |
+| `RGBD/LinearUpdate` | `string` | `"0.1"` | Minimum distance, in meters, the robot must have moved for an update to add a node. |
+| `RGBD/AngularUpdate` | `string` | `"0.1"` | Minimum rotation, in radians, the robot must have made for an update to add a node. |
+
+## Frames and TF
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `frame_id` | `string` | `"base_link"` | The robot frame. Every sensor is placed relative to it through TF. |
+| `odom_frame_id` | `string` | `""` | Read odometry from TF, as `odom_frame_id` → `frame_id` at each sensor stamp, instead of from the `odom` topic. Setting it forces `subscribe_odom` off. See [Odometry from TF](#odometry-from-tf). |
+| `odom_frame_id_init` | `string` | `""` | The odometry frame to publish `map` → it from the start, before any odometry has been received. Ignored when `odom_frame_id` is set. |
+| `map_frame_id` | `string` | `"map"` | The map frame, on TF and in the header of everything published in it. |
+| `publish_tf` | `bool` | `true` | Publish `map_frame_id` → odometry frame. |
+| `tf_delay` | `double` | `0.05` | Period of that publication, in seconds (20 Hz). `0` disables it. |
+| `tf_tolerance` | `double` | `0.1` | How far in the future the transform is stamped, in seconds, so that lookups at the latest sensor stamp do not have to wait for it. |
+| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a TF lookup before giving up on it. |
+| `ground_truth_frame_id` | `string` | `""` | The fixed frame of a ground truth system, for example `world` published by an external localization system like Vicon or OptiTrack. `ground_truth_frame_id` → `ground_truth_base_frame_id` is looked up and stored with each node, for evaluating a trajectory afterwards. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame in the ground truth tree, for example `base_link_gt`. To avoid breaking the TF tree, it represents the same frame as `frame_id`, but in a parallel TF tree, so that the robot frame does not get two parents. |
+
+**This node publishes exactly one transform: `map` → `odom`.** It is the correction that puts the odometry frame where the optimized graph says it belongs — the identity until a loop closure moves it. Odometry keeps publishing `odom` → `base_link`, and the sensors must be attached to `base_link` in TF, as in the [TF tree](../README.md#frames-and-tf).
+
+The odometry frame is taken from the odometry messages themselves, so the transform only starts once the first update has been processed — unless `odom_frame_id` or `odom_frame_id_init` says what it will be. It is published from a thread of its own at a fixed rate, independently from how fast SLAM runs.
+
+**With `Optimizer/Iterations` set to `0`, the `map` → `odom` transform is not published at all**, even with `publish_tf` on: with graph optimization disabled there is no correction to publish. That is the arrangement where another node optimizes the graph and publishes the transform instead.
+
+## Asynchronous inputs
+
+These are not synchronized with the sensors. Each is buffered as it arrives and attached to the next update, then cleared, so each value is stored with one node only. They are received on callback groups of their own, and keep being buffered while an update is processed.
+
+### Landmarks
+
+A landmark is anything recognized with an identity and a pose relative to the robot — typically a fiducial marker. It becomes a node of the graph under the **negative** of its id, linked to each node that saw it, so seeing the same marker again is a loop closure however far the odometry has drifted.
+
+- **Ids must be positive.** A detection with id 0 or less is refused.
+- The detection's frame must be in TF, connected to `frame_id`. Its pose is also corrected for the motion between its stamp and the node's, with the odometry in TF.
+- Without a covariance in the message, `landmark_linear_variance` and `landmark_angular_variance` are used. Their default of `0.001` is a standard deviation of about 3 cm, fitting a marker seen close; raise them for markers seen far away.
+- Between two updates, only the latest detection of each id is kept.
+
+`apriltag/detections` expects the apriltag_ros convention, where each detection is also published on TF as `family:id` from the camera frame; the pose is taken from there.
+
+The optimized landmarks are published on `landmarks`, and their ids on `labels`.
+
+**Landmarks can place the map in the world.** `Marker/Priors` gives some of them known world poses, `"id x y z roll pitch yaw"` with angles in radians, several separated by `|`: `"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57"` puts marker 2 one meter in front of marker 1, turned 90 degrees. As soon as one of them is seen, the map is transformed into that world frame: the robot's poses, and `map`, are then world coordinates. The priors are weighted by `Marker/PriorsVarianceLinear` and `Marker/PriorsVarianceAngular` (`0.001` by default). **They only apply with `Optimizer/PriorsIgnored` set to `false`**; at its default, `true`, they are ignored without a warning, like the GPS and global pose priors.
+
+### GPS and global pose
+
+**`gps/fix`** stores a GPS fix with the node closest in time to it, provided it is within one detection period of it (any, with `Rtabmap/DetectionRate` at `0`). Its error is the square root of the largest position variance, or 10 m when the covariance type is unknown. The fixes are stored for export and for georeferencing the map; `Rtabmap/LoopGPS` also uses them to discard loop closure candidates that are too far apart.
+
+**`global_pose`** is an absolute pose from outside — a motion capture system, a localization against another map. It is added to the node as a **pose prior**: a link from the node to itself, weighted by the message's covariance. The same time window applies. The message's frame is taken as the sensor frame, and it is transformed to `frame_id` with TF.
+
+**Priors are stored, but ignored by the optimizer by default.** GPS and global poses only pull the graph once `Optimizer/PriorsIgnored` is `false`, with an optimizer that supports them (g2o, GTSAM).
+
+### IMU
+
+The orientation from `imu`, interpolated at the node's stamp, is transformed to `frame_id` and turned into a **gravity constraint**: a link from the node to itself that holds its roll and pitch, used by the optimizer when `Optimizer/GravitySigma` is above `0` and the optimizer supports it (g2o, GTSAM). It keeps a long 3D map level where odometry alone would let it bend.
+
+- **Only the orientation is used**; the angular velocity and linear acceleration are ignored. Most IMU drivers publish raw rates and accelerations only: estimate the orientation first with a filter such as [`imu_filter_madgwick` or `imu_complementary_filter`](https://github.com/CCNYRoboticsLab/imu_tools), and feed its output here.
+- An IMU message with no orientation (all zeros) is ignored.
+- The node's stamp must match an IMU message or lie between two, or the IMU is not used for that node.
+- The IMU frame must not change: a message from another frame clears the buffer, since it means two sources are publishing on the same topic.
+
+### User data and environment sensors
+
+**`user_data_async`** is arbitrary data — a matrix, or bytes — stored with the next node only. It cannot be combined with the synchronized `user_data`: when both are present, the asynchronous one is dropped with a warning. The same goes for `sensor_data`, whose message has a user data field of its own: the async user data is attached when that field is empty, and dropped with a warning when it is set, never carried over to a later node.
+
+**`env_sensor`** readings are stored with the next node, the latest value of each type. The types mirror the [environment sensors Android devices have](https://developer.android.com/develop/sensors-and-location/sensors/sensors_environment), plus WiFi and custom values:
+
+| `type` | Reading | Unit |
+|---|---|---|
+| `TYPE_WIFI_SIGNAL_STRENGTH` | WiFi signal strength | dBm |
+| `TYPE_AMBIENT_TEMPERATURE` | Ambient temperature | °C |
+| `TYPE_AMBIENT_AIR_PRESSURE` | Air pressure | hPa |
+| `TYPE_AMBIENT_LIGHT` | Illuminance | lx |
+| `TYPE_AMBIENT_RELATIVE_HUMIDITY` | Relative humidity | % |
+| `TYPE_CUSTOM1` to `TYPE_CUSTOM9` | Anything else | yours |
+
+### Intermediate odometry
+
+**Intermediate nodes** record the trajectory between two nodes, with no loop closure detection on them. `Rtabmap/CreateIntermediateNodes` makes them two ways:
+
+- **With `Rtabmap/DetectionRate` above `0`**, the updates that arrive too soon after the last processed one, and would be skipped, become intermediate nodes instead (see [Update rate and dropped updates](../README.md#update-rate-and-dropped-updates)). They can only come as fast as the synchronized sensor updates, since they are those updates, and keep their sensor data only with `Mem/IntermediateNodeDataKept`, which helps for building a map from every scan, at the price of a larger database.
+- **With `Rtabmap/DetectionRate` at `0`**, every update is already a full node -- which is only tractable with slow sensor updates, see the [warning](../README.md#update-rate-and-dropped-updates) -- and `inter_odom` adds poses between them: a faster odometry, whose messages between two updates become intermediate nodes without sensor data. That is for when the sensor updates are slow (2 Hz or less) while the odometry is fast (10 Hz or more): the trajectory is then as dense as the odometry rather than as the sensors.
+
+`inter_odom` is only subscribed when the node starts with `Rtabmap/CreateIntermediateNodes` on and `Rtabmap/DetectionRate` at `0` (every update processed); changing either later has no effect on it. Intermediate poses are only added once the map has a node.
+
+With `subscribe_inter_odom_info`, `inter_odom` is synchronized by exact stamp with `inter_odom_info`, the `OdomInfo` of an `rtabmap_odom` node: each intermediate node then also stores that odometry's statistics, and its velocity is taken from the measured motion. A message on one topic without its match on the other is not used.
+
+## Deriving missing data
+
+**`gen_scan`** makes a 2D scan out of the depth image: its middle row, between `gen_scan_min_depth` and `gen_scan_max_depth`, as a lidar at the camera's height would see it. With a depth camera and no lidar, this lets the occupancy grid be built the way it would be from a lidar — it also triggers the scan [adjustments](#automatic-adjustments) — and lets proximity detection register scans. **The cameras must be level**, looking parallel to the ground, as for [depthimage_to_laserscan](https://github.com/ros-perception/depthimage_to_laserscan): the middle row of a tilted camera sees the floor or the ceiling, not the walls around the robot.
+
+**`gen_depth`** goes the other way: with an RGB camera (`subscribe_rgb`) and a lidar (`subscribe_scan_cloud`), the cloud is projected into the camera to give a sparse depth image, filled by `gen_depth_fill_*`, so visual loop closures get 3D features.
+
+**`stereo_to_depth`** computes a dense depth image from a stereo pair, so a stereo camera is mapped like an RGB-D one — denser grids and clouds, at the cost of the disparity computation.
+
+## Localization
+
+`localization_pose` is the robot's pose in the map frame — `map` → `odom` composed with the odometry — after each update, with RTAB-Map's covariance. While mapping, that is the odometry covariance accumulated along the graph, growing with distance until a loop closure brings it down. In localization mode before the first loop closure, it is `9999`: the robot is not localized yet.
+
+In localization mode (`Mem/IncrementalMemory` at `false`, see the [README](../README.md#mapping-and-localization)), the robot is placed on the map by the first loop closure. Until then:
+
+- **The `initial_pose` parameter**, `"x y z roll pitch yaw"`, read at startup, says where the robot starts, and the odometry is added to it.
+- **The `initialpose` topic** ([`geometry_msgs/msg/PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html)), as RViz's *2D Pose Estimate* publishes it, does the same at any time. A pose in another frame is transformed to the map frame with TF; one without a frame is taken as being in the map frame.
+- **Without either**, the robot is assumed to restart at the last localization pose saved in the database, where it was when the node last shut down. With **`RGBD/StartAtOrigin`** set to `true`, it is assumed to start at the map's origin instead.
+
+All three are ignored in mapping mode, `initial_pose` and `initialpose` with a warning.
+
+`pub_loc_pose_only_when_localizing` restricts `localization_pose` to the updates that actually localized — found a loop closure, a proximity detection or a landmark — for a consumer that should only hear about corrections.
+
+## Planning
+
+The node plans on its own graph: a goal is a node, the plan is the chain of nodes leading to it, and a local planner is handed the next one to reach. That gives global planning across a map the local planner cannot see all of — through areas the robot has mapped, and only those.
+
+**It does not replace nav2's planner: it is a layer over it, there for memory management.** With working memory bounded (`Rtabmap/TimeThr` or `Rtabmap/MemoryThr`), the nodes moved to long-term memory stop contributing to the occupancy grid, so parts of the map published on `map` disappear over time, and nav2 alone cannot plan to them. RTAB-Map's graph still holds them: it can plan to a node in long-term memory, and as the robot moves toward it, it brings back the areas ahead of the robot, so the robot stays localized and the map around it is there for nav2 again. The plan and the retrieval are described in [Long-Term Online Multi-Session Graph-Based SPLAM with Memory Management](https://arxiv.org/abs/2301.00050) (Labbé and Michaud, *Autonomous Robots*).
+
+```mermaid
+flowchart LR
+    GOAL(["goal, goal_node<br>or set_goal"])
+    RTAB["rtabmap<br><i>global plan on the graph</i>"]
+    NAV2["nav2<br><i>planner and controller</i>"]
+    GOAL --> RTAB
+    RTAB -->|"map (occupancy grid)"| NAV2
+    RTAB -->|"next node: navigate_to_pose action<br>or goal_out topic"| NAV2
+    NAV2 -->|action result| RTAB
+```
+
+**Setting a goal:**
+
+| How | Goal |
+|---|---|
+| `set_goal` service | A node id, or a label. Returns the planned path and the planning time. |
+| `goal_node` topic | A node id, or a label. A message with neither is refused. |
+| `goal` topic | A pose, in the map frame or any frame TF can transform to it. A pose in a frame it cannot is refused. |
+
+**A pose goal within `RGBD/LocalRadius` (10 m by default) of the robot is not planned through the graph**: the plan is the node the robot is at, followed by the pose itself, and it is up to the local planner to get there. Further away, the plan goes through the graph to the node nearest the pose, and the pose is appended after it.
+
+**Following it:**
+
+- `goal_out` is the next node to reach, as a pose in the map frame, sent again whenever it changes. Point a local planner at it — or set `use_action_for_goal` to send it to nav2's `navigate_to_pose` action instead. nav2 listens for goals on `goal_pose`, so to use the topic with nav2, remap `goal_out` to `goal_pose`.
+- `global_path` and `global_path_nodes` are the whole plan, as poses and as node ids; a pose goal appears at the end with node id `0`. `local_path` and `local_path_nodes` are the part of it within the local radius.
+- `goal_reached` says `true` once the robot is within `RGBD/GoalReachedRadius` (0.5 m by default) of the goal — straight away if it already is — and `false` when planning fails, the goal cannot be found or transformed, the plan is cancelled, or the robot strays too far from the path.
+
+`cancel_goal` abandons the plan (and cancels the nav2 goal, if any).
+
+**`get_plan`** (`nav_msgs/srv/GetPlan`) and **`get_plan_nodes`** compute a plan and return it, without following it or publishing anything. `get_plan` answers in the goal's frame; `get_plan_nodes` also takes a node id and returns the node ids along the plan.
+
+Labels, set with `set_label`, are what make goals readable: `set_goal` with `node_label: "kitchen"` rather than an id that changes from one map to the next.
+
+## Diagnostics
+
+`/diagnostics` carries the input and output rates of the synchronizer, as for every [`rtabmap_sync`](../../rtabmap_sync/README.md#library) consumer: a healthy input rate with a low output rate means updates arrive but are dropped — by the rate, or because SLAM takes longer than the period.
+
+In localization mode with `loc_thr` set, a *Localization status* entry says whether the robot is localized: an error with `Not localized!` until a loop closure has placed it, an error with `Localization error is high!` while the localization error — the square root of the largest translational variance — is over `loc_thr` meters, and OK under it. It is only set up when the node **starts** in localization mode.
diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
index ca41388c..4dc26b4e 100644
--- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
@@ -121,9 +121,35 @@ class StereoDense;
 
 namespace rtabmap_slam {
 
+/**
+ * @brief The `rtabmap` node: graph SLAM around an rtabmap::Rtabmap instance.
+ *
+ * Registered as the `rtabmap_slam::CoreWrapper` component, and run by the `rtabmap`
+ * executable. The node is always named `rtabmap` unless remapped, and advertises its
+ * services under that name (`/rtabmap/reset`...).
+ *
+ * The input topics come from rtabmap_sync::CommonDataSubscriber, chosen by the
+ * `subscribe_*` parameters; each synchronized update is converted to an
+ * rtabmap::SensorData and processed on a callback group of its own, while asynchronous
+ * inputs (GPS, IMU, landmarks, user data...) are buffered on theirs and attached to the
+ * next update. An update arriving while the previous one is still processed is dropped.
+ *
+ * The graph is published on `mapGraph`, `mapData` and `mapPath`, the assembled maps
+ * through an rtabmap_util::MapsManager, and the correction `map` -> odometry frame on TF.
+ * The database is saved when the node is destroyed.
+ *
+ * See the package README and doc/rtabmap.md for the topics, parameters and services.
+ */
 class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscriber
 {
 public:
+	/**
+	 * @brief Declares the parameters, opens the database and sets up every topic and
+	 *        service.
+	 *
+	 * RTAB-Map parameters are declared as strings under their RTAB-Map names, except the
+	 * odometry ones. Throws if one is given with another type.
+	 */
 	RTABMAP_SLAM_PUBLIC
 	explicit CoreWrapper(const rclcpp::NodeOptions & options);
 	virtual ~CoreWrapper();
diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml
index 708c7a5c..5c7881bf 100644
--- a/rtabmap_slam/package.xml
+++ b/rtabmap_slam/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_slam</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's SLAM package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -17,7 +17,7 @@
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
-  <!-- <depend>nav2_msgs</depend> Not available in lyrical-->
+  <depend>nav2_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
@@ -34,9 +34,13 @@
   <depend>rtabmap_msgs</depend>
   <depend>rtabmap_util</depend>
   <depend>rtabmap_sync</depend>
-  
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>rtabmap_conversions</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 
 </package>
diff --git a/rtabmap_slam/rosdoc2.yaml b/rtabmap_slam/rosdoc2.yaml
new file mode 100644
index 00000000..0b0420b9
--- /dev/null
+++ b/rtabmap_slam/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_slam
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_slam --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_slam 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_slam',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }
diff --git a/rtabmap_slam/src/CoreNode.cpp b/rtabmap_slam/src/CoreNode.cpp
index d16aa4f1..b4a82302 100644
--- a/rtabmap_slam/src/CoreNode.cpp
+++ b/rtabmap_slam/src/CoreNode.cpp
@@ -29,6 +29,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rclcpp/rclcpp.hpp"
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/core/Version.h>
+#ifdef RTABMAP_PYTHON
+#include <rtabmap/core/PythonInterface.h>
+#endif
 
 int main(int argc, char** argv)
 {
@@ -81,6 +85,12 @@ int main(int argc, char** argv)
 		arguments.push_back(argv[i]);
 	}
 
+#ifdef RTABMAP_PYTHON
+	// Initialize the embedded python interpreter on the main thread, as
+	// the nodelet below is loaded in a worker thread.
+	rtabmap::PythonInterface::instance("rtabmap");
+#endif	
+
 	rclcpp::init(argc, argv);
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);
diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp
index c1fb75b8..94fd5517 100644
--- a/rtabmap_slam/src/CoreWrapper.cpp
+++ b/rtabmap_slam/src/CoreWrapper.cpp
@@ -139,7 +139,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		tfThreadRunning_(false),
 		interOdomSync_(0),
 		stereoToDepth_(false),
-		odomSensorSync_(false),
+		odomSensorSync_(true),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		mappingMaxNodes_(Parameters::defaultGridGlobalMaxNodes()),
@@ -160,7 +160,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
-	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
 
 	bool publishTf = true;
 	std::string initialPoseStr;
@@ -232,6 +232,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 	stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_);
 	odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
+	bool interOdomInfo = false;
+	interOdomInfo = this->declare_parameter("subscribe_inter_odom_info", interOdomInfo);
 
 	RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id      = \"%s\"", frameId_.c_str());
 	RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str());
@@ -289,7 +291,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 	infoPub_ = this->create_publisher<rtabmap_msgs::msg::Info>("info", 1);
 	mapDataPub_ = this->create_publisher<rtabmap_msgs::msg::MapData>("mapData", 1);
-	mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	// Intra-process communication doesn't support transient local durability: when latching,
+	// disable it on this publisher, otherwise keep the node's setting.
+	rclcpp::PublisherOptions latchedPubOptions;
+	if(mapsManager_.isLatching())
+	{
+		latchedPubOptions.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+	}
+	mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), latchedPubOptions);
 	odomCachePub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapOdomCache", 1);
 	landmarksPub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("landmarks", 1);
 	labelsPub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("labels", 1);
@@ -418,11 +427,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		iter!=Parameters::getRemovedParameters().end();
 		++iter)
 	{
+		// Old names are never declared, so they can only be found among the overrides.
 		std::string paramValue;
-		rclcpp::Parameter parameter;
-		if(get_parameter(iter->first, parameter))
+		std::map<std::string, rclcpp::ParameterValue>::const_iterator oter = overrides.find(iter->first);
+		if(oter != overrides.end() && oter->second.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
 		{
-			paramValue = parameter.as_string();
+			paramValue = oter->second.get<std::string>();
 		}
 		if(!paramValue.empty())
 		{
@@ -563,8 +573,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 			RCLCPP_INFO(this->get_logger(), "Create intermediate nodes");
 			if(rate_ == 0.0f)
 			{
-				bool interOdomInfo = false;
-				if(get_parameter("subscribe_inter_odom_info", interOdomInfo))
+				if(interOdomInfo)
 				{
 					RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom + info messages");
 					interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
@@ -808,13 +817,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 			if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
 			{
-				if(this->isSubscribedToScan2d())
+				if(this->isSubscribedToScan2d() || (this->isSubscribedToScan3d() && scanCloudIs2d_))
 				{
-					RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
+					RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"%s\" is "
 							"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
 							"scans. To disable, set \"%s\" to 0. To suppress this warning, "
 							"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
 							Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
+							this->isSubscribedToScan2d()?"subscribe_scan":"scan_cloud_is_2d",
 							Parameters::kRegStrategy().c_str(),
 							Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
 							Parameters::kRGBDProximityPathMaxNeighbors().c_str());
@@ -1392,7 +1402,8 @@ void CoreWrapper::commonMultiCameraCallback(
 		}
 	}
 
-	if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
+	UScopeMutex syncDataLock(syncDataMutex_, false);
+	if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
 	{
 		UScopeMutex lock(lastPoseMutex_);
 		commonMultiCameraCallbackImpl(odomFrameId,
@@ -1412,7 +1423,6 @@ void CoreWrapper::commonMultiCameraCallback(
 		if(syncData_.valid) {
 			syncTimer_->reset();
 		}
-		syncDataMutex_.unlock();
 	}
 }
 
@@ -1783,7 +1793,8 @@ void CoreWrapper::commonLaserScanCallback(
 		}
 	}
 
-	if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
+	UScopeMutex syncDataLock(syncDataMutex_, false);
+	if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
 	{
 		UScopeMutex lock(lastPoseMutex_);
 		LaserScan scan;
@@ -1878,7 +1889,6 @@ void CoreWrapper::commonLaserScanCallback(
 		lastPoseCovariance_ = cv::Mat();
 
 		syncTimer_->reset();
-		syncDataMutex_.unlock();
 	}
 }
 
@@ -1895,7 +1905,8 @@ void CoreWrapper::commonOdomCallback(
 		return;
 	}
 
-	if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
+	UScopeMutex syncDataLock(syncDataMutex_, false);
+	if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
 	{
 		UScopeMutex lock(lastPoseMutex_);
 		cv::Mat userData;
@@ -1947,7 +1958,6 @@ void CoreWrapper::commonOdomCallback(
 		lastPoseCovariance_ = cv::Mat();
 
 		syncTimer_->reset();
-		syncDataMutex_.unlock();
 	}
 }
 
@@ -1983,12 +1993,29 @@ void CoreWrapper::commonSensorDataCallback(
 		}
 	}
 
-	if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
+	UScopeMutex syncDataLock(syncDataMutex_, false);
+	if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
 	{
 		UScopeMutex lock(lastPoseMutex_);
 		syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
 		syncData_.data.setId(lastPoseIntermediate_?-1:0);
 
+		{
+			UScopeMutex lock(userDataMutex_);
+			if(!userData_.empty())
+			{
+				if(!syncData_.data.userDataRaw().empty() || !syncData_.data.userDataCompressed().empty())
+				{
+					RCLCPP_WARN(this->get_logger(), "Sensor data received already contains user data. Async user data dropped!");
+				}
+				else
+				{
+					syncData_.data.setUserData(userData_);
+				}
+				userData_ = cv::Mat();
+			}
+		}
+
 		OdometryInfo odomInfo;
 		if(odomInfoMsg.get())
 		{
@@ -2012,7 +2039,6 @@ void CoreWrapper::commonSensorDataCallback(
 		lastPoseCovariance_ = cv::Mat();
 
 		syncTimer_->reset();
-		syncDataMutex_.unlock();
 	}
 }
 
@@ -2057,7 +2083,7 @@ void CoreWrapper::process(
 		// Add intermediate nodes?
 		for(std::list<std::pair<nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
 		{
-			if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp)
+			if(rclcpp::Time(iter->first.header.stamp) < stamp)
 			{
 				Transform interOdom;
 				if(!rtabmap_.getLocalOptimizedPoses().empty())
@@ -2206,8 +2232,8 @@ void CoreWrapper::process(
 				Transform correction = rtabmap_conversions::getMovingTransform(
 						frameId_,
 						odomFrameId,
-						stamp,
 						rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec),
+						stamp,
 						*tfBuffer_,
 						waitForTransform_);
 				if(!correction.isNull())
@@ -3742,10 +3768,10 @@ void CoreWrapper::globalBundleAdjustmentCallback(
 	UTimer timer;
 	int optimizer = (int)Optimizer::kTypeG2O; // g2o
 	int iterations = Parameters::defaultOptimizerIterations();
-	float pixelVariance = Parameters::defaultg2oPixelVariance();
+	float pixelVariance = Parameters::defaultOptimizerPixelVariance();
 	bool rematchFeatures = true;
 	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations);
-	Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance);
+	Parameters::parse(parameters_, Parameters::kOptimizerPixelVariance(), pixelVariance);
 	if(req->type == 1.0f)
 	{
 		optimizer = (int)Optimizer::kTypeCVSBA;
diff --git a/rtabmap_slam/test/core_wrapper_fixture.hpp b/rtabmap_slam/test/core_wrapper_fixture.hpp
new file mode 100644
index 00000000..382337a4
--- /dev/null
+++ b/rtabmap_slam/test/core_wrapper_fixture.hpp
@@ -0,0 +1,431 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
+#define RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_
+
+#include <gtest/gtest.h>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2_ros/static_transform_broadcaster.hpp>
+#include <std_srvs/srv/empty.hpp>
+
+#include <rtabmap_msgs/msg/info.hpp>
+#include <rtabmap_msgs/msg/map_graph.hpp>
+#include <rtabmap_msgs/srv/get_node_data.hpp>
+#include <rtabmap_msgs/srv/get_map.hpp>
+
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UFile.h>
+
+#include <rtabmap_slam/CoreWrapper.h>
+
+#include "msg_builders.hpp"
+#include "node_test_utils.hpp"
+
+#include <unistd.h>
+
+#include <atomic>
+#include <chrono>
+#include <cstdlib>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace rtabmap_slam_test {
+
+/**
+ * @brief Drives one `rtabmap` node over real ROS topics, against its own database.
+ *
+ * Every test gets a fresh directory for the database and the working directory, so no
+ * test reads another's map and nothing lands in ~/.ros. The node is destroyed before the
+ * directory is removed: its destructor is what saves the database, and a test that wants
+ * to reopen a map does it by destroying the node itself and building another one.
+ *
+ * By default the node subscribes to odometry only (`subscribe_depth` and `subscribe_rgb`
+ * off), the cheapest input that still builds a graph: each odometry message is a node.
+ * `Rtabmap/DetectionRate` is 0 so every message is processed rather than one per second.
+ */
+class CoreWrapperTest : public NodeTest
+{
+protected:
+	void SetUp() override
+	{
+		NodeTest::SetUp();
+		static std::atomic<int> counter(0);
+		const char * tmp = std::getenv("TMPDIR");
+		dir_ = std::string(tmp && *tmp ? tmp : "/tmp") + "/rtabmap_slam_test_" +
+				std::to_string(::getpid()) + "_" + std::to_string(counter++);
+		UDirectory::makeDir(dir_);
+	}
+
+	void TearDown() override
+	{
+		// Removes the node from the executor first: the destructor then runs with nothing
+		// left to call back into it.
+		stopNodeThreads();
+		NodeTest::TearDown();
+		node_.reset();
+		staticTf_.reset();
+		tfPub_.reset();
+		removeDir(dir_);
+	}
+
+	/// Where this test's database lives.
+	std::string databasePath() const { return dir_ + "/rtabmap.db"; }
+	const std::string & dir() const { return dir_; }
+
+	/// The parameters every test starts from; @p params are applied on top.
+	std::vector<rclcpp::Parameter> defaultParameters(
+			const std::vector<rclcpp::Parameter> & params = {}) const
+	{
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("database_path", databasePath()),
+			rclcpp::Parameter("Rtabmap/WorkingDirectory", dir_),
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_rgb", false),
+			rclcpp::Parameter("Rtabmap/DetectionRate", "0"),
+		};
+		for(const rclcpp::Parameter & p : params)
+		{
+			bool replaced = false;
+			for(rclcpp::Parameter & q : all)
+			{
+				if(q.get_name() == p.get_name())
+				{
+					q = p;
+					replaced = true;
+				}
+			}
+			if(!replaced)
+			{
+				all.push_back(p);
+			}
+		}
+		return all;
+	}
+
+	/// Builds the node under test with defaultParameters() plus @p params.
+	std::shared_ptr<rtabmap_slam::CoreWrapper> makeNode(
+			const std::vector<rclcpp::Parameter> & params = {},
+			const std::vector<std::string> & arguments = {})
+	{
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(defaultParameters(params));
+		if(!arguments.empty())
+		{
+			options.arguments(arguments);
+		}
+		node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options));
+		return node_;
+	}
+
+	/**
+	 * @brief Builds the node under test like makeNode(), but spins it on a multi-threaded
+	 *        executor of its own, in the background, as the `rtabmap` executable does.
+	 *
+	 * The node's mutexes are recursive: on the shared single-threaded executor, a mutex a
+	 * callback leaves locked is simply taken again by the next callback, on the same thread,
+	 * and nothing shows. With callbacks on several threads, the next one is blocked or skips
+	 * its update, as in the real node. The helper node keeps spinning on the shared executor.
+	 */
+	std::shared_ptr<rtabmap_slam::CoreWrapper> makeMultiThreadedNode(
+			const std::vector<rclcpp::Parameter> & params = {})
+	{
+		rclcpp::NodeOptions options;
+		options.parameter_overrides(defaultParameters(params));
+		node_ = std::make_shared<rtabmap_slam::CoreWrapper>(options);
+		nodeExecutor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(
+				rclcpp::ExecutorOptions(), 4);
+		nodeExecutor_->add_node(node_);
+		nodeThread_ = std::thread([this]() { nodeExecutor_->spin(); });
+		spinFor(std::chrono::milliseconds(50));   // see NodeTest::addNode()
+		return node_;
+	}
+
+	/**
+	 * @brief Destroys the node under test, which is what saves its database.
+	 *
+	 * The executor and the helper node are rebuilt along with it, so publishers and
+	 * collectors made before this call are dead afterwards. Reusing the executor is not
+	 * an option: on Humble, one that had a node removed from it still holds that node's
+	 * guard condition and dereferences it on the next spin.
+	 */
+	void destroyNode()
+	{
+		stopNodeThreads();
+		staticTf_.reset();
+		tfPub_.reset();
+		NodeTest::TearDown();
+		node_.reset();
+		NodeTest::SetUp();
+	}
+
+	/// A parameter of the node under test, as the string RTAB-Map stores it as.
+	std::string param(const std::string & name) const
+	{
+		return node_->get_parameter(name).as_string();
+	}
+
+	/// Latches base_link -> @p child as a static transform, as a URDF would.
+	void publishStaticTf(const std::string & child, double x = 0.0, double y = 0.0,
+			double z = 0.0, const std::string & parent = "base_link")
+	{
+		geometry_msgs::msg::TransformStamped tf = makeTransform(parent, child, 0.0, x, y);
+		tf.header.stamp = helper()->now();
+		tf.transform.translation.z = z;
+		publishStaticTf(tf);
+	}
+
+	/**
+	 * @brief base_link -> @p child as a camera optical frame, @p z meters up.
+	 *
+	 * An optical frame looks along its own +z, with +x to the right of the image: rotated
+	 * here so the camera looks along the robot's +x, as mounted on the front of a robot.
+	 */
+	static geometry_msgs::msg::TransformStamped opticalTransform(
+			const std::string & child, double z = 0.0)
+	{
+		geometry_msgs::msg::TransformStamped tf = makeTransform("base_link", child, 0.0);
+		tf.transform.translation.z = z;
+		tf.transform.rotation.x = -0.5;
+		tf.transform.rotation.y = 0.5;
+		tf.transform.rotation.z = -0.5;
+		tf.transform.rotation.w = 0.5;
+		return tf;
+	}
+
+	/// Latches opticalTransform() as a static transform.
+	void publishOpticalTf(const std::string & child, double z = 0.0)
+	{
+		geometry_msgs::msg::TransformStamped tf = opticalTransform(child, z);
+		tf.header.stamp = helper()->now();
+		publishStaticTf(tf);
+	}
+
+	/// Latches @p tf as a static transform.
+	void publishStaticTf(const geometry_msgs::msg::TransformStamped & tf)
+	{
+		if(!staticTf_)
+		{
+			staticTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		}
+		staticTf_->sendTransform(tf);
+		spinFor(std::chrono::milliseconds(100));
+	}
+
+	/// Publishes one transform on /tf, as a moving odometry source does.
+	void publishTf(const geometry_msgs::msg::TransformStamped & tf)
+	{
+		if(!tfPub_)
+		{
+			tfPub_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+			// The node's listener and nothing else: publishing before it is matched loses
+			// the transform.
+			waitForSubscriber(tfPub_);
+		}
+		tf2_msgs::msg::TFMessage msg;
+		msg.transforms.push_back(tf);
+		tfPub_->publish(msg);
+	}
+
+	/**
+	 * @brief Publishes one odometry update the way an odometry node does: TF, then topic.
+	 *
+	 * The node looks odom -> base_link up in TF at the message's stamp and prefers it to
+	 * the pose in the message, so the two are published together and agree.
+	 */
+	void sendOdom(
+			const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
+			double stamp, double x, double y = 0.0, double yaw = 0.0,
+			double variance = 0.001)
+	{
+		publishTf(makeTransform("odom", "base_link", stamp, x, y, yaw));
+		pub->publish(makeOdometry(stamp, x, y, yaw, variance));
+	}
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPublisher()
+	{
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub =
+				helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+		return pub;
+	}
+
+	/// Subscribes to `info`, which the node publishes once per processed update.
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> collectInfo()
+	{
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info =
+				collect<rtabmap_msgs::msg::Info>("info");
+		EXPECT_TRUE(waitForPublisher(info->subscription));
+		return info;
+	}
+
+	/**
+	 * @brief Sends @p count odometry updates @p step meters apart along x, one second apart.
+	 *
+	 * Waits for each to come out on @p info before sending the next, so the node never has
+	 * one queued while it is still processing the previous: the processing timer only
+	 * takes a new update once the last one is done, and drops what arrives in between.
+	 */
+	void driveStraight(
+			const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & pub,
+			const std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> & info,
+			int count, double step = 0.5, double firstStamp = 1.0, double firstX = 0.0)
+	{
+		for(int i=0; i<count; ++i)
+		{
+			const size_t before = info->size();
+			sendOdom(pub, firstStamp + double(i), firstX + step*double(i));
+			ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
+					<< "update " << i << " was not processed";
+		}
+	}
+
+	/// Calls @p service on the node and returns its response, or null if it never came.
+	template <typename SrvT>
+	typename SrvT::Response::SharedPtr call(
+			const std::string & service,
+			typename SrvT::Request::SharedPtr request = std::make_shared<typename SrvT::Request>(),
+			std::chrono::milliseconds timeout = std::chrono::milliseconds(10000))
+	{
+		// The node advertises its services under its own name: /rtabmap/reset, not /reset.
+		typename rclcpp::Client<SrvT>::SharedPtr client =
+				helper()->create_client<SrvT>("/rtabmap/" + service);
+		if(!spinUntil([&]() { return client->service_is_ready(); }))
+		{
+			return typename SrvT::Response::SharedPtr();
+		}
+		auto future = client->async_send_request(request).future.share();
+		if(!spinUntil([&]() {
+				return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; },
+				timeout))
+		{
+			return typename SrvT::Response::SharedPtr();
+		}
+		return future.get();
+	}
+
+	bool callEmpty(const std::string & service)
+	{
+		return call<std_srvs::srv::Empty>(service).get() != nullptr;
+	}
+
+	/// The whole graph, as `get_map_data` returns it, graph only.
+	rtabmap_msgs::msg::MapData getGraph(bool global = true, bool optimized = true)
+	{
+		rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+		req->global_map = global;
+		req->optimized = optimized;
+		req->graph_only = true;
+		rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		return res ? res->data : rtabmap_msgs::msg::MapData();
+	}
+
+	/// Node @p id with everything it stores; its `id` is 0 if the node does not exist.
+	rtabmap_msgs::msg::Node getNode(int id)
+	{
+		rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
+		req->ids.push_back(id);
+		req->images = true;
+		req->scan = true;
+		req->grid = true;
+		req->user_data = true;
+		rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		return res && !res->data.empty() ? res->data.front() : rtabmap_msgs::msg::Node();
+	}
+
+	/// The map ids of every node in the graph, in node id order.
+	std::vector<int> mapIds()
+	{
+		std::vector<int> ids;
+		rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+		req->global_map = true;
+		req->optimized = false;
+		req->graph_only = false;
+		rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
+		if(res)
+		{
+			for(const rtabmap_msgs::msg::Node & n : res->data.nodes)
+			{
+				ids.push_back(n.map_id);
+			}
+		}
+		return ids;
+	}
+
+	/// The value of RTAB-Map statistic @p key in @p info, or @p fallback if absent.
+	static float stat(const rtabmap_msgs::msg::Info & info, const std::string & key,
+			float fallback = -1.0f)
+	{
+		for(size_t i=0; i<info.stats_keys.size(); ++i)
+		{
+			if(info.stats_keys[i] == key)
+			{
+				return info.stats_values[i];
+			}
+		}
+		return fallback;
+	}
+
+	std::shared_ptr<rtabmap_slam::CoreWrapper> node_;
+
+private:
+	/**
+	 * Stops the executor of makeMultiThreadedNode(), if any. After a failure, a callback may
+	 * be blocked for good on a leaked lock, and joining would hang the binary instead of
+	 * reporting it: the thread and the node are then abandoned, to die with the process.
+	 */
+	void stopNodeThreads()
+	{
+		if(!nodeExecutor_)
+		{
+			return;
+		}
+		nodeExecutor_->cancel();
+		if(HasFailure())
+		{
+			nodeThread_.detach();
+			new std::shared_ptr<rtabmap_slam::CoreWrapper>(node_);   // never destroyed
+			new std::shared_ptr<rclcpp::executors::MultiThreadedExecutor>(nodeExecutor_);
+		}
+		else
+		{
+			nodeThread_.join();
+			nodeExecutor_->remove_node(node_);
+		}
+		nodeExecutor_.reset();
+	}
+
+	rclcpp::executors::MultiThreadedExecutor::SharedPtr nodeExecutor_;
+	std::thread nodeThread_;
+
+	static void removeDir(const std::string & dir)
+	{
+		UDirectory d(dir);
+		for(std::string f = d.getNextFilePath(); !f.empty(); f = d.getNextFilePath())
+		{
+			UFile::erase(f);
+		}
+		UDirectory::removeDir(dir);
+	}
+
+	std::string dir_;
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> staticTf_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
+};
+
+}  // namespace rtabmap_slam_test
+
+#endif /* RTABMAP_SLAM_CORE_WRAPPER_FIXTURE_HPP_ */
diff --git a/rtabmap_slam/test/msg_builders.hpp b/rtabmap_slam/test/msg_builders.hpp
new file mode 100644
index 00000000..88707647
--- /dev/null
+++ b/rtabmap_slam/test/msg_builders.hpp
@@ -0,0 +1,348 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_SLAM_MSG_BUILDERS_HPP_
+#define RTABMAP_SLAM_MSG_BUILDERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <rtabmap_msgs/msg/landmark_detection.hpp>
+#include <rtabmap_msgs/msg/user_data.hpp>
+
+#include <opencv2/core/core.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_transforms.h>
+#ifdef PRE_ROS_IRON
+#include <cv_bridge/cv_bridge.h>
+#else
+#include <cv_bridge/cv_bridge.hpp>
+#endif
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <string>
+#include <vector>
+
+namespace rtabmap_slam_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 planar pose as a TF: @p x, @p y in meters and @p yaw in radians.
+inline geometry_msgs::msg::TransformStamped makeTransform(
+		const std::string & parent, const std::string & child, double stamp,
+		double x = 0.0, double y = 0.0, double yaw = 0.0)
+{
+	geometry_msgs::msg::TransformStamped tf;
+	tf.header.frame_id = parent;
+	tf.header.stamp = stampOf(stamp);
+	tf.child_frame_id = child;
+	tf.transform.translation.x = x;
+	tf.transform.translation.y = y;
+	tf.transform.rotation.z = std::sin(yaw/2.0);
+	tf.transform.rotation.w = std::cos(yaw/2.0);
+	return tf;
+}
+
+/**
+ * @brief An odometry message at (@p x, @p y, @p yaw), with a small valid covariance.
+ *
+ * The covariance matters to rtabmap: 9999 on both diagonals, or an identity pose after a
+ * non-identity one, is read as an odometry reset and starts a new map.
+ */
+inline nav_msgs::msg::Odometry makeOdometry(
+		double stamp, double x = 0.0, double y = 0.0, double yaw = 0.0,
+		double variance = 0.001,
+		const std::string & frameId = "odom", 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.position.y = y;
+	msg.pose.pose.orientation.z = std::sin(yaw/2.0);
+	msg.pose.pose.orientation.w = std::cos(yaw/2.0);
+	for(int i=0; i<6; ++i)
+	{
+		msg.pose.covariance[i*7] = variance;
+		msg.twist.covariance[i*7] = variance;
+	}
+	return msg;
+}
+
+/// What an odometry node publishes when it is lost or has just been reset.
+inline nav_msgs::msg::Odometry makeResetOdometry(double stamp)
+{
+	nav_msgs::msg::Odometry msg = makeOdometry(stamp, 0.0, 0.0, 0.0, 9999.0);
+	return msg;
+}
+
+/**
+ * @name The room the tests' robot drives in
+ *
+ * A rectangle fixed in the world (the odom and map frames coincide in these tests), with
+ * walls 1 m high. Scans and clouds are generated from the sensor's actual pose in it, so
+ * every node sees the same walls wherever the robot is -- which is what makes the
+ * assembled map, and the occupancy grid in particular, comparable to the room.
+ * @{
+ */
+constexpr double kRoomXMin = -1.5;
+constexpr double kRoomXMax = 3.5;
+constexpr double kRoomYMin = -2.0;
+constexpr double kRoomYMax = 2.0;
+constexpr double kRoomHeight = 1.0;
+
+/// Distance from (@p x, @p y), inside the room, to its walls along direction @p theta.
+inline double rayToRoom(double x, double y, double theta)
+{
+	const double dx = std::cos(theta);
+	const double dy = std::sin(theta);
+	double t = std::numeric_limits<double>::infinity();
+	if(dx > 1e-9) { t = std::min(t, (kRoomXMax - x) / dx); }
+	else if(dx < -1e-9) { t = std::min(t, (kRoomXMin - x) / dx); }
+	if(dy > 1e-9) { t = std::min(t, (kRoomYMax - y) / dy); }
+	else if(dy < -1e-9) { t = std::min(t, (kRoomYMin - y) / dy); }
+	return t;
+}
+
+/// A 360 degree LaserScan of the room from a laser at (@p x, @p y, @p yaw) in the world.
+inline sensor_msgs::msg::LaserScan makeRoomScan(
+		const std::string & frameId, double stamp,
+		double x, double y = 0.0, double yaw = 0.0, size_t count = 720)
+{
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = frameId;
+	scan.header.stamp = stampOf(stamp);
+	scan.angle_increment = float(2.0 * M_PI / double(count));
+	scan.angle_min = float(-M_PI);
+	scan.angle_max = scan.angle_min + scan.angle_increment * float(count - 1);
+	scan.time_increment = 0.0f;
+	scan.scan_time = 0.1f;
+	scan.range_min = 0.1f;
+	scan.range_max = 10.0f;
+	scan.ranges.resize(count);
+	for(size_t i=0; i<count; ++i)
+	{
+		scan.ranges[i] = float(rayToRoom(x, y, yaw + scan.angle_min + scan.angle_increment * double(i)));
+	}
+	return scan;
+}
+
+/**
+ * The room as a 3D scan in the world frame: its walls, floor to top, every 0.05 m along
+ * them and every 0.1 m up, dense enough for the grid's obstacle clustering
+ * (Grid/ClusterRadius); and its floor every 0.025 m, so that each 0.05 m grid cell gets
+ * ground points. Built once: only its pose relative to the sensor changes.
+ */
+inline const rtabmap::LaserScan & roomPoints()
+{
+	static const rtabmap::LaserScan room = []() {
+		const double step = 0.05;
+		std::vector<cv::Vec3f> points;
+		for(double fx=kRoomXMin + 0.025; fx<kRoomXMax - 1e-6; fx+=0.025)
+		{
+			for(double fy=kRoomYMin + 0.025; fy<kRoomYMax - 1e-6; fy+=0.025)
+			{
+				points.push_back(cv::Vec3f(float(fx), float(fy), 0.0f));
+			}
+		}
+		for(double h=0.0; h<=kRoomHeight + 1e-6; h+=0.1)
+		{
+			for(double t=kRoomXMin; t<=kRoomXMax + 1e-6; t+=step)
+			{
+				points.push_back(cv::Vec3f(float(t), float(kRoomYMin), float(h)));
+				points.push_back(cv::Vec3f(float(t), float(kRoomYMax), float(h)));
+			}
+			for(double t=kRoomYMin + step; t<kRoomYMax - 1e-6; t+=step)
+			{
+				points.push_back(cv::Vec3f(float(kRoomXMin), float(t), float(h)));
+				points.push_back(cv::Vec3f(float(kRoomXMax), float(t), float(h)));
+			}
+		}
+		return rtabmap::LaserScan(cv::Mat(points, true).reshape(3, 1), int(points.size()),
+				0.0f, rtabmap::LaserScan::kXYZ);
+	}();
+	return room;
+}
+
+/**
+ * The room as seen by a 3D lidar at (@p x, @p y, @p z) in the world, facing @p yaw, in the
+ * lidar's frame. Every point of the room is visible from anywhere inside it, so moving the
+ * room is all it takes -- unlike a 2D LaserScan message, whose ranges are per angle from
+ * the sensor and are ray-cast again from each pose by makeRoomScan().
+ */
+inline rtabmap::LaserScan roomScan3d(double x, double y = 0.0, double z = 0.0, double yaw = 0.0)
+{
+	return rtabmap::util3d::transformLaserScan(
+			roomPoints(), rtabmap::Transform(x, y, z, 0, 0, yaw).inverse());
+}
+
+/// @p scan as the PointCloud2 a 3D lidar driver would publish.
+inline sensor_msgs::msg::PointCloud2 makeCloud(
+		const std::string & frameId, double stamp, const rtabmap::LaserScan & scan)
+{
+	sensor_msgs::msg::PointCloud2 msg;
+	pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(scan), msg);
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	return msg;
+}
+/** @} */
+
+/// A rectified pinhole CameraInfo.
+inline sensor_msgs::msg::CameraInfo makeCameraInfo(
+		const std::string & frameId, double stamp, int width = 64, int height = 48,
+		double fx = 50.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, 0.0, 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 random bgr8 texture: something a feature detector finds corners in.
+inline cv::Mat texturedImage(int width = 64, int height = 48, uint64_t seed = 42)
+{
+	cv::Mat image(height, width, CV_8UC3);
+	cv::RNG rng(seed);
+	rng.fill(image, cv::RNG::UNIFORM, 0, 255);
+	return image;
+}
+
+inline sensor_msgs::msg::Image makeTexturedImage(
+		const std::string & frameId, double stamp, int width = 64, int height = 48,
+		uint64_t seed = 42)
+{
+	return makeImage(frameId, stamp, texturedImage(width, height, seed), "bgr8");
+}
+
+/// A 16UC1 depth image of a flat wall @p millimeters away.
+inline cv::Mat depthImage(int width = 64, int height = 48, uint16_t millimeters = 1500)
+{
+	return cv::Mat(height, width, CV_16UC1, cv::Scalar(millimeters));
+}
+
+inline sensor_msgs::msg::Image makeDepthImage(
+		const std::string & frameId, double stamp, int width = 64, int height = 48,
+		uint16_t millimeters = 1500)
+{
+	return makeImage(frameId, stamp, depthImage(width, height, millimeters), "16UC1");
+}
+
+/// An uncompressed 2x2 user data matrix.
+inline rtabmap_msgs::msg::UserData makeUserData(double stamp, uint8_t first = 1)
+{
+	rtabmap_msgs::msg::UserData msg;
+	msg.header.stamp = stampOf(stamp);
+	msg.rows = 2;
+	msg.cols = 2;
+	msg.type = CV_8UC1;
+	msg.data = {first, 2, 3, 4};
+	return msg;
+}
+
+inline sensor_msgs::msg::NavSatFix makeGpsFix(
+		double stamp, double latitude, double longitude, double altitude = 100.0,
+		double variance = 4.0)
+{
+	sensor_msgs::msg::NavSatFix msg;
+	msg.header.frame_id = "gps";
+	msg.header.stamp = stampOf(stamp);
+	msg.latitude = latitude;
+	msg.longitude = longitude;
+	msg.altitude = altitude;
+	msg.position_covariance = {variance, 0, 0, 0, variance, 0, 0, 0, variance};
+	msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+	return msg;
+}
+
+/// An IMU carrying only an orientation, rolled by @p roll radians.
+inline sensor_msgs::msg::Imu makeImu(
+		const std::string & frameId, double stamp, double roll = 0.0)
+{
+	sensor_msgs::msg::Imu msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.orientation.x = std::sin(roll/2.0);
+	msg.orientation.w = std::cos(roll/2.0);
+	return msg;
+}
+
+/// A landmark (fiducial) detected @p x meters in front of @p frameId.
+inline rtabmap_msgs::msg::LandmarkDetection makeLandmark(
+		const std::string & frameId, double stamp, int id, double x = 1.0)
+{
+	rtabmap_msgs::msg::LandmarkDetection msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.landmark_frame_id = "tag_" + std::to_string(id);
+	msg.id = id;
+	msg.size = 0.1f;
+	msg.pose.pose.position.x = x;
+	msg.pose.pose.orientation.w = 1.0;
+	for(int i=0; i<6; ++i)
+	{
+		msg.pose.covariance[i*7] = 0.01;
+	}
+	return msg;
+}
+
+inline geometry_msgs::msg::PoseWithCovarianceStamped makePoseWithCovariance(
+		const std::string & frameId, double stamp, double x, double y = 0.0,
+		double yaw = 0.0, double variance = 0.01)
+{
+	geometry_msgs::msg::PoseWithCovarianceStamped msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.pose.pose.position.x = x;
+	msg.pose.pose.position.y = y;
+	msg.pose.pose.orientation.z = std::sin(yaw/2.0);
+	msg.pose.pose.orientation.w = std::cos(yaw/2.0);
+	for(int i=0; i<6; ++i)
+	{
+		msg.pose.covariance[i*7] = variance;
+	}
+	return msg;
+}
+
+}  // namespace rtabmap_slam_test
+
+#endif /* RTABMAP_SLAM_MSG_BUILDERS_HPP_ */
diff --git a/rtabmap_slam/test/node_test_utils.hpp b/rtabmap_slam/test/node_test_utils.hpp
new file mode 100644
index 00000000..0563a153
--- /dev/null
+++ b/rtabmap_slam/test/node_test_utils.hpp
@@ -0,0 +1,256 @@
+/*
+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_SLAM_NODE_TEST_UTILS_HPP_
+#define RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace rtabmap_slam_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_slam_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();
+	}
+
+	/**
+	 * @brief Adds a node under test to the shared executor and keeps it alive for the test.
+	 *
+	 * The wait is for tf2_ros, not for anything the test does with the node.
+	 * ~TransformListener cancels its worker's executor and joins it without ordering the
+	 * cancel after the worker reached spin(), so a node dropped microseconds after it was
+	 * built -- which a test that only reads a parameter back does -- hangs the binary for
+	 * good (ros2/geometry2#517). The window is a few instructions wide and nothing here
+	 * can observe that thread, so this buys time instead. Drop it once #752 lands.
+	 */
+	template <typename NodeT>
+	std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
+	{
+		executor_->add_node(node);
+		nodes_.push_back(node);
+		spinFor(std::chrono::milliseconds(50));
+		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(); }
+
+		/**
+		 * @brief The last (first) message received.
+		 *
+		 * A test that reads these without having waited for the topic it is reading --
+		 * having waited for a different one, say -- gets a legible failure rather than a
+		 * segmentation fault: std::vector::back() on an empty vector dereferences
+		 * nullptr-1, which crashes the whole binary and takes the rest of its tests with
+		 * it. gtest turns the exception into a failure of the test that threw it.
+		 */
+		const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
+		const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
+
+	private:
+		const typename MsgT::ConstSharedPtr & checked(
+				const typename MsgT::ConstSharedPtr * msg) const
+		{
+			if(msg == 0)
+			{
+				throw std::out_of_range(
+						std::string("nothing was received on \"") +
+						(subscription?subscription->get_topic_name():"?") +
+						"\", so there is no message to read: wait for it to arrive first");
+			}
+			return *msg;
+		}
+	};
+
+	/**
+	 * @brief Subscribes the helper node to @p topic and records everything it receives.
+	 *
+	 * The callback holds the collector weakly. Capturing it by shared_ptr would close a
+	 * cycle -- collector owns the subscription, the subscription owns the callback, the
+	 * callback owns the collector -- and neither would ever be freed. A subscription that
+	 * outlives its test keeps the helper node's rcl handle alive with it, which leaves the
+	 * node's rosout publisher registered and greets the next test with "Publisher already
+	 * registered for node name: 'rtabmap_slam_test_helper'".
+	 */
+	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>>();
+		std::weak_ptr<Collector<MsgT>> weak = collector;
+		collector->subscription = helper_->create_subscription<MsgT>(
+				topic, qos,
+				[weak](const typename MsgT::ConstSharedPtr msg) {
+					if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
+					{
+						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_slam_test
+
+#endif /* RTABMAP_SLAM_NODE_TEST_UTILS_HPP_ */
diff --git a/rtabmap_slam/test/test_core_wrapper_inputs.cpp b/rtabmap_slam/test/test_core_wrapper_inputs.cpp
new file mode 100644
index 00000000..2f09c2dd
--- /dev/null
+++ b/rtabmap_slam/test/test_core_wrapper_inputs.cpp
@@ -0,0 +1,1551 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <sstream>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/srv/get_map.hpp>
+
+#include <rtabmap_msgs/msg/env_sensor.hpp>
+#include <rtabmap_msgs/msg/odom_info.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/rgbd_images.hpp>
+#include <rtabmap_msgs/msg/sensor_data.hpp>
+
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_registration.h>
+#include <rtabmap/core/Parameters.h>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include "core_wrapper_fixture.hpp"
+
+namespace rtabmap_slam_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+using rtabmap::Parameters;
+
+class CoreWrapperInputsTest : public CoreWrapperTest
+{
+protected:
+	/// Whether @p graph has a link of @p type between @p from and @p to, either way.
+	static bool hasLink(const rtabmap_msgs::msg::MapGraph & graph, int type, int from, int to)
+	{
+		for(const rtabmap_msgs::msg::Link & l : graph.links)
+		{
+			if(l.type == type && ((l.from_id == from && l.to_id == to) ||
+								  (l.from_id == to && l.to_id == from)))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	/// The link of @p type between @p from and @p to in @p graph, either way, or null.
+	static const rtabmap_msgs::msg::Link * findLink(
+			const rtabmap_msgs::msg::MapGraph & graph, int type, int from, int to)
+	{
+		for(const rtabmap_msgs::msg::Link & l : graph.links)
+		{
+			if(l.type == type && ((l.from_id == from && l.to_id == to) ||
+								  (l.from_id == to && l.to_id == from)))
+			{
+				return &l;
+			}
+		}
+		return nullptr;
+	}
+
+	static bool hasPose(const rtabmap_msgs::msg::MapGraph & graph, int id)
+	{
+		return std::find(graph.poses_id.begin(), graph.poses_id.end(), id) != graph.poses_id.end();
+	}
+
+	/// The occupancy at (@p x, @p y) in the map frame: 100, 0, or -1 if unknown or off the grid.
+	static int occupancyAt(const nav_msgs::msg::OccupancyGrid & grid, double x, double y)
+	{
+		const int cx = int(std::floor((x - grid.info.origin.position.x) / grid.info.resolution));
+		const int cy = int(std::floor((y - grid.info.origin.position.y) / grid.info.resolution));
+		if(cx < 0 || cy < 0 || cx >= int(grid.info.width) || cy >= int(grid.info.height))
+		{
+			return -1;
+		}
+		return grid.data[size_t(cy) * grid.info.width + cx];
+	}
+
+	/// The highest occupancy within one cell of (@p x, @p y): a wall falls on a cell boundary.
+	static int occupancyAround(const nav_msgs::msg::OccupancyGrid & grid, double x, double y)
+	{
+		int best = -1;
+		for(int dx=-1; dx<=1; ++dx)
+		{
+			for(int dy=-1; dy<=1; ++dy)
+			{
+				best = std::max(best, occupancyAt(grid, x + dx*grid.info.resolution, y + dy*grid.info.resolution));
+			}
+		}
+		return best;
+	}
+
+	static std::string where(double x, double y)
+	{
+		std::stringstream s;
+		s << "(" << x << ", " << y << ")";
+		return s.str();
+	}
+
+	/**
+	 * @brief Checks @p grid against the room, cell by cell, in the map frame.
+	 *
+	 * Along the walls, 0.3 m clear of the corners: occupied. Inside, 0.3 m clear of the
+	 * walls: free -- walls from scans that did not move with the robot would show up here
+	 * as occupied cells. 0.5 m outside: unknown.
+	 */
+	static void expectRoomGrid(const nav_msgs::msg::OccupancyGrid & grid)
+	{
+		ASSERT_GT(grid.info.resolution, 0.0f);
+		ASSERT_EQ(size_t(grid.info.width) * grid.info.height, grid.data.size());
+
+		int missingWalls = 0;
+		std::string firstMissing;
+		const auto checkWall = [&](double x, double y) {
+			if(occupancyAround(grid, x, y) != 100)
+			{
+				if(missingWalls++ == 0) { firstMissing = where(x, y); }
+			}
+		};
+		for(double t=kRoomXMin + 0.3; t<=kRoomXMax - 0.3; t+=0.1)
+		{
+			checkWall(t, kRoomYMin);
+			checkWall(t, kRoomYMax);
+		}
+		for(double t=kRoomYMin + 0.3; t<=kRoomYMax - 0.3; t+=0.1)
+		{
+			checkWall(kRoomXMin, t);
+			checkWall(kRoomXMax, t);
+		}
+		EXPECT_EQ(0, missingWalls) << "wall cells not occupied, the first at " << firstMissing;
+
+		int wrongInside = 0;
+		std::string firstWrong;
+		int firstValue = 0;
+		for(double x=kRoomXMin + 0.3; x<=kRoomXMax - 0.3; x+=0.1)
+		{
+			for(double y=kRoomYMin + 0.3; y<=kRoomYMax - 0.3; y+=0.1)
+			{
+				const int v = occupancyAt(grid, x, y);
+				if(v != 0)
+				{
+					if(wrongInside++ == 0) { firstWrong = where(x, y); firstValue = v; }
+				}
+			}
+		}
+		EXPECT_EQ(0, wrongInside) << "cells inside the room not free, the first at "
+				<< firstWrong << " = " << firstValue;
+
+		EXPECT_EQ(-1, occupancyAt(grid, kRoomXMax + 0.5, 0.0));
+		EXPECT_EQ(-1, occupancyAt(grid, kRoomXMin - 0.5, 0.0));
+		EXPECT_EQ(-1, occupancyAt(grid, 1.0, kRoomYMax + 0.5));
+		EXPECT_EQ(-1, occupancyAt(grid, 1.0, kRoomYMin - 0.5));
+	}
+
+	/**
+	 * @brief Sends @p count scans of the room with matching odometry, @p step meters apart
+	 *        along x, from a laser @p laserX meters ahead of base_link.
+	 */
+	void driveWithScans(
+			const rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr & odom,
+			const rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr & scan,
+			const std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> & info,
+			int count, double laserX, double step = 0.5)
+	{
+		for(int i=0; i<count; ++i)
+		{
+			const size_t before = info->size();
+			const double stamp = 1.0 + double(i);
+			if(odom)
+			{
+				sendOdom(odom, stamp, step*i);
+			}
+			else
+			{
+				publishTf(makeTransform("odom", "base_link", stamp, step*i));
+			}
+			scan->publish(makeRoomScan("laser", stamp, step*i + laserX));
+			ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
+					<< "scan " << i << " was not processed";
+		}
+	}
+};
+
+//==========================================================================================
+// Sensor inputs, synchronized with odometry
+//==========================================================================================
+
+/**
+ * A 2D lidar: each node stores its scan, and the occupancy grid is built from the scans.
+ * The scan is converted into base_link, so the lidar must be in TF. Driven across the
+ * room, the grid is the room: occupied walls, free inside, unknown beyond.
+ */
+TEST_F(CoreWrapperInputsTest, maps_a_laser_scan)
+{
+	publishStaticTf("laser", 0.1);
+	makeNode({rclcpp::Parameter("subscribe_scan", true)});
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collect<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().transient_local());
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(scan));
+	ASSERT_TRUE(waitForPublisher(grid->subscription));
+
+	driveWithScans(odom, scan, info, 3, 0.1);
+
+	rtabmap_msgs::msg::Node node = getNode(1);
+	EXPECT_FALSE(node.data.laser_scan_compressed.empty());
+	EXPECT_NEAR(0.1, node.data.laser_scan_local_transform.translation.x, 1e-4);
+
+	ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); }));
+	EXPECT_EQ("map", grid->back().header.frame_id);
+	{
+		SCOPED_TRACE("map topic");
+		expectRoomGrid(grid->back());
+	}
+
+	nav_msgs::srv::GetMap::Response::SharedPtr map = call<nav_msgs::srv::GetMap>("get_map");
+	ASSERT_TRUE(map.get() != nullptr);
+	{
+		SCOPED_TRACE("get_map");
+		expectRoomGrid(map->map);
+	}
+	nav_msgs::srv::GetMap::Response::SharedPtr prob = call<nav_msgs::srv::GetMap>("get_prob_map");
+	ASSERT_TRUE(prob.get() != nullptr);
+	EXPECT_GT(prob->map.info.width, 0u);
+}
+
+/**
+ * odom_sensor_sync, on by default, deskews scans through the odometry frame in TF. With
+ * odometry published only as a topic, there is no such frame: the scans are then used as
+ * they are, with a warning, rather than refused.
+ */
+TEST_F(CoreWrapperInputsTest, maps_scans_as_they_are_without_odometry_on_tf)
+{
+	publishStaticTf("laser", 0.1);
+	makeNode({rclcpp::Parameter("subscribe_scan", true)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(scan));
+
+	for(int i=0; i<3; ++i)
+	{
+		const size_t before = info->size();
+		odom->publish(makeOdometry(1.0 + i, 0.5*i));   // on the topic, not on TF
+		scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i + 0.1));
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; })) << "scan " << i << " was dropped";
+	}
+	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
+}
+
+/// A scan whose frame is not in TF cannot be placed on the robot and is dropped.
+TEST_F(CoreWrapperInputsTest, drops_a_scan_without_its_tf)
+{
+	makeNode({rclcpp::Parameter("subscribe_scan", true),
+			  rclcpp::Parameter("wait_for_transform", 0.05)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(scan));
+
+	sendOdom(odom, 1.0, 0.0);
+	scan->publish(makeRoomScan("laser", 1.0, 0.0));
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(info->empty());
+}
+
+/**
+ * A scan that cannot be converted is dropped, but must not block the next ones: once the
+ * lidar's TF is there, the following scans are mapped. The conversion failure used to
+ * return with the synchronization mutex still locked, and every later update was then
+ * silently skipped. The node runs on a multi-threaded executor, as in the `rtabmap`
+ * executable: on a single thread, the recursive mutex would just be taken again.
+ */
+TEST_F(CoreWrapperInputsTest, maps_the_next_scans_after_one_without_its_tf)
+{
+	makeMultiThreadedNode({rclcpp::Parameter("subscribe_scan", true),
+			  rclcpp::Parameter("wait_for_transform", 0.05)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(scan));
+
+	sendOdom(odom, 1.0, 0.0);
+	scan->publish(makeRoomScan("laser", 1.0, 0.0));
+	spinFor(std::chrono::milliseconds(500));
+	ASSERT_TRUE(info->empty()) << "the scan without TF should have been dropped";
+
+	publishStaticTf("laser");
+	for(int i=1; i<=3; ++i)
+	{
+		const size_t before = info->size();
+		sendOdom(odom, 1.0 + i, 0.5*i);
+		scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i));
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
+				<< "scan " << i << " was not processed after the one without TF";
+	}
+	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
+}
+
+/// The same with a 3D lidar, whose conversion fails the same way without its TF.
+TEST_F(CoreWrapperInputsTest, maps_the_next_clouds_after_one_without_its_tf)
+{
+	makeMultiThreadedNode({rclcpp::Parameter("subscribe_scan_cloud", true),
+			  rclcpp::Parameter("wait_for_transform", 0.05)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(cloud));
+
+	sendOdom(odom, 1.0, 0.0);
+	cloud->publish(makeCloud("lidar", 1.0, roomScan3d(0.0, 0.0, 0.5)));
+	spinFor(std::chrono::milliseconds(500));
+	ASSERT_TRUE(info->empty()) << "the cloud without TF should have been dropped";
+
+	publishStaticTf("lidar", 0.0, 0.0, 0.5);
+	for(int i=1; i<=3; ++i)
+	{
+		const size_t before = info->size();
+		sendOdom(odom, 1.0 + i, 0.5*i);
+		cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5)));
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
+				<< "cloud " << i << " was not processed after the one without TF";
+	}
+	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
+}
+
+/**
+ * With odom_frame_id set, odometry is read from TF at each scan's stamp instead of from a
+ * topic, and subscribe_odom is ignored.
+ */
+TEST_F(CoreWrapperInputsTest, reads_odometry_from_tf_with_odom_frame_id)
+{
+	publishStaticTf("laser");
+	makeNode({rclcpp::Parameter("subscribe_scan", true),
+			  rclcpp::Parameter("odom_frame_id", "odom")});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+	ASSERT_TRUE(waitForSubscriber(scan));
+	EXPECT_EQ(0u, helper()->count_publishers("odom") + helper()->count_subscribers("odom"));
+
+	driveWithScans(nullptr, scan, info, 3, 0.0);
+
+	rtabmap_msgs::msg::MapData map = getGraph();
+	ASSERT_EQ(3u, map.graph.poses.size());
+	EXPECT_NEAR(1.0, map.graph.poses[2].position.x, 1e-4);
+}
+
+/// A 3D lidar: each node stores its cloud, and the grid is built from it.
+TEST_F(CoreWrapperInputsTest, maps_a_point_cloud)
+{
+	publishStaticTf("lidar", 0.0, 0.0, 0.5);
+	makeNode({rclcpp::Parameter("subscribe_scan_cloud", true)});
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collect<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().transient_local());
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(cloud));
+	ASSERT_TRUE(waitForPublisher(grid->subscription));
+
+	for(int i=0; i<3; ++i)
+	{
+		const size_t before = info->size();
+		sendOdom(odom, 1.0 + i, 0.5*i);
+		cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5)));
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
+	}
+
+	rtabmap_msgs::msg::Node node = getNode(1);
+	EXPECT_FALSE(node.data.laser_scan_compressed.empty());
+	EXPECT_NEAR(0.5, node.data.laser_scan_local_transform.translation.z, 1e-4);
+
+	// The walls are obstacles and the floor is ground, which the 2D grid shows as free:
+	// the same grid as from the 2D lidar.
+	ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); }));
+	expectRoomGrid(grid->back());
+}
+
+/**
+ * An RGB-D camera, through rtabmap_msgs/RGBDImage as rgbd_sync publishes it: each node
+ * stores the images and the calibration.
+ */
+TEST_F(CoreWrapperInputsTest, maps_an_rgbd_image)
+{
+	publishStaticTf("camera");
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(rgbd));
+
+	for(int i=0; i<2; ++i)
+	{
+		const double stamp = 1.0 + i;
+		rtabmap_msgs::msg::RGBDImage msg;
+		msg.header.frame_id = "camera";
+		msg.header.stamp = stampOf(stamp);
+		msg.rgb = makeTexturedImage("camera", stamp, 64, 48, 42 + i);
+		msg.depth = makeDepthImage("camera", stamp);
+		msg.rgb_camera_info = makeCameraInfo("camera", stamp);
+		msg.depth_camera_info = makeCameraInfo("camera", stamp);
+		const size_t before = info->size();
+		sendOdom(odom, stamp, 0.5*i);
+		rgbd->publish(msg);
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
+	}
+
+	rtabmap_msgs::msg::Node node = getNode(1);
+	EXPECT_FALSE(node.data.left_compressed.empty());
+	EXPECT_FALSE(node.data.right_compressed.empty());
+	ASSERT_EQ(1u, node.data.left_camera_info.size());
+	EXPECT_EQ(64u, node.data.left_camera_info[0].width);
+}
+
+/**
+ * gen_scan makes a 2D scan out of the depth image, for a robot with a depth camera and no
+ * lidar, so the grid can be built the way it would be from a lidar. Given the depth image
+ * of a known scan, the scan it makes is that scan: every point of it within 1 cm of the
+ * original.
+ */
+TEST_F(CoreWrapperInputsTest, gen_scan_derives_a_scan_from_depth)
+{
+	// A known scan of the room from the camera's position -- the robot is at the origin
+	// facing +x, the camera on it, 0.3 m up, looking forward -- projected into a depth
+	// image with util3d::projectCloudToCamera(). The scan is at the camera's height, so it
+	// lands on the middle row, the one gen_scan reads back.
+	const sensor_msgs::msg::LaserScan known = makeRoomScan("camera", 1.0, 0.0, 0.0, 0.0, 14400);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr knownCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	for(size_t i=0; i<known.ranges.size(); ++i)
+	{
+		const double a = known.angle_min + known.angle_increment * double(i);
+		knownCloud->push_back(pcl::PointXYZ(float(known.ranges[i] * std::cos(a)), float(known.ranges[i] * std::sin(a)), 0.0f));
+	}
+
+	// A projected point lands up to a pixel off its column's center ray, where gen_scan puts
+	// it back, so the focal length bounds the error: depth / fx, 5.5 mm on the far wall.
+	// Only the middle row matters: a wide, short image, 90 degrees across -- the far wall
+	// and both sides.
+	const int width = 1280, height = 20;
+	const double fx = 640.0;
+	const cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0);
+	const cv::Mat depth = rtabmap::util3d::projectCloudToCamera(
+			cv::Size(width, height), K, knownCloud,
+			rtabmap_conversions::transformFromGeometryMsg(opticalTransform("camera_optical").transform));
+	const int filled = cv::countNonZero(depth.row(height/2));
+	ASSERT_EQ(width, filled) << "some columns of the middle row got no point";
+
+	publishOpticalTf("camera_optical", 0.3);
+	makeNode({rclcpp::Parameter("subscribe_rgbd", true),
+			  rclcpp::Parameter("gen_scan", true),
+			  rclcpp::Parameter("gen_scan_max_depth", 0.0)});   // the far wall is 3.5 m away and beyond
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(rgbd));
+
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.header.frame_id = "camera_optical";
+	msg.header.stamp = stampOf(1.0);
+	msg.rgb = makeTexturedImage("camera_optical", 1.0, width, height);
+	msg.depth = makeImage("camera_optical", 1.0, depth, "32FC1");
+	msg.rgb_camera_info = makeCameraInfo("camera_optical", 1.0, width, height, fx);
+	msg.depth_camera_info = msg.rgb_camera_info;
+	sendOdom(odom, 1.0, 0.0);
+	rgbd->publish(msg);
+	ASSERT_TRUE(spinUntil([&]() { return !info->empty(); }));
+
+	// The generated scan, in base_link, against the known one: every point within 1 cm.
+	rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(getNode(1).data);
+	rtabmap::LaserScan scan;
+	data.uncompressData(0, 0, &scan);
+	ASSERT_FALSE(scan.isEmpty());
+	EXPECT_TRUE(scan.is2d());
+	EXPECT_EQ(filled, scan.size()) << "one point per column of the middle row";
+	pcl::PointCloud<pcl::PointXYZ>::Ptr generated = rtabmap::util3d::laserScanToPointCloud(scan, scan.localTransform());
+	double variance = 0.0;
+	int correspondences = 0;
+	rtabmap::util3d::computeVarianceAndCorrespondences(
+			generated, knownCloud, 0.01, variance, correspondences, false);
+	EXPECT_EQ(int(generated->size()), correspondences) << "generated points farther than 1 cm from the known scan";
+}
+
+/**
+ * rtabmap_msgs/SensorData carries everything a node holds in one message, the way the
+ * odometry nodes republish what they processed.
+ */
+TEST_F(CoreWrapperInputsTest, maps_sensor_data)
+{
+	makeNode({rclcpp::Parameter("subscribe_sensor_data", true)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
+			helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
+	ASSERT_TRUE(waitForSubscriber(data));
+
+	for(int i=0; i<2; ++i)
+	{
+		const double stamp = 1.0 + i;
+		rtabmap_msgs::msg::SensorData msg;
+		msg.header.frame_id = "base_link";
+		msg.header.stamp = stampOf(stamp);
+		msg.user_data = {1, 2, 3};   // opaque to the node, carried through as is
+		const size_t before = info->size();
+		sendOdom(odom, stamp, 0.5*i);
+		data->publish(msg);
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
+	}
+
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+}
+
+//==========================================================================================
+// Synchronization of sensors that are not stamped together
+//==========================================================================================
+
+/**
+ * A robot with odometry, a 2D lidar and four cameras, none of them stamped together, the
+ * way they are on a real robot:
+ *
+ * - odometry at 50 Hz, arriving 5 ms after its stamp, with its TF;
+ * - the lidar at 10 Hz, 7 ms out of phase with the odometry, arriving 40 ms after its stamp;
+ * - the cameras at 30 Hz, each triggered 5 ms after the previous one, packed in one
+ *   rtabmap_msgs/RGBDImages -- each camera keeping its own stamp -- that arrives 60 ms
+ *   after the last one.
+ *
+ * The robot drives an arc, 1 m/s turning at 0.5 rad/s, for two seconds, and every message is
+ * published at its arrival time, in arrival order. Each camera frame's depth is uniform,
+ * 1000 mm plus the frame number, and depth is stored losslessly, so a node's depth images
+ * tell which frame, and so which stamp, each camera contributed. Each odometry message
+ * has a different variance, so a link's information tells which message it came from.
+ */
+class CoreWrapperSyncTest : public CoreWrapperInputsTest
+{
+protected:
+	static constexpr double kStart = 1.0;
+	static constexpr double kDuration = 2.0;
+	static constexpr double kSpeed = 1.0;      // m/s
+	static constexpr double kTurnRate = 0.5;   // rad/s
+	static constexpr int kCameras = 4;
+	static constexpr int kWidth = 160;     // 90 degrees each at kFx: the four cover 360
+	static constexpr int kHeight = 20;     // gen_scan only reads the middle row
+	static constexpr double kFx = 80.0;
+
+	static double odomStamp(int k) { return kStart + 0.02 * k; }
+	static double scanStamp(int j) { return kStart + 0.007 + 0.1 * j; }
+	static double cameraStamp(int frame, int camera) { return kStart + frame / 30.0 + 0.005 * camera; }
+	static double odomVariance(int k) { return 0.001 * (1.0 + k); }
+
+	/// Where the robot is at @p t, in odom (and map): an arc from the origin.
+	static rtabmap::Transform robotPose(double t)
+	{
+		const double yaw = kTurnRate * (t - kStart);
+		return rtabmap::Transform(
+				float(kSpeed / kTurnRate * std::sin(yaw)),
+				float(kSpeed / kTurnRate * (1.0 - std::cos(yaw))),
+				0.0f, 0.0f, 0.0f, float(yaw));
+	}
+
+	static rtabmap::Transform laserMount() { return rtabmap::Transform(0.1f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); }
+
+	/// Camera @p c looks out at c x 90 degrees, 0.2 m from the center, 0.3 m up.
+	static rtabmap::Transform cameraMount(int c)
+	{
+		const double yaw = c * M_PI / 2.0;
+		return rtabmap::Transform(float(0.2 * std::cos(yaw)), float(0.2 * std::sin(yaw)), 0.3f, 0.0f, 0.0f, float(yaw)) *
+				rtabmap::Transform(0, 0, 0, -0.5f, 0.5f, -0.5f, 0.5f); // optical: z forward, x right
+	}
+	static std::string cameraFrame(int c) { return "camera" + std::to_string(c) + "_optical"; }
+
+	/**
+	 * @brief The depth image of the room seen by a level camera at @p camera, an optical
+	 *        frame in the world: each column holds the room's depth along its center ray.
+	 */
+	static cv::Mat renderRoomDepth(const rtabmap::Transform & camera)
+	{
+		const Eigen::Vector3f axis = camera.toEigen3f().rotation() * Eigen::Vector3f::UnitZ();
+		const double axisYaw = std::atan2(axis.y(), axis.x());
+		cv::Mat depth(kHeight, kWidth, CV_16UC1);
+		for(int u=0; u<kWidth; ++u)
+		{
+			// Optical x is to the right: a column right of center looks clockwise.
+			const double angle = std::atan2(-(u - kWidth/2.0), kFx);
+			const double z = rayToRoom(camera.x(), camera.y(), axisYaw + angle) * std::cos(angle);
+			depth.col(u).setTo(cv::Scalar(std::round(z * 1000.0)));
+		}
+		return depth;
+	}
+
+	/**
+	 * @brief Drives the timeline above through a node with @p params on top of the inputs'.
+	 *
+	 * Without @p withScan, there is no lidar, and each camera's depth is the room seen from
+	 * where it is at its own stamp, instead of a frame number.
+	 */
+	void driveUnsynchronized(const std::vector<rclcpp::Parameter> & params, bool withScan = true)
+	{
+		publishStaticTf(makeTransform("base_link", "laser", 0.0, laserMount().x()));
+		for(int c=0; c<kCameras; ++c)
+		{
+			geometry_msgs::msg::TransformStamped tf;
+			tf.header.frame_id = "base_link";
+			tf.child_frame_id = cameraFrame(c);
+			tf.header.stamp = helper()->now();
+			rtabmap_conversions::transformToGeometryMsg(cameraMount(c), tf.transform);
+			publishStaticTf(tf);
+		}
+
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter("subscribe_rgbd", true),
+			rclcpp::Parameter("rgbd_cameras", 0),
+			rclcpp::Parameter("subscribe_scan", withScan),
+			rclcpp::Parameter("approx_sync", true),
+			rclcpp::Parameter("topic_queue_size", 50),
+			rclcpp::Parameter("sync_queue_size", 50),
+			rclcpp::Parameter(Parameters::kKpMaxFeatures(), "-1")};   // what is checked here is not visual
+		all.insert(all.end(), params.begin(), params.end());
+		makeNode(all);
+		info_ = collectInfo();
+		odom_ = odomPublisher();
+		rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+				helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 50);
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbd =
+				helper()->create_publisher<rtabmap_msgs::msg::RGBDImages>("rgbd_images", 50);
+		if(withScan)
+		{
+			ASSERT_TRUE(waitForSubscriber(scan));
+		}
+		ASSERT_TRUE(waitForSubscriber(rgbd));
+
+		enum Type { kOdom, kScan, kImages };
+		struct Event { double arrival; Type type; int index; };
+		std::vector<Event> events;
+		for(int k=0; odomStamp(k) <= kStart + kDuration + 1e-9; ++k) { events.push_back({odomStamp(k) + 0.005, kOdom, k}); }
+		for(int j=0; withScan && scanStamp(j) <= kStart + kDuration + 1e-9; ++j) { events.push_back({scanStamp(j) + 0.040, kScan, j}); }
+		for(int f=0; cameraStamp(f, kCameras-1) <= kStart + kDuration + 1e-9; ++f) { events.push_back({cameraStamp(f, kCameras-1) + 0.060, kImages, f}); }
+		std::stable_sort(events.begin(), events.end(),
+				[](const Event & a, const Event & b) { return a.arrival < b.arrival; });
+
+		for(const Event & e : events)
+		{
+			if(e.type == kOdom)
+			{
+				const rtabmap::Transform pose = robotPose(odomStamp(e.index));
+				float x, y, z, roll, pitch, yaw;
+				pose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+				publishTf(makeTransform("odom", "base_link", odomStamp(e.index), x, y, yaw));
+				odom_->publish(makeOdometry(odomStamp(e.index), x, y, yaw, odomVariance(e.index)));
+			}
+			else if(e.type == kScan)
+			{
+				const rtabmap::Transform laser = robotPose(scanStamp(e.index)) * laserMount();
+				scan->publish(makeRoomScan("laser", scanStamp(e.index), laser.x(), laser.y(), laser.theta()));
+			}
+			else
+			{
+				rtabmap_msgs::msg::RGBDImages msg;
+				msg.header.stamp = stampOf(cameraStamp(e.index, 0));
+				msg.header.frame_id = cameraFrame(0);
+				for(int c=0; c<kCameras; ++c)
+				{
+					const double stamp = cameraStamp(e.index, c);
+					rtabmap_msgs::msg::RGBDImage image;
+					image.header.frame_id = cameraFrame(c);
+					image.header.stamp = stampOf(stamp);
+					image.rgb = makeTexturedImage(cameraFrame(c), stamp, kWidth, kHeight, 100 * e.index + c);
+					image.depth = withScan ?
+							makeDepthImage(cameraFrame(c), stamp, kWidth, kHeight, uint16_t(1000 + e.index)) :
+							makeImage(cameraFrame(c), stamp, renderRoomDepth(robotPose(stamp) * cameraMount(c)), "16UC1");
+					image.rgb_camera_info = makeCameraInfo(cameraFrame(c), stamp, kWidth, kHeight, kFx);
+					image.depth_camera_info = image.rgb_camera_info;
+					msg.rgbd_images.push_back(image);
+				}
+				rgbd->publish(msg);
+			}
+			spinFor(std::chrono::milliseconds(2));
+		}
+		spinFor(std::chrono::milliseconds(500));
+	}
+
+	/// The nodes, with their images and scans, in id order.
+	std::vector<rtabmap_msgs::msg::Node> nodes()
+	{
+		rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+		req->global_map = true;
+		req->optimized = false;
+		req->graph_only = false;
+		rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		return res ? res->data.nodes : std::vector<rtabmap_msgs::msg::Node>();
+	}
+
+	/// The frame each camera of @p data contributed, read back from its depth.
+	static std::vector<int> cameraFrames(const rtabmap::SensorData & data)
+	{
+		cv::Mat depth;
+		rtabmap::SensorData copy = data;
+		copy.uncompressData(0, &depth);
+		std::vector<int> frames;
+		for(int c=0; c<kCameras && !depth.empty(); ++c)
+		{
+			frames.push_back(int(depth.at<uint16_t>(kHeight/2, c*kWidth + kWidth/2)) - 1000);
+		}
+		return frames;
+	}
+
+	/// The farthest any point of any node's generated scan is from the room's walls.
+	double farthestFromTheWalls(const std::vector<rtabmap_msgs::msg::Node> & all, size_t & points)
+	{
+		double worst = 0.0;
+		points = 0;
+		for(const rtabmap_msgs::msg::Node & node : all)
+		{
+			rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data);
+			rtabmap::LaserScan scan;
+			data.uncompressData(0, 0, &scan);
+			EXPECT_FALSE(scan.isEmpty()) << "node " << node.id;
+			const rtabmap::Transform toMap = rtabmap_conversions::transformFromPoseMsg(node.pose) * scan.localTransform();
+			for(int i=0; i<scan.size(); ++i)
+			{
+				const float * p = scan.data().ptr<float>(0, i);
+				const cv::Point3f pt = rtabmap::util3d::transformPoint(cv::Point3f(p[0], p[1], 0.0f), toMap);
+				const double d = std::min(std::min(std::fabs(pt.x - kRoomXMin), std::fabs(pt.x - kRoomXMax)),
+										  std::min(std::fabs(pt.y - kRoomYMin), std::fabs(pt.y - kRoomYMax)));
+				worst = std::max(worst, d);
+				++points;
+			}
+		}
+		return worst;
+	}
+
+	/**
+	 * @brief A 0.1 s sweep of the room from the lidar, each ray measured from where the
+	 *        laser is at that ray's own time while the robot drives its arc.
+	 */
+	static sensor_msgs::msg::LaserScan makeSweepingRoomScan(double stamp, size_t count = 720)
+	{
+		sensor_msgs::msg::LaserScan scan = makeRoomScan("laser", stamp, 0.0, 0.0, 0.0, count);
+		scan.time_increment = float(0.1 / double(count));
+		for(size_t i=0; i<count; ++i)
+		{
+			const rtabmap::Transform laser = robotPose(stamp + scan.time_increment * double(i)) * laserMount();
+			scan.ranges[i] = float(rayToRoom(laser.x(), laser.y(),
+					laser.theta() + scan.angle_min + scan.angle_increment * double(i)));
+		}
+		return scan;
+	}
+
+	/**
+	 * @brief Drives the arc with a sweeping lidar and odometry on the topic and in TF,
+	 *        sampled every 10 ms around each sweep.
+	 */
+	void driveSweepingLidar(const std::vector<rclcpp::Parameter> & params)
+	{
+		publishStaticTf(makeTransform("base_link", "laser", 0.0, laserMount().x()));
+		std::vector<rclcpp::Parameter> all = {rclcpp::Parameter("subscribe_scan", true)};
+		all.insert(all.end(), params.begin(), params.end());
+		makeNode(all);
+		info_ = collectInfo();
+		odom_ = odomPublisher();
+		rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
+				helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+		ASSERT_TRUE(waitForSubscriber(scan));
+
+		for(double stamp = kStart; stamp <= kStart + 1.0 + 1e-9; stamp += 0.2)
+		{
+			for(double t = stamp - 0.02; t <= stamp + 0.12 + 1e-9; t += 0.01)
+			{
+				const rtabmap::Transform pose = robotPose(std::max(t, kStart));
+				publishTf(makeTransform("odom", "base_link", t, pose.x(), pose.y(), pose.theta()));
+			}
+			spinFor(std::chrono::milliseconds(20));
+			const rtabmap::Transform pose = robotPose(stamp);
+			const size_t before = info_->size();
+			odom_->publish(makeOdometry(stamp, pose.x(), pose.y(), pose.theta()));
+			scan->publish(makeSweepingRoomScan(stamp));
+			ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; })) << "scan at " << stamp;
+		}
+	}
+
+	static double angleBetween(const rtabmap::Transform & a, const rtabmap::Transform & b)
+	{
+		const rtabmap::Transform d = a.inverse() * b;
+		return std::fabs(Eigen::AngleAxisd(d.getQuaterniond()).angle());
+	}
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
+};
+
+/**
+ * Each node takes the lidar's stamp, and the odometry interpolated in TF at that stamp --
+ * between two samples, the lidar being out of phase with the odometry. With
+ * odom_sensor_sync, each camera's local transform is moved by the robot's motion between
+ * the lidar's stamp and that camera's, so the images are placed where the robot really
+ * was when each was taken.
+ */
+TEST_F(CoreWrapperSyncTest, places_each_sensor_at_its_own_stamp_with_odom_sensor_sync)
+{
+	driveUnsynchronized({});   // odom_sensor_sync is on by default
+
+	const std::vector<rtabmap_msgs::msg::Node> all = nodes();
+	ASSERT_GE(all.size(), 3u) << "not enough updates made it through";
+	double largestCorrection = 0.0;
+	for(const rtabmap_msgs::msg::Node & node : all)
+	{
+		SCOPED_TRACE("node " + std::to_string(node.id));
+		// The lidar's stamp.
+		const double stamp = node.stamp;
+		const int j = int(std::lround((stamp - kStart - 0.007) / 0.1));
+		EXPECT_NEAR(scanStamp(j), stamp, 1e-6) << "not a lidar stamp";
+
+		// The odometry, interpolated at it.
+		const rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(node.pose);
+		const rtabmap::Transform expected = robotPose(stamp);
+		EXPECT_NEAR(expected.x(), pose.x(), 1e-3);
+		EXPECT_NEAR(expected.y(), pose.y(), 1e-3);
+		EXPECT_NEAR(expected.theta(), pose.theta(), 1e-3);
+
+		rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data);
+		rtabmap::LaserScan scan;
+		data.uncompressData(0, 0, &scan);
+		EXPECT_NEAR(laserMount().x(), scan.localTransform().x(), 1e-4) << "the scan is at the node's stamp";
+
+		// Each camera where the robot was at its own stamp.
+		const std::vector<int> frames = cameraFrames(data);
+		ASSERT_EQ(size_t(kCameras), frames.size());
+		ASSERT_EQ(size_t(kCameras), data.cameraModels().size());
+		for(int c=0; c<kCameras; ++c)
+		{
+			SCOPED_TRACE("camera " + std::to_string(c) + ", frame " + std::to_string(frames[c]));
+			const rtabmap::Transform local = data.cameraModels()[c].localTransform();
+			const rtabmap::Transform expectedLocal =
+					robotPose(stamp).inverse() * robotPose(cameraStamp(frames[c], c)) * cameraMount(c);
+			EXPECT_LT(local.getDistance(expectedLocal), 1e-3);
+			EXPECT_LT(angleBetween(local, expectedLocal), 1e-3);
+			largestCorrection = std::max(largestCorrection, double(cameraMount(c).getDistance(expectedLocal)));
+		}
+	}
+	// Five times the tolerance: the corrections are large enough that skipping them fails.
+	EXPECT_GT(largestCorrection, 0.005) << "the cameras were never far enough from the lidar's stamp to tell";
+
+	// The link to a node carries the variance of the odometry message synchronized with its
+	// lidar scan -- the one nearest the scan's stamp -- not of the last one received, which
+	// is 40 ms of odometry later by the time the scan arrives.
+	std::map<int, double> stamps;
+	for(const rtabmap_msgs::msg::Node & node : all)
+	{
+		stamps[node.id] = node.stamp;
+	}
+	int links = 0;
+	for(const rtabmap_msgs::msg::Link & link : getGraph(true, false).graph.links)
+	{
+		if(link.type == rtabmap::Link::kNeighbor)
+		{
+			const int to = std::max(link.from_id, link.to_id);
+			ASSERT_TRUE(stamps.count(to));
+			const int k = int(std::lround((stamps.at(to) - kStart) / 0.02));
+			EXPECT_NEAR(odomVariance(k), 1.0 / link.information[0], 1e-6) << "link to node " << to;
+			++links;
+		}
+	}
+	EXPECT_EQ(int(all.size()) - 1, links);
+}
+
+/**
+ * gen_scan with four cameras and no lidar: each node takes the first camera's stamp, and
+ * with odom_sensor_sync the other three are moved to where the robot was when each was
+ * taken. The scans generated from all nodes then line up on the room's walls: every point,
+ * placed in the map by its node's pose, within 1 cm of a wall.
+ */
+TEST_F(CoreWrapperSyncTest, gen_scan_from_unsynchronized_cameras_lines_up_with_odom_sensor_sync)
+{
+	driveUnsynchronized({rclcpp::Parameter("gen_scan", true),   // odom_sensor_sync on by default
+						 rclcpp::Parameter("gen_scan_max_depth", 0.0)}, false);
+
+	const std::vector<rtabmap_msgs::msg::Node> all = nodes();
+	ASSERT_GE(all.size(), 3u) << "not enough updates made it through";
+	for(const rtabmap_msgs::msg::Node & node : all)
+	{
+		const int f = int(std::lround((node.stamp - kStart) * 30.0));
+		EXPECT_NEAR(cameraStamp(f, 0), node.stamp, 1e-6) << "node " << node.id << " is not at the first camera's stamp";
+	}
+	size_t points = 0;
+	const double worst = farthestFromTheWalls(all, points);
+	EXPECT_GE(points, all.size() * kCameras * kWidth * 9 / 10);
+	EXPECT_LT(worst, 0.01) << "a generated point is " << worst << " m from the walls";
+}
+
+/**
+ * Without odom_sensor_sync, the three cameras taken after the first are placed as if taken
+ * at its stamp, while the robot was turning: their part of the scan misses the walls.
+ */
+TEST_F(CoreWrapperSyncTest, gen_scan_from_unsynchronized_cameras_misses_without_odom_sensor_sync)
+{
+	driveUnsynchronized({rclcpp::Parameter("odom_sensor_sync", false),
+						 rclcpp::Parameter("gen_scan", true),
+						 rclcpp::Parameter("gen_scan_max_depth", 0.0)}, false);
+
+	const std::vector<rtabmap_msgs::msg::Node> all = nodes();
+	ASSERT_GE(all.size(), 3u) << "not enough updates made it through";
+	size_t points = 0;
+	EXPECT_GT(farthestFromTheWalls(all, points), 0.01);
+}
+
+/**
+ * A 2D lidar sweeping while the robot drives: with odom_sensor_sync, on by default, each
+ * ray is placed where the robot was when it was measured, so every point of every node's
+ * scan lands on the room's walls.
+ */
+TEST_F(CoreWrapperSyncTest, deskews_laser_scans_with_odom_sensor_sync)
+{
+	driveSweepingLidar({});
+
+	size_t points = 0;
+	const double worst = farthestFromTheWalls(nodes(), points);
+	EXPECT_GT(points, 0u);
+	EXPECT_LT(worst, 0.01) << "a scan point is " << worst << " m from the walls";
+}
+
+/// Without it, the rays measured late in the sweep are placed from where the robot started.
+TEST_F(CoreWrapperSyncTest, leaves_laser_scans_skewed_without_odom_sensor_sync)
+{
+	driveSweepingLidar({rclcpp::Parameter("odom_sensor_sync", false)});
+
+	size_t points = 0;
+	EXPECT_GT(farthestFromTheWalls(nodes(), points), 0.03);
+}
+
+/// Without odom_sensor_sync, each camera stays where it is mounted, whatever its stamp.
+TEST_F(CoreWrapperSyncTest, leaves_cameras_at_their_mount_without_odom_sensor_sync)
+{
+	driveUnsynchronized({rclcpp::Parameter("odom_sensor_sync", false)});
+
+	const std::vector<rtabmap_msgs::msg::Node> all = nodes();
+	ASSERT_GE(all.size(), 3u) << "not enough updates made it through";
+	for(const rtabmap_msgs::msg::Node & node : all)
+	{
+		SCOPED_TRACE("node " + std::to_string(node.id));
+		rtabmap::SensorData data = rtabmap_conversions::sensorDataFromROS(node.data);
+		ASSERT_EQ(size_t(kCameras), data.cameraModels().size());
+		for(int c=0; c<kCameras; ++c)
+		{
+			const rtabmap::Transform local = data.cameraModels()[c].localTransform();
+			EXPECT_LT(local.getDistance(cameraMount(c)), 1e-4) << "camera " << c;
+			EXPECT_LT(angleBetween(local, cameraMount(c)), 1e-4) << "camera " << c;
+		}
+	}
+}
+
+//==========================================================================================
+// Asynchronous inputs: buffered, then attached to the next node
+//==========================================================================================
+
+/**
+ * user_data_async is attached to the next node only: it is data about a moment, not a
+ * setting that sticks.
+ */
+TEST_F(CoreWrapperInputsTest, attaches_async_user_data_to_the_next_node)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::UserData>::SharedPtr userData =
+			helper()->create_publisher<rtabmap_msgs::msg::UserData>("user_data_async", 1);
+	ASSERT_TRUE(waitForSubscriber(userData));
+
+	userData->publish(makeUserData(1.0));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 2);
+
+	EXPECT_FALSE(getNode(1).data.user_data.empty());
+	EXPECT_TRUE(getNode(2).data.user_data.empty());
+}
+
+/// The first byte of the user data stored with @p node, or -1 if it has none.
+int firstUserDataByte(const rtabmap_msgs::msg::Node & node)
+{
+	if(node.data.user_data.empty())
+	{
+		return -1;
+	}
+	const cv::Mat userData = rtabmap::uncompressData(
+			rtabmap_conversions::compressedMatFromBytes(node.data.user_data));
+	return userData.empty() ? -1 : int(userData.at<uint8_t>(0, 0));
+}
+
+/**
+ * With subscribe_sensor_data too, user_data_async is attached to the next node. The
+ * SensorData message has its own user data field; when it is set, it wins, and the
+ * async user data is dropped with a warning rather than kept for a later node.
+ */
+TEST_F(CoreWrapperInputsTest, attaches_async_user_data_to_sensor_data_without_its_own)
+{
+	makeNode({rclcpp::Parameter("subscribe_sensor_data", true)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr data =
+			helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
+	rclcpp::Publisher<rtabmap_msgs::msg::UserData>::SharedPtr userData =
+			helper()->create_publisher<rtabmap_msgs::msg::UserData>("user_data_async", 1);
+	ASSERT_TRUE(waitForSubscriber(data));
+	ASSERT_TRUE(waitForSubscriber(userData));
+
+	const auto update = [&](double stamp, double x, int ownUserData) {
+		rtabmap::SensorData sensorData(cv::Mat(), 0, stamp,
+				ownUserData < 0 ? cv::Mat() :
+				rtabmap_conversions::userDataFromROS(makeUserData(stamp, uint8_t(ownUserData))));
+		rtabmap_msgs::msg::SensorData msg;
+		rtabmap_conversions::sensorDataToROS(sensorData, msg, "base_link", true);
+		msg.header.stamp = stampOf(stamp);
+		const size_t before = info->size();
+		sendOdom(odom, stamp, x);
+		data->publish(msg);
+		return spinUntil([&]() { return info->size() > before; });
+	};
+
+	// No user data in the message: the async one is taken.
+	userData->publish(makeUserData(1.0, 7));
+	spinFor(std::chrono::milliseconds(100));
+	ASSERT_TRUE(update(1.0, 0.0, -1));
+
+	// User data in the message: it is kept, and the async one dropped...
+	userData->publish(makeUserData(2.0, 9));
+	spinFor(std::chrono::milliseconds(100));
+	ASSERT_TRUE(update(2.0, 0.5, 5));
+
+	// ...not carried over to the next node.
+	ASSERT_TRUE(update(3.0, 1.0, -1));
+
+	EXPECT_EQ(7, firstUserDataByte(getNode(1)));
+	EXPECT_EQ(5, firstUserDataByte(getNode(2)));
+	EXPECT_EQ(-1, firstUserDataByte(getNode(3)));
+}
+
+/// A GPS fix is attached to the node closest in time, with its error from the covariance.
+TEST_F(CoreWrapperInputsTest, attaches_gps_to_the_next_node)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps =
+			helper()->create_publisher<sensor_msgs::msg::NavSatFix>("gps/fix", 1);
+	ASSERT_TRUE(waitForSubscriber(gps));
+
+	gps->publish(makeGpsFix(1.0, 45.3786, -71.9277, 250.0, 4.0));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	rtabmap_msgs::msg::Node node = getNode(1);
+	EXPECT_NEAR(45.3786, node.data.gps.latitude, 1e-6);
+	EXPECT_NEAR(-71.9277, node.data.gps.longitude, 1e-6);
+	EXPECT_NEAR(250.0, node.data.gps.altitude, 1e-6);
+	EXPECT_NEAR(2.0, node.data.gps.error, 1e-6) << "sqrt of the largest variance";
+}
+
+/**
+ * The IMU orientation, interpolated at the node's stamp, is attached to it, and RTAB-Map
+ * turns it into a gravity constraint on the node -- a link from the node to itself that
+ * holds the graph's roll and pitch to gravity. The IMU messages here are a quarter and
+ * three quarters of the way around the node's stamp, so the orientation the link carries
+ * is interpolated, not the nearest one, nor their average.
+ */
+TEST_F(CoreWrapperInputsTest, attaches_imu_orientation_to_the_next_node)
+{
+	publishStaticTf("imu_link");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+	ASSERT_TRUE(waitForSubscriber(imu));
+
+	imu->publish(makeImu("imu_link", 0.9, 0.0));
+	imu->publish(makeImu("imu_link", 1.3, 0.4));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);   // stamped 1.0: a quarter of the way from 0.9 to 1.3
+
+	const rtabmap_msgs::msg::Link * gravity = findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1);
+	ASSERT_TRUE(gravity != nullptr);
+	float roll, pitch, yaw;
+	rtabmap_conversions::transformFromGeometryMsg(gravity->transform).getEulerAngles(roll, pitch, yaw);
+	EXPECT_NEAR(0.1, roll, 1e-4);
+	EXPECT_NEAR(0.0, pitch, 1e-4);
+}
+
+/**
+ * The orientation is re-expressed in base_link: an IMU mounted turned 90 degrees reports
+ * a roll about its own x axis, which is the robot's y axis, so the robot pitches.
+ */
+TEST_F(CoreWrapperInputsTest, expresses_imu_orientation_in_the_robot_frame)
+{
+	publishStaticTf(makeTransform("base_link", "imu_link", 0.0, 0.0, 0.0, M_PI/2.0));
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+	ASSERT_TRUE(waitForSubscriber(imu));
+
+	imu->publish(makeImu("imu_link", 0.9, 0.1));
+	imu->publish(makeImu("imu_link", 1.1, 0.1));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	const rtabmap_msgs::msg::Link * gravity = findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1);
+	ASSERT_TRUE(gravity != nullptr);
+	float roll, pitch, yaw;
+	rtabmap_conversions::transformFromGeometryMsg(gravity->transform).getEulerAngles(roll, pitch, yaw);
+	EXPECT_NEAR(0.0, roll, 1e-4);
+	EXPECT_NEAR(0.1, pitch, 1e-4);   // +0.1 about the robot's y axis
+}
+
+/// With no IMU message after the node's stamp, there is nothing to interpolate: no link.
+TEST_F(CoreWrapperInputsTest, adds_no_gravity_link_without_imu_around_the_stamp)
+{
+	publishStaticTf("imu_link");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+	ASSERT_TRUE(waitForSubscriber(imu));
+
+	imu->publish(makeImu("imu_link", 0.8, 0.1));
+	imu->publish(makeImu("imu_link", 0.9, 0.1));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	EXPECT_TRUE(findLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1) == nullptr);
+}
+
+/// An IMU message without an orientation carries nothing the node uses and is ignored.
+TEST_F(CoreWrapperInputsTest, ignores_an_imu_without_orientation)
+{
+	publishStaticTf("imu_link");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
+	ASSERT_TRUE(waitForSubscriber(imu));
+
+	sensor_msgs::msg::Imu msg = makeImu("imu_link", 0.9);
+	msg.orientation.w = 0.0;
+	imu->publish(msg);
+	msg.header.stamp = stampOf(1.1);
+	imu->publish(msg);
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	EXPECT_FALSE(hasLink(getGraph().graph, rtabmap::Link::kGravity, 1, 1));
+}
+
+/**
+ * A landmark detection -- a fiducial seen by a camera -- becomes a landmark in the graph,
+ * under the negative of its id, linked to the node that saw it.
+ */
+TEST_F(CoreWrapperInputsTest, adds_detected_landmarks_to_the_graph)
+{
+	publishStaticTf("camera");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmark =
+			helper()->create_publisher<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1);
+	ASSERT_TRUE(waitForSubscriber(landmark));
+
+	publishTf(makeTransform("odom", "base_link", 1.0));
+	landmark->publish(makeLandmark("camera", 1.0, 5, 1.5));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	rtabmap_msgs::msg::MapData map = getGraph();
+	ASSERT_TRUE(hasPose(map.graph, -5));
+	EXPECT_TRUE(hasLink(map.graph, rtabmap::Link::kLandmark, 1, -5));
+	for(size_t i=0; i<map.graph.poses_id.size(); ++i)
+	{
+		if(map.graph.poses_id[i] == -5)
+		{
+			EXPECT_NEAR(1.5, map.graph.poses[i].position.x, 1e-3);
+		}
+	}
+}
+
+/**
+ * A detection is rarely stamped with the node it ends up in: the robot kept moving in
+ * between. Its pose is corrected by the robot's motion from the detection's stamp to the
+ * node's, taken from the odometry in TF, interpolated between samples -- so the landmark
+ * lands where the tag is, not where it would be had the robot been standing still.
+ */
+TEST_F(CoreWrapperInputsTest, corrects_a_landmark_for_the_motion_since_its_detection)
+{
+	publishStaticTf("camera");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmark =
+			helper()->create_publisher<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1);
+	ASSERT_TRUE(waitForSubscriber(landmark));
+
+	// Driving at 1 m/s: x = 0 at 1.0 s, x = 0.4 at 1.4 s. The tag is seen at 1.1 s, 1.5 m
+	// ahead: the robot was at x = 0.1 then, so the tag is at x = 1.6.
+	publishTf(makeTransform("odom", "base_link", 1.0, 0.0));
+	publishTf(makeTransform("odom", "base_link", 1.4, 0.4));
+	landmark->publish(makeLandmark("camera", 1.1, 5, 1.5));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1, 0.5, 1.4, 0.4);   // the node, at 1.4 s and x = 0.4
+
+	rtabmap_msgs::msg::MapData map = getGraph();
+	const rtabmap_msgs::msg::Link * link = findLink(map.graph, rtabmap::Link::kLandmark, 1, -5);
+	ASSERT_TRUE(link != nullptr);
+	// The graph may hold the link either way; seen from the node, the tag is 1.2 m ahead.
+	const double ahead = link->from_id == 1 ? link->transform.translation.x : -link->transform.translation.x;
+	EXPECT_NEAR(1.2, ahead, 1e-3);
+	bool found = false;
+	for(size_t i=0; i<map.graph.poses_id.size(); ++i)
+	{
+		if(map.graph.poses_id[i] == -5)
+		{
+			found = true;
+			EXPECT_NEAR(1.6, map.graph.poses[i].position.x, 1e-3)
+					<< "1.9 would be uncorrected, 1.5 the correction from the nearest sample";
+		}
+	}
+	EXPECT_TRUE(found);
+}
+
+/**
+ * Marker/Priors gives landmarks known world poses: once one is seen, the map is moved into
+ * that world frame. Here marker 5 is at x = 10 and seen 1.5 m ahead from where the robot
+ * starts, so the robot's first node is at x = 8.5. The priors only apply with
+ * Optimizer/PriorsIgnored off; at its default they are ignored, and the map stays where
+ * odometry started it.
+ */
+TEST_F(CoreWrapperInputsTest, marker_priors_place_the_map_in_the_world)
+{
+	for(bool priorsIgnored : {true, false})
+	{
+		SCOPED_TRACE(std::string("Optimizer/PriorsIgnored=") + (priorsIgnored?"true":"false"));
+		publishStaticTf("camera");
+		makeNode({rclcpp::Parameter(Parameters::kMarkerPriors(), "5 10 0 0 0 0 0"),
+				  rclcpp::Parameter(Parameters::kOptimizerPriorsIgnored(), priorsIgnored?"true":"false"),
+				  rclcpp::Parameter("delete_db_on_start", true)});
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+		rclcpp::Publisher<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmark =
+				helper()->create_publisher<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1);
+		ASSERT_TRUE(waitForSubscriber(landmark));
+
+		for(int i=0; i<3; ++i)
+		{
+			publishTf(makeTransform("odom", "base_link", 1.0 + i, 0.5*i));
+			landmark->publish(makeLandmark("camera", 1.0 + i, 5, 1.5 - 0.5*i));
+			spinFor(std::chrono::milliseconds(100));
+			driveStraight(odom, info, 1, 0.5, 1.0 + i, 0.5*i);
+		}
+
+		std::map<int, double> x;
+		const rtabmap_msgs::msg::MapData map = getGraph();
+		for(size_t k=0; k<map.graph.poses_id.size(); ++k)
+		{
+			x[map.graph.poses_id[k]] = map.graph.poses[k].position.x;
+		}
+		ASSERT_TRUE(x.count(-5) && x.count(1));
+		EXPECT_NEAR(priorsIgnored ? 1.5 : 10.0, x.at(-5), 1e-3) << "the marker";
+		EXPECT_NEAR(priorsIgnored ? 0.0 : 8.5, x.at(1), 1e-3) << "the robot's first node";
+		destroyNode();
+	}
+}
+
+/// Landmark ids must be positive: 0 and below are refused.
+TEST_F(CoreWrapperInputsTest, refuses_a_landmark_with_a_non_positive_id)
+{
+	publishStaticTf("camera");
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmark =
+			helper()->create_publisher<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1);
+	ASSERT_TRUE(waitForSubscriber(landmark));
+
+	publishTf(makeTransform("odom", "base_link", 1.0));
+	landmark->publish(makeLandmark("camera", 1.0, 0));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	EXPECT_EQ(std::vector<int>({1}), getGraph().graph.poses_id);
+}
+
+/**
+ * global_pose -- an absolute pose from outside, a motion capture system say -- is
+ * attached to the next node as a pose prior: a link from the node to itself.
+ */
+TEST_F(CoreWrapperInputsTest, adds_a_global_pose_as_a_prior)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr global =
+			helper()->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1);
+	ASSERT_TRUE(waitForSubscriber(global));
+
+	publishTf(makeTransform("odom", "base_link", 1.0));
+	global->publish(makePoseWithCovariance("base_link", 1.0, 3.0));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	EXPECT_TRUE(hasLink(getGraph().graph, rtabmap::Link::kPosePrior, 1, 1));
+}
+
+/**
+ * Like a landmark, a global pose is rarely stamped with the node it ends up in. It is
+ * moved forward by the robot's motion from its stamp to the node's, from the odometry in
+ * TF, interpolated between samples.
+ */
+TEST_F(CoreWrapperInputsTest, corrects_a_global_pose_for_the_motion_since_its_stamp)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr global =
+			helper()->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1);
+	ASSERT_TRUE(waitForSubscriber(global));
+
+	// Driving at 1 m/s: x = 0 at 1.0 s, x = 0.4 at 1.4 s in odom. At 1.1 s, the robot is
+	// at x = 5.0 in the world, say the global pose: by 1.4 s it has moved 0.3 m further.
+	publishTf(makeTransform("odom", "base_link", 1.0, 0.0));
+	publishTf(makeTransform("odom", "base_link", 1.4, 0.4));
+	global->publish(makePoseWithCovariance("base_link", 1.1, 5.0));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1, 0.5, 1.4, 0.4);   // the node, at 1.4 s
+
+	const rtabmap_msgs::msg::Link * prior = findLink(getGraph().graph, rtabmap::Link::kPosePrior, 1, 1);
+	ASSERT_TRUE(prior != nullptr);
+	EXPECT_NEAR(5.3, prior->transform.translation.x, 1e-3)
+			<< "5.0 would be uncorrected, 5.0 or 5.4 the correction from the nearest sample";
+	EXPECT_NEAR(0.0, prior->transform.translation.y, 1e-3);
+}
+
+TEST_F(CoreWrapperInputsTest, attaches_env_sensors_to_the_next_node)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<rtabmap_msgs::msg::EnvSensor>::SharedPtr env =
+			helper()->create_publisher<rtabmap_msgs::msg::EnvSensor>("env_sensor", 1);
+	ASSERT_TRUE(waitForSubscriber(env));
+
+	rtabmap_msgs::msg::EnvSensor msg;
+	msg.header.stamp = stampOf(1.0);
+	msg.type = rtabmap_msgs::msg::EnvSensor::TYPE_AMBIENT_TEMPERATURE;
+	msg.value = 21.5;
+	env->publish(msg);
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1);
+
+	rtabmap_msgs::msg::Node node = getNode(1);
+	ASSERT_EQ(1u, node.data.env_sensors.size());
+	EXPECT_EQ(rtabmap_msgs::msg::EnvSensor::TYPE_AMBIENT_TEMPERATURE, node.data.env_sensors[0].type);
+	EXPECT_DOUBLE_EQ(21.5, node.data.env_sensors[0].value);
+}
+
+/**
+ * inter_odom fills the gaps between nodes with intermediate poses, when intermediate
+ * nodes are enabled and the detection rate is 0 -- the node is then driven by its sensor
+ * topics, and inter_odom is a faster odometry to interpolate the trajectory with.
+ *
+ * The stamps of those messages are compared with the update's, both in ROS time: built
+ * from their seconds and nanoseconds instead, they would be in system time, and rclcpp
+ * throws on a comparison across clocks.
+ */
+TEST_F(CoreWrapperInputsTest, inter_odom_adds_intermediate_nodes)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true")});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr inter =
+			helper()->create_publisher<nav_msgs::msg::Odometry>("inter_odom", 10);
+	ASSERT_TRUE(waitForSubscriber(inter));
+
+	driveStraight(odom, info, 1);
+	inter->publish(makeOdometry(1.3, 0.15));
+	inter->publish(makeOdometry(1.6, 0.3));
+	spinFor(std::chrono::milliseconds(100));
+	driveStraight(odom, info, 1, 0.5, 2.0, 0.5);
+
+	EXPECT_EQ(4u, getGraph().graph.poses_id.size());
+}
+
+/**
+ * With subscribe_inter_odom_info, inter_odom is synchronized with inter_odom_info by exact
+ * stamp, so each intermediate node also gets the statistics of the odometry that
+ * produced it. A message on only one of the two is not used.
+ */
+TEST_F(CoreWrapperInputsTest, inter_odom_info_adds_intermediate_nodes)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true"),
+			  rclcpp::Parameter("subscribe_inter_odom_info", true)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr inter =
+			helper()->create_publisher<nav_msgs::msg::Odometry>("inter_odom", 10);
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr interInfo =
+			helper()->create_publisher<rtabmap_msgs::msg::OdomInfo>("inter_odom_info", 10);
+	ASSERT_TRUE(waitForSubscriber(inter));
+	ASSERT_TRUE(waitForSubscriber(interInfo));
+
+	driveStraight(odom, info, 1);
+	for(double stamp : {1.3, 1.6})
+	{
+		nav_msgs::msg::Odometry msg = makeOdometry(stamp, 0.5*(stamp-1.0));
+		rtabmap_msgs::msg::OdomInfo odomInfo;
+		odomInfo.header = msg.header;
+		odomInfo.time_estimation = 0.01f;
+		odomInfo.interval = 0.3f;
+		odomInfo.transform.translation.x = 0.15;
+		odomInfo.transform.rotation.w = 1.0;
+		inter->publish(msg);
+		interInfo->publish(odomInfo);
+	}
+	// Alone on inter_odom, with no inter_odom_info to pair with: dropped.
+	inter->publish(makeOdometry(1.8, 0.4));
+	spinFor(std::chrono::milliseconds(200));
+	driveStraight(odom, info, 1, 0.5, 2.0, 0.5);
+
+	EXPECT_EQ(4u, getGraph().graph.poses_id.size());
+}
+
+//==========================================================================================
+// Localization
+//==========================================================================================
+
+class CoreWrapperLocalizationTest : public CoreWrapperInputsTest
+{
+protected:
+	/// Builds a 3-node map along x and restarts on it in localization mode.
+	void restartInLocalization(const std::vector<rclcpp::Parameter> & params = {})
+	{
+		{
+			makeNode();
+			std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+			rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+			driveStraight(odom, info, 3);
+		}
+		destroyNode();
+		std::vector<rclcpp::Parameter> all = {
+			rclcpp::Parameter(Parameters::kMemIncrementalMemory(), "false")};
+		all.insert(all.end(), params.begin(), params.end());
+		makeNode(all);
+	}
+};
+
+/**
+ * In localization mode on a saved map, the map is loaded and not extended, and the pose
+ * starts where initial_pose says: it is added to the odometry until a loop closure
+ * localizes the robot for real. Until then the covariance says it is not localized.
+ */
+TEST_F(CoreWrapperLocalizationTest, initial_pose_places_the_robot_in_the_map)
+{
+	restartInLocalization({rclcpp::Parameter("initial_pose", "1 0 0 0 0 0")});
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
+			collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	ASSERT_TRUE(waitForPublisher(pose->subscription));
+
+	driveStraight(odom, info, 2, 0.3, 10.0);
+
+	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
+	ASSERT_TRUE(spinUntil([&]() { return pose->size() >= 2; }));
+	EXPECT_NEAR(1.3, pose->back().pose.pose.position.x, 1e-3);
+	EXPECT_EQ(9999.0, pose->back().pose.covariance[0]) << "not localized by a loop closure yet";
+}
+
+/// initialpose does the same at runtime, as RViz's "2D Pose Estimate" tool publishes it.
+TEST_F(CoreWrapperLocalizationTest, initialpose_topic_places_the_robot_in_the_map)
+{
+	restartInLocalization();
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
+			collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial =
+			helper()->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 1);
+	ASSERT_TRUE(waitForPublisher(pose->subscription));
+	ASSERT_TRUE(waitForSubscriber(initial));
+
+	initial->publish(makePoseWithCovariance("map", 0.0, 0.5));
+	spinFor(std::chrono::milliseconds(200));
+	driveStraight(odom, info, 2, 0.3, 10.0);
+
+	ASSERT_TRUE(spinUntil([&]() { return pose->size() >= 2; }));
+	EXPECT_NEAR(0.8, pose->back().pose.pose.position.x, 1e-3);
+}
+
+/**
+ * loc_thr adds a "Localization status" entry to /diagnostics: an error until the
+ * localization covariance falls under the threshold.
+ */
+TEST_F(CoreWrapperLocalizationTest, reports_localization_status_on_diagnostics)
+{
+	restartInLocalization({rclcpp::Parameter("loc_thr", 0.25)});
+	std::shared_ptr<Collector<diagnostic_msgs::msg::DiagnosticArray>> diagnostics =
+			collect<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 1, 0.3, 10.0);
+
+	const diagnostic_msgs::msg::DiagnosticStatus * status = nullptr;
+	ASSERT_TRUE(spinUntil([&]() {
+		for(const auto & msg : diagnostics->messages)
+		{
+			for(const diagnostic_msgs::msg::DiagnosticStatus & s : msg->status)
+			{
+				if(s.name.find("Localization status") != std::string::npos)
+				{
+					status = &s;
+				}
+			}
+		}
+		return status != nullptr; }, std::chrono::milliseconds(5000)));
+	EXPECT_EQ(diagnostic_msgs::msg::DiagnosticStatus::ERROR, status->level);
+}
+
+}  // namespace
+
+}  // namespace rtabmap_slam_test
diff --git a/rtabmap_slam/test/test_core_wrapper_mapping.cpp b/rtabmap_slam/test/test_core_wrapper_mapping.cpp
new file mode 100644
index 00000000..803e8224
--- /dev/null
+++ b/rtabmap_slam/test/test_core_wrapper_mapping.cpp
@@ -0,0 +1,428 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
+
+#include <rtabmap/core/Parameters.h>
+
+#include "core_wrapper_fixture.hpp"
+
+namespace rtabmap_slam_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+using rtabmap::Parameters;
+
+class CoreWrapperMappingTest : public CoreWrapperTest
+{
+protected:
+	/// The x of every pose in @p graph, in node id order.
+	static std::vector<double> xs(const rtabmap_msgs::msg::MapGraph & graph)
+	{
+		std::vector<double> out;
+		for(const geometry_msgs::msg::Pose & p : graph.poses)
+		{
+			out.push_back(p.position.x);
+		}
+		return out;
+	}
+
+	/// The neighbor link between @p from and @p to, or one with from_id 0 if none.
+	static rtabmap_msgs::msg::Link neighborLink(
+			const rtabmap_msgs::msg::MapGraph & graph, int from, int to)
+	{
+		for(const rtabmap_msgs::msg::Link & l : graph.links)
+		{
+			if(l.type == 0 && ((l.from_id == from && l.to_id == to) ||
+							   (l.from_id == to && l.to_id == from)))
+			{
+				return l;
+			}
+		}
+		return rtabmap_msgs::msg::Link();
+	}
+
+	/// Counts the map -> odom transforms seen on /tf.
+	static size_t countTransforms(
+			const std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> & tf,
+			const std::string & parent, const std::string & child)
+	{
+		size_t found = 0;
+		for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+		{
+			for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+			{
+				found += (t.header.frame_id == parent && t.child_frame_id == child) ? 1 : 0;
+			}
+		}
+		return found;
+	}
+};
+
+/**
+ * The simplest input rtabmap accepts is odometry alone, and every update that moved far
+ * enough becomes a node, linked to the previous one by the odometry between them.
+ */
+TEST_F(CoreWrapperMappingTest, adds_a_node_per_odometry_update)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 3);
+
+	ASSERT_EQ(3u, info->size());
+	EXPECT_EQ(1, info->messages[0]->ref_id);
+	EXPECT_EQ(2, info->messages[1]->ref_id);
+	EXPECT_EQ(3, info->messages[2]->ref_id);
+
+	rtabmap_msgs::msg::MapData map = getGraph();
+	ASSERT_EQ(3u, map.graph.poses_id.size());
+	std::vector<double> x = xs(map.graph);
+	EXPECT_NEAR(0.0, x[0], 1e-4);
+	EXPECT_NEAR(0.5, x[1], 1e-4);
+	EXPECT_NEAR(1.0, x[2], 1e-4);
+	EXPECT_NE(0, neighborLink(map.graph, 1, 2).from_id);
+	EXPECT_NE(0, neighborLink(map.graph, 2, 3).from_id);
+}
+
+/**
+ * An update that did not move at least RGBD/LinearUpdate (or turn RGBD/AngularUpdate)
+ * since the last node is not added: a robot standing still does not grow the map.
+ */
+TEST_F(CoreWrapperMappingTest, does_not_add_nodes_while_standing_still)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.1")});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 4, 0.02);
+
+	EXPECT_EQ(1u, getGraph().graph.poses_id.size());
+}
+
+/**
+ * Rtabmap/DetectionRate throttles the updates by their stamps, not by when they arrive:
+ * one closer than 1/rate to the last one processed is dropped.
+ */
+TEST_F(CoreWrapperMappingTest, throttles_updates_to_the_detection_rate)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1")});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	for(int i=0; i<6; ++i)
+	{
+		sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
+		spinFor(std::chrono::milliseconds(150));
+	}
+	spinFor(std::chrono::milliseconds(300));
+
+	// 1.0 and 2.0 are a full period apart; 1.25, 1.5, 1.75 and 2.25 are not.
+	EXPECT_EQ(2u, info->size());
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+}
+
+/**
+ * With Rtabmap/CreateIntermediateNodes, the updates the detection rate would have dropped
+ * are kept as intermediate nodes instead: poses in the graph, without the sensor data or
+ * the loop closure detection. They do not publish `info`.
+ */
+TEST_F(CoreWrapperMappingTest, keeps_throttled_updates_as_intermediate_nodes)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
+			  rclcpp::Parameter(Parameters::kRtabmapCreateIntermediateNodes(), "true")});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	for(int i=0; i<5; ++i)
+	{
+		sendOdom(odom, 1.0 + 0.25*i, 0.5*i);
+		spinFor(std::chrono::milliseconds(150));
+	}
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_EQ(2u, info->size()) << "only the updates at 1.0 and 2.0 are full nodes";
+	rtabmap_msgs::msg::MapData map = getGraph();
+	EXPECT_EQ(5u, map.graph.poses_id.size());
+}
+
+/**
+ * An odometry that resets -- an identity pose after a non-identity one, or 9999 on both
+ * covariance diagonals -- starts a new map in the same database, rather than tearing the
+ * graph across a jump the robot never made.
+ */
+TEST_F(CoreWrapperMappingTest, starts_a_new_map_when_odometry_resets)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 2);
+	{
+		const size_t before = info->size();
+		publishTf(makeTransform("odom", "base_link", 3.0));
+		odom->publish(makeResetOdometry(3.0));
+		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }));
+	}
+	driveStraight(odom, info, 2, 0.5, 4.0, 0.5);
+
+	EXPECT_EQ(std::vector<int>({0, 0, 1, 1, 1}), mapIds());
+}
+
+/**
+ * staleness_factor: when the gap between two updates exceeds that many detection
+ * periods, the odometry is not trusted across it and a new map is started, as if it had
+ * reset.
+ */
+TEST_F(CoreWrapperMappingTest, starts_a_new_map_after_a_stale_gap)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
+			  rclcpp::Parameter("staleness_factor", 2.0)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 2);                  // stamps 1 and 2
+	driveStraight(odom, info, 2, 0.5, 6.0, 1.0);   // 4 s later: more than 2 periods
+
+	EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
+}
+
+/// Values of staleness_factor between 0 and 1 make no sense and disable it.
+TEST_F(CoreWrapperMappingTest, ignores_a_staleness_factor_below_one)
+{
+	makeNode({rclcpp::Parameter(Parameters::kRtabmapDetectionRate(), "1"),
+			  rclcpp::Parameter("staleness_factor", 0.5)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 2);
+	driveStraight(odom, info, 2, 0.5, 6.0, 1.0);
+
+	EXPECT_EQ(std::vector<int>({0, 0, 0, 0}), mapIds());
+}
+
+/// A message with a zero stamp cannot be placed in time and is dropped.
+TEST_F(CoreWrapperMappingTest, drops_updates_with_a_null_stamp)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	odom->publish(makeOdometry(0.0, 1.0));
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(info->empty());
+}
+
+/**
+ * The odometry's covariance becomes the information matrix of the link between two nodes
+ * (its inverse). The twist covariance is preferred, since it is the uncertainty of the
+ * motion between the two rather than accumulated since the start.
+ */
+TEST_F(CoreWrapperMappingTest, weights_links_with_the_odometry_covariance)
+{
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.01);
+	ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
+	sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.01);
+	ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
+
+	rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
+	ASSERT_NE(0, link.from_id);
+	EXPECT_NEAR(100.0, link.information[0], 1e-3);
+	EXPECT_NEAR(100.0, link.information[35], 1e-3);
+}
+
+/**
+ * An odometry with no covariance -- all zeros, as many drivers publish -- gets
+ * odom_tf_linear_variance and odom_tf_angular_variance instead of an infinitely
+ * confident link.
+ */
+TEST_F(CoreWrapperMappingTest, falls_back_to_default_variances_without_covariance)
+{
+	makeNode({rclcpp::Parameter("odom_tf_linear_variance", 0.04),
+			  rclcpp::Parameter("odom_tf_angular_variance", 0.25)});
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	sendOdom(odom, 1.0, 0.0, 0.0, 0.0, 0.0);
+	ASSERT_TRUE(spinUntil([&]() { return info->size() == 1; }));
+	sendOdom(odom, 2.0, 0.5, 0.0, 0.0, 0.0);
+	ASSERT_TRUE(spinUntil([&]() { return info->size() == 2; }));
+
+	rtabmap_msgs::msg::Link link = neighborLink(getGraph().graph, 1, 2);
+	ASSERT_NE(0, link.from_id);
+	EXPECT_NEAR(25.0, link.information[0], 1e-3);
+	EXPECT_NEAR(4.0, link.information[35], 1e-3);
+}
+
+/**
+ * The node's job on TF is map -> odom: the correction that puts the odometry frame where
+ * the optimized graph says it is. Identity until a loop closure moves it. It is published
+ * from its own thread once the odometry frame is known, at a rate of 1/tf_delay.
+ */
+TEST_F(CoreWrapperMappingTest, publishes_map_to_odom_on_tf)
+{
+	makeNode();
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_EQ(0u, countTransforms(tf, "map", "odom"))
+			<< "the odometry frame is not known before the first update";
+
+	driveStraight(odom, info, 1);
+	ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
+}
+
+/// odom_frame_id_init publishes map -> odom from the start, before any odometry arrives.
+TEST_F(CoreWrapperMappingTest, odom_frame_id_init_publishes_tf_before_the_first_update)
+{
+	makeNode({rclcpp::Parameter("odom_frame_id_init", "odom")});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+
+	EXPECT_TRUE(spinUntil([&]() { return countTransforms(tf, "map", "odom") >= 3; }));
+}
+
+TEST_F(CoreWrapperMappingTest, publish_tf_false_publishes_no_tf)
+{
+	makeNode({rclcpp::Parameter("publish_tf", false)});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+
+	driveStraight(odom, info, 2);
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
+}
+
+/// map_frame_id renames the map frame everywhere: TF and every map-frame topic.
+TEST_F(CoreWrapperMappingTest, map_frame_id_renames_the_map_frame)
+{
+	makeNode({rclcpp::Parameter("map_frame_id", "world")});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+	std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
+			collect<nav_msgs::msg::Path>("mapPath");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	ASSERT_TRUE(waitForPublisher(path->subscription));
+
+	driveStraight(odom, info, 2);
+
+	ASSERT_TRUE(spinUntil([&]() { return countTransforms(tf, "world", "odom") > 0; }));
+	EXPECT_EQ(0u, countTransforms(tf, "map", "odom"));
+	ASSERT_TRUE(spinUntil([&]() { return !path->empty(); }));
+	EXPECT_EQ("world", path->back().header.frame_id);
+	EXPECT_EQ("world", info->back().header.frame_id);
+}
+
+/**
+ * mapPath and mapGraph carry the optimized graph after every update: the trajectory for
+ * display, and the graph with its links for the nodes that assemble maps from it.
+ */
+TEST_F(CoreWrapperMappingTest, publishes_the_graph_after_every_update)
+{
+	makeNode();
+	std::shared_ptr<Collector<nav_msgs::msg::Path>> path =
+			collect<nav_msgs::msg::Path>("mapPath");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
+			collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
+					rclcpp::QoS(1).reliable().transient_local());
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	ASSERT_TRUE(waitForPublisher(path->subscription));
+	ASSERT_TRUE(waitForPublisher(graph->subscription));
+
+	driveStraight(odom, info, 3);
+
+	ASSERT_TRUE(spinUntil([&]() {
+		return !path->empty() && path->back().poses.size() == 3 &&
+			   !graph->empty() && graph->back().poses_id.size() == 3; }));
+	EXPECT_EQ("map", path->back().header.frame_id);
+	EXPECT_NEAR(1.0, path->back().poses[2].pose.position.x, 1e-4);
+	EXPECT_EQ(2u, graph->back().links.size());
+}
+
+/**
+ * localization_pose is the robot's pose in the map frame -- map -> odom composed with
+ * the odometry -- published on every update. While mapping, its covariance is the
+ * odometry's accumulated along the graph, so it grows with distance until a loop closure
+ * brings it back down.
+ */
+TEST_F(CoreWrapperMappingTest, publishes_the_pose_in_the_map_frame)
+{
+	makeNode();
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
+			collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	ASSERT_TRUE(waitForPublisher(pose->subscription));
+
+	driveStraight(odom, info, 2);
+
+	ASSERT_TRUE(spinUntil([&]() { return pose->size() == 2; }));
+	EXPECT_EQ("map", pose->back().header.frame_id);
+	EXPECT_NEAR(0.5, pose->back().pose.pose.position.x, 1e-4);
+	EXPECT_GT(pose->back().pose.covariance[0], 0.0);
+	EXPECT_LT(pose->back().pose.covariance[0], 1.0) << "not the 9999 of an unknown pose";
+}
+
+/// pub_loc_pose_only_when_localizing holds it back until a loop closure has localized.
+TEST_F(CoreWrapperMappingTest, pub_loc_pose_only_when_localizing_holds_back_the_pose)
+{
+	makeNode({rclcpp::Parameter("pub_loc_pose_only_when_localizing", true)});
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
+			collect<geometry_msgs::msg::PoseWithCovarianceStamped>("localization_pose");
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	ASSERT_TRUE(waitForPublisher(pose->subscription));
+
+	driveStraight(odom, info, 2);
+	spinFor(std::chrono::milliseconds(200));
+
+	EXPECT_TRUE(pose->empty());
+}
+
+/**
+ * The map survives a restart: the database saved on shutdown is reopened, the next update
+ * starts a new session in it, and the new nodes carry on numbering after the old ones.
+ */
+TEST_F(CoreWrapperMappingTest, continues_the_saved_map_after_a_restart)
+{
+	{
+		makeNode();
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+		driveStraight(odom, info, 2);
+	}
+	destroyNode();
+
+	makeNode();
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+	driveStraight(odom, info, 2, 0.5, 10.0);
+
+	EXPECT_EQ(3, info->front().ref_id);
+	EXPECT_EQ(std::vector<int>({0, 0, 1, 1}), mapIds());
+}
+
+}  // namespace
+
+}  // namespace rtabmap_slam_test
diff --git a/rtabmap_slam/test/test_core_wrapper_parameters.cpp b/rtabmap_slam/test/test_core_wrapper_parameters.cpp
new file mode 100644
index 00000000..285c113d
--- /dev/null
+++ b/rtabmap_slam/test/test_core_wrapper_parameters.cpp
@@ -0,0 +1,308 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <fstream>
+#include <sstream>
+
+#include <rtabmap/core/Parameters.h>
+
+#include "core_wrapper_fixture.hpp"
+
+namespace rtabmap_slam_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+using rtabmap::Parameters;
+
+class CoreWrapperParametersTest : public CoreWrapperTest
+{
+protected:
+	/// RTAB-Map's own default for @p key, spelled the way the node stores it.
+	static std::string rtabmapDefault(const std::string & key)
+	{
+		return Parameters::getDefaultParameters().at(key);
+	}
+
+	static std::string readFile(const std::string & path)
+	{
+		std::ifstream in(path);
+		std::stringstream s;
+		s << in.rdbuf();
+		return s.str();
+	}
+};
+
+/**
+ * Every RTAB-Map parameter is a ROS parameter under its own name, declared as a string --
+ * that is how RTAB-Map's own parameter map stores them, whatever the value looks like.
+ * The odometry ones are left out: they belong to the odometry nodes, and declaring them
+ * here would suggest that setting them on rtabmap does something.
+ */
+TEST_F(CoreWrapperParametersTest, declares_rtabmap_parameters_as_strings_except_odometry)
+{
+	makeNode();
+
+	ASSERT_TRUE(node_->has_parameter(Parameters::kRtabmapDetectionRate()));
+	EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
+			node_->get_parameter(Parameters::kRtabmapDetectionRate()).get_type());
+	EXPECT_EQ(rclcpp::ParameterType::PARAMETER_STRING,
+			node_->get_parameter(Parameters::kMemIncrementalMemory()).get_type());
+
+	EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
+	EXPECT_FALSE(node_->has_parameter(Parameters::kOdomResetCountdown()));
+	EXPECT_FALSE(node_->has_parameter(Parameters::kOdomF2MMaxSize()));
+}
+
+/**
+ * Two defaults differ from RTAB-Map's own: the occupancy grid is built by default, since
+ * on a robot that is what the map is for, and the working directory is ~/.ros, or
+ * $ROS_HOME, instead of RTAB-Map's own.
+ */
+TEST_F(CoreWrapperParametersTest, builds_the_occupancy_grid_by_default)
+{
+	ASSERT_FALSE(Parameters::defaultRGBDCreateOccupancyGrid())
+			<< "RTAB-Map's own default changed: this test no longer shows a difference";
+
+	makeNode();
+
+	EXPECT_EQ("true", param(Parameters::kRGBDCreateOccupancyGrid()));
+	EXPECT_EQ(dir(), param(Parameters::kRtabmapWorkingDirectory()));
+}
+
+TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_set_as_ros_parameters)
+{
+	makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.45"),
+			  rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.3")});
+
+	EXPECT_EQ("0.45", param(Parameters::kMemRehearsalSimilarity()));
+	EXPECT_EQ("0.3", param(Parameters::kRGBDLinearUpdate()));
+}
+
+/**
+ * The declared type is string, so a value given with its natural type is refused at
+ * construction rather than silently converted: the quoting in `-p "Rtabmap/DetectionRate:='2'"`
+ * is not optional.
+ */
+TEST_F(CoreWrapperParametersTest, refuses_a_rtabmap_parameter_given_as_a_number)
+{
+	rclcpp::NodeOptions options;
+	options.parameter_overrides(defaultParameters(
+			{rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), 0.3)}));
+	EXPECT_ANY_THROW(std::make_shared<rtabmap_slam::CoreWrapper>(options));
+}
+
+TEST_F(CoreWrapperParametersTest, applies_rtabmap_parameters_passed_as_arguments)
+{
+	makeNode({}, {"--Mem/RehearsalSimilarity", "0.21"});
+
+	EXPECT_EQ("0.21", param(Parameters::kMemRehearsalSimilarity()));
+}
+
+/**
+ * config_path is an INI file of RTAB-Map parameters, read at startup. The odometry
+ * parameters in it are ignored, like everywhere else on this node, and ROS parameters
+ * set explicitly win over the file.
+ */
+TEST_F(CoreWrapperParametersTest, loads_parameters_from_config_path)
+{
+	const std::string ini = dir() + "/config.ini";
+	{
+		std::ofstream out(ini);
+		out << "[Core]\n"
+			<< "Mem/RehearsalSimilarity = 0.44\n"
+			<< "RGBD/LinearUpdate = 0.7\n"
+			<< "Odom/Strategy = 1\n";
+	}
+
+	makeNode({rclcpp::Parameter("config_path", ini),
+			  rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.2")});
+
+	EXPECT_EQ("0.44", param(Parameters::kMemRehearsalSimilarity()));
+	EXPECT_EQ("0.2", param(Parameters::kRGBDLinearUpdate()));
+	EXPECT_FALSE(node_->has_parameter(Parameters::kOdomStrategy()));
+}
+
+/// The node writes its parameters back to config_path when it shuts down.
+TEST_F(CoreWrapperParametersTest, saves_parameters_to_config_path_on_shutdown)
+{
+	const std::string ini = dir() + "/generated.ini";
+	ASSERT_FALSE(UFile::exists(ini));
+
+	makeNode({rclcpp::Parameter("config_path", ini),
+			  rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.37")});
+	destroyNode();
+
+	ASSERT_TRUE(UFile::exists(ini));
+	rtabmap::ParametersMap saved;
+	Parameters::readINI(ini, saved);
+	ASSERT_TRUE(saved.count(Parameters::kMemRehearsalSimilarity()));
+	EXPECT_EQ("0.37", saved.at(Parameters::kMemRehearsalSimilarity()));
+}
+
+/**
+ * A database remembers the parameters it was built with, and reopening it without
+ * setting them again brings them back: a map made with a given configuration is reopened
+ * with that configuration. What is set explicitly still wins.
+ */
+TEST_F(CoreWrapperParametersTest, reuses_the_parameters_stored_in_the_database)
+{
+	makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33"),
+			  rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.25")});
+	destroyNode();
+	ASSERT_TRUE(UFile::exists(databasePath()));
+
+	makeNode({rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "0.15")});
+
+	EXPECT_EQ("0.33", param(Parameters::kMemRehearsalSimilarity()));
+	EXPECT_EQ("0.15", param(Parameters::kRGBDLinearUpdate()));
+}
+
+/// delete_db_on_start starts over: a new, empty database, with none of the old parameters.
+TEST_F(CoreWrapperParametersTest, delete_db_on_start_forgets_the_stored_parameters)
+{
+	makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
+	destroyNode();
+
+	makeNode({rclcpp::Parameter("delete_db_on_start", true)});
+
+	EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
+			param(Parameters::kMemRehearsalSimilarity()));
+}
+
+/// `-d` and `--delete_db_on_start` as arguments do the same, the form launch files used.
+TEST_F(CoreWrapperParametersTest, delete_db_on_start_can_be_passed_as_an_argument)
+{
+	makeNode({rclcpp::Parameter(Parameters::kMemRehearsalSimilarity(), "0.33")});
+	destroyNode();
+
+	makeNode({}, {"-d"});
+
+	EXPECT_EQ(rtabmapDefault(Parameters::kMemRehearsalSimilarity()),
+			param(Parameters::kMemRehearsalSimilarity()));
+}
+
+/**
+ * With no camera subscribed, there is nothing to extract visual words from: bag-of-words
+ * loop closure detection is switched off rather than left to fail on every frame.
+ */
+TEST_F(CoreWrapperParametersTest, odometry_only_input_disables_bag_of_words)
+{
+	makeNode();
+
+	EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
+	EXPECT_EQ(rtabmapDefault(Parameters::kRegStrategy()), param(Parameters::kRegStrategy()));
+}
+
+/**
+ * With a 2D lidar and no camera, the node reconfigures itself for it: the grid is built
+ * from the scan without a range limit, loop closures are registered with ICP, proximity
+ * detection merges the last 10 scans, and bag-of-words is off.
+ */
+TEST_F(CoreWrapperParametersTest, laser_scan_input_switches_to_icp_and_scan_grid)
+{
+	makeNode({rclcpp::Parameter("subscribe_scan", true)});
+
+	EXPECT_EQ("0", param(Parameters::kGridSensor()));
+	EXPECT_EQ("0", param(Parameters::kGridRangeMax()));
+	EXPECT_EQ("1", param(Parameters::kRegStrategy()));
+	EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
+	EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
+}
+
+/// None of those adjustments overrides a value set explicitly.
+TEST_F(CoreWrapperParametersTest, laser_scan_adjustments_keep_explicit_values)
+{
+	makeNode({rclcpp::Parameter("subscribe_scan", true),
+			  rclcpp::Parameter(Parameters::kGridSensor(), "1"),
+			  rclcpp::Parameter(Parameters::kRGBDProximityPathMaxNeighbors(), "3")});
+
+	EXPECT_EQ("1", param(Parameters::kGridSensor()));
+	EXPECT_EQ(rtabmapDefault(Parameters::kGridRangeMax()), param(Parameters::kGridRangeMax()))
+			<< "the range limit is only lifted for a grid built from the scan";
+	EXPECT_EQ("3", param(Parameters::kRGBDProximityPathMaxNeighbors()));
+}
+
+/**
+ * A 3D lidar gets the same treatment, with one difference: proximity detection registers
+ * against the single nearest scan rather than merging ten.
+ */
+TEST_F(CoreWrapperParametersTest, scan_cloud_input_switches_to_icp)
+{
+	makeNode({rclcpp::Parameter("subscribe_scan_cloud", true)});
+
+	EXPECT_EQ("0", param(Parameters::kGridSensor()));
+	EXPECT_EQ("1", param(Parameters::kRegStrategy()));
+	EXPECT_EQ("1", param(Parameters::kRGBDProximityPathMaxNeighbors()));
+	EXPECT_EQ("-1", param(Parameters::kKpMaxFeatures()));
+}
+
+/**
+ * A cloud flagged as 2D -- a 2D lidar published as a cloud -- is treated like a laser
+ * scan, merging ten, whether ICP was selected explicitly or by the node's own switch to it
+ * for lack of a camera.
+ */
+TEST_F(CoreWrapperParametersTest, scan_cloud_flagged_2d_merges_scans_like_a_laser_scan)
+{
+	makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
+			  rclcpp::Parameter("scan_cloud_is_2d", true),
+			  rclcpp::Parameter(Parameters::kRegStrategy(), "1")});
+	EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors())) << "ICP selected explicitly";
+	destroyNode();
+
+	makeNode({rclcpp::Parameter("subscribe_scan_cloud", true),
+			  rclcpp::Parameter("scan_cloud_is_2d", true),
+			  rclcpp::Parameter("delete_db_on_start", true)});
+	EXPECT_EQ("1", param(Parameters::kRegStrategy())) << "switched to ICP by the node";
+	EXPECT_EQ("10", param(Parameters::kRGBDProximityPathMaxNeighbors()));
+}
+
+/**
+ * A parameter RTAB-Map has renamed is still honoured under its old name, with a warning,
+ * so that an old launch file keeps working. Old names are never declared, so this is only
+ * possible by reading them from the overrides.
+ */
+TEST_F(CoreWrapperParametersTest, migrates_a_renamed_parameter)
+{
+	// g2o/PixelVariance became Optimizer/PixelVariance.
+	ASSERT_TRUE(Parameters::getRemovedParameters().count("g2o/PixelVariance"));
+	makeNode({rclcpp::Parameter("g2o/PixelVariance", "2.5")});
+
+	EXPECT_EQ("2.5", param(Parameters::kOptimizerPixelVariance()));
+}
+
+/**
+ * Loaded in a component container with intra-process communication on, the node must
+ * still start. Intra-process communication doesn't support transient local durability,
+ * so the latched publishers (`latch`, on by default) opt out of it, while the others keep
+ * the container's setting.
+ */
+TEST_F(CoreWrapperParametersTest, starts_with_intra_process_comms_whether_latching_or_not)
+{
+	for(bool latch : {true, false})
+	{
+		SCOPED_TRACE(latch ? "latch" : "no latch");
+		rclcpp::NodeOptions options;
+		options.use_intra_process_comms(true);
+		options.parameter_overrides(defaultParameters({rclcpp::Parameter("latch", latch)}));
+		ASSERT_NO_THROW(node_ = addNode(std::make_shared<rtabmap_slam::CoreWrapper>(options)));
+
+		for(const std::string & topic : {std::string("mapGraph"), std::string("map")})
+		{
+			auto infos = node_->get_publishers_info_by_topic(node_->get_node_topics_interface()->resolve_topic_name(topic));
+			ASSERT_EQ(1u, infos.size()) << topic;
+			EXPECT_EQ(latch ? rclcpp::DurabilityPolicy::TransientLocal : rclcpp::DurabilityPolicy::Volatile,
+					infos[0].qos_profile().durability()) << topic;
+		}
+		destroyNode();
+	}
+}
+
+}  // namespace
+
+}  // namespace rtabmap_slam_test
diff --git a/rtabmap_slam/test/test_core_wrapper_planning.cpp b/rtabmap_slam/test/test_core_wrapper_planning.cpp
new file mode 100644
index 00000000..3101e495
--- /dev/null
+++ b/rtabmap_slam/test/test_core_wrapper_planning.cpp
@@ -0,0 +1,400 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <nav_msgs/srv/get_plan.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include <rtabmap_msgs/msg/goal.hpp>
+#include <rtabmap_msgs/msg/path.hpp>
+#include <rtabmap_msgs/srv/get_plan.hpp>
+#include <rtabmap_msgs/srv/set_goal.hpp>
+#include <rtabmap_msgs/srv/set_label.hpp>
+
+#include <rtabmap/core/Parameters.h>
+
+#include "core_wrapper_fixture.hpp"
+
+namespace rtabmap_slam_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+/**
+ * Planning happens on the graph: a goal is a node (or a pose near one), the plan is the
+ * chain of nodes leading to it, and the node hands the next one to reach to a local
+ * planner on goal_out. These tests drive a straight corridor, x = 0 to 2 m in 0.5 m steps,
+ * and plan back along it.
+ */
+class CoreWrapperPlanningTest : public CoreWrapperTest
+{
+protected:
+	void SetUp() override
+	{
+		CoreWrapperTest::SetUp();
+		makeNode(nodeParameters());
+		goalOut_ = collect<geometry_msgs::msg::PoseStamped>("goal_out");
+		goalReached_ = collect<std_msgs::msg::Bool>("goal_reached");
+		globalPath_ = collect<nav_msgs::msg::Path>("global_path");
+		globalPathNodes_ = collect<rtabmap_msgs::msg::Path>("global_path_nodes");
+		info_ = collectInfo();
+		odom_ = odomPublisher();
+		ASSERT_TRUE(waitForPublisher(goalOut_->subscription));
+		ASSERT_TRUE(waitForPublisher(goalReached_->subscription));
+		ASSERT_TRUE(waitForPublisher(globalPath_->subscription));
+		ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription));
+		driveStraight(odom_, info_, 5);   // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0
+		nextStamp_ = 6.0;
+	}
+
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr setGoal(int id, const std::string & label = "")
+	{
+		rtabmap_msgs::srv::SetGoal::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::SetGoal::Request>();
+		req->node_id = id;
+		req->node_label = label;
+		return call<rtabmap_msgs::srv::SetGoal>("set_goal", req);
+	}
+
+	virtual std::vector<rclcpp::Parameter> nodeParameters() { return {}; }
+
+	/// Moves the robot to @p x and waits for the update to be processed.
+	bool moveTo(double x)
+	{
+		const size_t before = info_->size();
+		sendOdom(odom_, nextStamp_, x);
+		nextStamp_ += 1.0;
+		return spinUntil([&]() { return info_->size() > before; });
+	}
+
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseStamped>> goalOut_;
+	std::shared_ptr<Collector<std_msgs::msg::Bool>> goalReached_;
+	std::shared_ptr<Collector<nav_msgs::msg::Path>> globalPath_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Path>> globalPathNodes_;
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
+	double nextStamp_ = 0.0;
+};
+
+/**
+ * set_goal plans to a node and returns the path; the next node to reach goes out on
+ * goal_out, and the whole plan on global_path and global_path_nodes.
+ */
+TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_node)
+{
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(1);
+
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_FALSE(res->path_ids.empty());
+	EXPECT_EQ(1, res->path_ids.back());
+	EXPECT_EQ(res->path_ids.size(), res->path_poses.size());
+	EXPECT_NEAR(0.0, res->path_poses.back().position.x, 1e-4);
+
+	ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
+	EXPECT_EQ("map", goalOut_->back().header.frame_id);
+	ASSERT_TRUE(spinUntil([&]() { return !globalPath_->empty() && !globalPathNodes_->empty(); }));
+	EXPECT_EQ(res->path_ids.size(), globalPath_->back().poses.size());
+	EXPECT_EQ(res->path_ids, globalPathNodes_->back().node_ids);
+}
+
+/// The goal can be named by its label instead of its id.
+TEST_F(CoreWrapperPlanningTest, set_goal_plans_to_a_label)
+{
+	rtabmap_msgs::srv::SetLabel::Request::SharedPtr label =
+			std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
+	label->node_id = 2;
+	label->node_label = "kitchen";
+	ASSERT_TRUE(call<rtabmap_msgs::srv::SetLabel>("set_label", label).get() != nullptr);
+
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "kitchen");
+
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_FALSE(res->path_ids.empty());
+	EXPECT_EQ(2, res->path_ids.back());
+}
+
+/// A goal on a node that does not exist fails, and says so on goal_reached.
+TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_node)
+{
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(42);
+
+	ASSERT_TRUE(res.get() != nullptr);
+	EXPECT_TRUE(res->path_ids.empty());
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_FALSE(goalReached_->back().data);
+}
+
+TEST_F(CoreWrapperPlanningTest, reports_failure_for_an_unknown_label)
+{
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(0, "nowhere");
+
+	ASSERT_TRUE(res.get() != nullptr);
+	EXPECT_TRUE(res->path_ids.empty());
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_FALSE(goalReached_->back().data);
+}
+
+/// A goal on the node the robot is already at is reached straight away.
+TEST_F(CoreWrapperPlanningTest, reports_a_goal_already_reached)
+{
+	rtabmap_msgs::srv::SetGoal::Response::SharedPtr res = setGoal(5);
+
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_TRUE(goalReached_->back().data);
+}
+
+/**
+ * The plan is followed as the robot moves: once it is back at the goal node,
+ * goal_reached says so and the goal is cleared.
+ *
+ * The last step stops 5 cm short of the origin on purpose: an odometry pose of exactly
+ * identity after a non-identity one is how an odometry reset looks, and would start a new
+ * map instead.
+ */
+TEST_F(CoreWrapperPlanningTest, reports_the_goal_reached_when_the_robot_gets_there)
+{
+	ASSERT_TRUE(setGoal(1).get() != nullptr);
+	ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
+	ASSERT_TRUE(goalReached_->empty());
+
+	for(double x : {1.5, 1.0, 0.5, 0.05})
+	{
+		ASSERT_TRUE(moveTo(x));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_TRUE(goalReached_->back().data);
+}
+
+/// cancel_goal abandons the plan, which counts as not reaching it.
+TEST_F(CoreWrapperPlanningTest, cancel_goal_abandons_the_plan)
+{
+	ASSERT_TRUE(setGoal(1).get() != nullptr);
+	ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
+
+	ASSERT_TRUE(callEmpty("cancel_goal"));
+
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_FALSE(goalReached_->back().data);
+	const size_t sent = goalOut_->size();
+	ASSERT_TRUE(moveTo(1.5));
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(sent, goalOut_->size()) << "no new goal after cancelling";
+}
+
+/**
+ * A pose on the goal topic within RGBD/LocalRadius of the robot is not planned through
+ * the graph at all: the plan is the node the robot is at, followed by the pose itself as
+ * a last waypoint with node id 0, and it is left to the local planner to get there.
+ */
+TEST_F(CoreWrapperPlanningTest, plans_to_a_pose_on_the_goal_topic)
+{
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
+			helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
+	ASSERT_TRUE(waitForSubscriber(goal));
+
+	geometry_msgs::msg::PoseStamped pose;
+	pose.header.frame_id = "map";
+	pose.pose.position.x = 0.1;
+	pose.pose.orientation.w = 1.0;
+	goal->publish(pose);
+
+	ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
+	EXPECT_EQ(std::vector<int>({5, 0}), globalPathNodes_->back().node_ids);
+	EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
+	ASSERT_TRUE(spinUntil([&]() { return !goalOut_->empty(); }));
+}
+
+/**
+ * Beyond RGBD/LocalRadius, a pose goal is planned through the graph to the node nearest
+ * to it, and the pose is appended after that node.
+ */
+TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius)
+{
+	ASSERT_TRUE(node_->set_parameter(
+			rclcpp::Parameter(rtabmap::Parameters::kRGBDLocalRadius(), "1.0")).successful);
+	spinFor(std::chrono::milliseconds(300));   // applied on the parameter event
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
+			helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
+	ASSERT_TRUE(waitForSubscriber(goal));
+
+	geometry_msgs::msg::PoseStamped pose;
+	pose.header.frame_id = "map";
+	pose.pose.position.x = 0.1;
+	pose.pose.orientation.w = 1.0;
+	goal->publish(pose);
+
+	ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
+	EXPECT_EQ(std::vector<int>({5, 4, 3, 2, 1, 0}), globalPathNodes_->back().node_ids);
+	EXPECT_NEAR(0.1, globalPathNodes_->back().poses.back().position.x, 1e-4);
+}
+
+/**
+ * A goal in a frame the node cannot transform to the map frame is refused rather than
+ * taken as a map-frame pose.
+ */
+/**
+ * The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is
+ * then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the
+ * clock's time. Only for tests whose TF lookups never have to wait: with a clock that only
+ * moves when told to, a lookup waiting for a transform that is not there -- a goal in an
+ * unknown frame, say -- would wait forever.
+ */
+class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest
+{
+protected:
+	std::vector<rclcpp::Parameter> nodeParameters() override
+	{
+		return {rclcpp::Parameter("use_sim_time", true),
+				rclcpp::Parameter("tf_tolerance", 0.0)};
+	}
+
+	/// Sets the node's clock to @p seconds.
+	void setClock(double seconds)
+	{
+		if(!clock_)
+		{
+			clock_ = helper()->create_publisher<rosgraph_msgs::msg::Clock>("/clock", rclcpp::ClockQoS());
+			ASSERT_TRUE(waitForSubscriber(clock_));
+		}
+		rosgraph_msgs::msg::Clock msg;
+		msg.clock = stampOf(seconds);
+		clock_->publish(msg);
+		spinFor(std::chrono::milliseconds(50));
+	}
+
+	rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_;
+};
+
+TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_the_map_frame)
+{
+	// Turn the robot to face +y where it stands, at x = 2.
+	const size_t before = info_->size();
+	sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0);
+	ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }));
+
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
+			helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
+	ASSERT_TRUE(waitForSubscriber(goal));
+
+	// The goal is looked up through map -> odom -> base_link at its stamp: bring the clock
+	// to the turn's stamp, and wait for map -> odom to be published at it.
+	const double stamp = nextStamp_;
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+	setClock(stamp);
+	ASSERT_TRUE(spinUntil([&]() {
+		for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages)
+		{
+			for(const geometry_msgs::msg::TransformStamped & t : msg->transforms)
+			{
+				if(t.child_frame_id == "odom" && rclcpp::Time(t.header.stamp) == stampOf(stamp))
+				{
+					return true;
+				}
+			}
+		}
+		return false; }));
+
+	// 1 m straight ahead of the robot, facing where it faces.
+	geometry_msgs::msg::PoseStamped pose;
+	pose.header.frame_id = "base_link";
+	pose.header.stamp = stampOf(stamp);
+	pose.pose.position.x = 1.0;
+	pose.pose.orientation.w = 1.0;
+	goal->publish(pose);
+
+	ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
+	const geometry_msgs::msg::Pose & target = globalPathNodes_->back().poses.back();
+	EXPECT_EQ(0, globalPathNodes_->back().node_ids.back()) << "the pose itself, last";
+	EXPECT_NEAR(2.0, target.position.x, 1e-3);
+	EXPECT_NEAR(1.0, target.position.y, 1e-3);
+	EXPECT_NEAR(std::sin(M_PI/4.0), target.orientation.z, 1e-3) << "facing +y";
+	EXPECT_NEAR(std::cos(M_PI/4.0), target.orientation.w, 1e-3);
+	EXPECT_TRUE(goalReached_->empty());
+}
+
+TEST_F(CoreWrapperPlanningTest, refuses_a_goal_in_an_unknown_frame)
+{
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =
+			helper()->create_publisher<geometry_msgs::msg::PoseStamped>("goal", 1);
+	ASSERT_TRUE(waitForSubscriber(goal));
+
+	geometry_msgs::msg::PoseStamped pose;
+	pose.header.frame_id = "nowhere";
+	pose.header.stamp = stampOf(5.0);
+	pose.pose.orientation.w = 1.0;
+	goal->publish(pose);
+
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_FALSE(goalReached_->back().data);
+	EXPECT_TRUE(goalOut_->empty());
+}
+
+/// goal_node takes a node id or a label, and refuses a message with neither.
+TEST_F(CoreWrapperPlanningTest, goal_node_topic_plans_to_a_node)
+{
+	rclcpp::Publisher<rtabmap_msgs::msg::Goal>::SharedPtr goal =
+			helper()->create_publisher<rtabmap_msgs::msg::Goal>("goal_node", 1);
+	ASSERT_TRUE(waitForSubscriber(goal));
+
+	rtabmap_msgs::msg::Goal msg;
+	goal->publish(msg);
+	ASSERT_TRUE(spinUntil([&]() { return !goalReached_->empty(); }));
+	EXPECT_FALSE(goalReached_->back().data);
+
+	msg.node_id = 2;
+	goal->publish(msg);
+	ASSERT_TRUE(spinUntil([&]() { return !globalPathNodes_->empty(); }));
+	EXPECT_EQ(2, globalPathNodes_->back().node_ids.back());
+}
+
+/**
+ * get_plan only computes a plan -- nothing is followed and nothing is published -- and
+ * returns it in the goal's frame.
+ */
+TEST_F(CoreWrapperPlanningTest, get_plan_computes_without_following)
+{
+	nav_msgs::srv::GetPlan::Request::SharedPtr req =
+			std::make_shared<nav_msgs::srv::GetPlan::Request>();
+	req->goal.header.frame_id = "map";
+	req->goal.pose.position.x = 0.0;
+	req->goal.pose.orientation.w = 1.0;
+	nav_msgs::srv::GetPlan::Response::SharedPtr res =
+			call<nav_msgs::srv::GetPlan>("get_plan", req);
+
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_FALSE(res->plan.poses.empty());
+	EXPECT_EQ("map", res->plan.header.frame_id);
+	EXPECT_NEAR(0.0, res->plan.poses.back().pose.position.x, 1e-4);
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(goalOut_->empty());
+}
+
+/// get_plan_nodes is the same, with the node ids along the plan, to a node or a pose.
+TEST_F(CoreWrapperPlanningTest, get_plan_nodes_returns_the_node_ids)
+{
+	rtabmap_msgs::srv::GetPlan::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::GetPlan::Request>();
+	req->goal_node = 2;
+	rtabmap_msgs::srv::GetPlan::Response::SharedPtr res =
+			call<rtabmap_msgs::srv::GetPlan>("get_plan_nodes", req);
+
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_FALSE(res->plan.node_ids.empty());
+	EXPECT_EQ(2, res->plan.node_ids.back());
+	EXPECT_EQ(res->plan.node_ids.size(), res->plan.poses.size());
+	EXPECT_TRUE(goalOut_->empty());
+}
+
+}  // namespace
+
+}  // namespace rtabmap_slam_test
diff --git a/rtabmap_slam/test/test_core_wrapper_services.cpp b/rtabmap_slam/test/test_core_wrapper_services.cpp
new file mode 100644
index 00000000..f7ba4322
--- /dev/null
+++ b/rtabmap_slam/test/test_core_wrapper_services.cpp
@@ -0,0 +1,892 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#include <gtest/gtest.h>
+
+#include <algorithm>
+#include <tuple>
+#include <utility>
+
+#include <rtabmap_msgs/msg/rgbd_image.hpp>
+#include <rtabmap_msgs/msg/sensor_data.hpp>
+#include <rtabmap_msgs/srv/add_link.hpp>
+#include <rtabmap_msgs/srv/get_map2.hpp>
+#include <rtabmap_msgs/srv/get_nodes_in_radius.hpp>
+#include <rtabmap_msgs/srv/list_labels.hpp>
+#include <rtabmap_msgs/srv/load_database.hpp>
+#include <rtabmap_msgs/srv/publish_map.hpp>
+#include <rtabmap_msgs/srv/remove_label.hpp>
+#include <rtabmap_msgs/srv/set_label.hpp>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/GlobalDescriptor.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/ULogger.h>
+
+#include <rtabmap_conversions/MsgConversion.h>
+#include <tf2_ros/buffer.hpp>
+
+#include "core_wrapper_fixture.hpp"
+
+namespace rtabmap_slam_test {
+
+namespace {
+
+::testing::Environment * const kRclcppEnv = registerRclcppEnvironment();
+
+using rtabmap::Parameters;
+
+class CoreWrapperServicesTest : public CoreWrapperTest
+{
+protected:
+	/// A node with @p count nodes already in its map, 0.5 m apart along x.
+	void makeMap(int count = 3, const std::vector<rclcpp::Parameter> & params = {})
+	{
+		makeNode(params);
+		info_ = collectInfo();
+		odom_ = odomPublisher();
+		driveStraight(odom_, info_, count);
+	}
+
+	/// Sends one more update, @p x meters along, and waits for it to be processed.
+	bool updateAt(double stamp, double x)
+	{
+		const size_t before = info_->size();
+		sendOdom(odom_, stamp, x);
+		return spinUntil([&]() { return info_->size() > before; });
+	}
+
+	rtabmap_msgs::srv::ListLabels::Response::SharedPtr listLabels()
+	{
+		return call<rtabmap_msgs::srv::ListLabels>("list_labels");
+	}
+
+	bool setLabel(int id, const std::string & label)
+	{
+		rtabmap_msgs::srv::SetLabel::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::SetLabel::Request>();
+		req->node_id = id;
+		req->node_label = label;
+		return call<rtabmap_msgs::srv::SetLabel>("set_label", req).get() != nullptr;
+	}
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info_;
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_;
+};
+
+/// Every service the node offers is advertised under its own name, /rtabmap/<service>.
+TEST_F(CoreWrapperServicesTest, advertises_its_services_under_its_name)
+{
+	makeNode();
+	const std::vector<std::string> expected = {
+		"update_parameters", "reset", "pause", "resume", "load_database",
+		"trigger_new_map", "backup", "detect_more_loop_closures", "global_bundle_adjustment",
+		"cleanup_local_grids", "set_mode_localization", "set_mode_mapping", "get_node_data",
+		"get_map_data", "get_map_data2", "get_map", "get_prob_map", "publish_map",
+		"get_plan", "get_plan_nodes", "set_goal", "cancel_goal", "set_label", "list_labels",
+		"remove_label", "add_link", "get_nodes_in_radius",
+		"log_debug", "log_info", "log_warning", "log_error"};
+
+	std::map<std::string, std::vector<std::string>> advertised;
+	ASSERT_TRUE(spinUntil([&]() {
+		advertised = helper()->get_service_names_and_types_by_node("rtabmap", "/");
+		return advertised.size() >= expected.size(); }));
+	for(const std::string & name : expected)
+	{
+		EXPECT_TRUE(advertised.count("/rtabmap/" + name)) << "/rtabmap/" << name;
+	}
+}
+
+/**
+ * pause stops the node from taking any input at all -- the odometry is dropped, not
+ * queued -- and resume picks up from the next message. The state is mirrored in the
+ * is_rtabmap_paused parameter.
+ */
+TEST_F(CoreWrapperServicesTest, pause_drops_input_until_resume)
+{
+	makeMap(1);
+
+	ASSERT_TRUE(callEmpty("pause"));
+	EXPECT_TRUE(node_->get_parameter("is_rtabmap_paused").as_bool());
+	sendOdom(odom_, 2.0, 0.5);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(1u, info_->size());
+
+	ASSERT_TRUE(callEmpty("resume"));
+	EXPECT_FALSE(node_->get_parameter("is_rtabmap_paused").as_bool());
+	EXPECT_TRUE(updateAt(3.0, 1.0));
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+}
+
+/// is_rtabmap_paused starts the node paused, waiting for a resume.
+TEST_F(CoreWrapperServicesTest, is_rtabmap_paused_starts_paused)
+{
+	makeNode({rclcpp::Parameter("is_rtabmap_paused", true)});
+	info_ = collectInfo();
+	odom_ = odomPublisher();
+
+	sendOdom(odom_, 1.0, 0.0);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_TRUE(info_->empty());
+
+	ASSERT_TRUE(callEmpty("resume"));
+	EXPECT_TRUE(updateAt(2.0, 0.5));
+}
+
+/// reset erases the map, in memory and in the database, and numbering starts over.
+TEST_F(CoreWrapperServicesTest, reset_erases_the_map)
+{
+	makeMap(3);
+
+	ASSERT_TRUE(callEmpty("reset"));
+	EXPECT_TRUE(getGraph().graph.poses_id.empty());
+
+	ASSERT_TRUE(updateAt(10.0, 5.0));
+	EXPECT_EQ(1, info_->back().ref_id);
+}
+
+/// trigger_new_map starts a new session in the same database; the old one is kept.
+TEST_F(CoreWrapperServicesTest, trigger_new_map_starts_a_new_session)
+{
+	makeMap(2);
+
+	ASSERT_TRUE(callEmpty("trigger_new_map"));
+	ASSERT_TRUE(updateAt(10.0, 1.0));
+
+	EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
+}
+
+/**
+ * Labels name nodes, so a goal can be given as "kitchen" rather than as an id. Node 0
+ * means the latest node.
+ */
+TEST_F(CoreWrapperServicesTest, labels_nodes)
+{
+	makeMap(3);
+
+	ASSERT_TRUE(setLabel(1, "kitchen"));
+	ASSERT_TRUE(setLabel(0, "door"));
+
+	rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
+	ASSERT_TRUE(labels.get() != nullptr);
+	ASSERT_EQ(2u, labels->ids.size());
+	EXPECT_EQ(1, labels->ids[0]);
+	EXPECT_EQ("kitchen", labels->labels[0]);
+	EXPECT_EQ(3, labels->ids[1]);
+	EXPECT_EQ("door", labels->labels[1]);
+	EXPECT_EQ("kitchen", getNode(1).label);
+}
+
+TEST_F(CoreWrapperServicesTest, removes_a_label)
+{
+	makeMap(2);
+	ASSERT_TRUE(setLabel(1, "kitchen"));
+	ASSERT_TRUE(setLabel(2, "door"));
+
+	rtabmap_msgs::srv::RemoveLabel::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::RemoveLabel::Request>();
+	req->label = "kitchen";
+	ASSERT_TRUE(call<rtabmap_msgs::srv::RemoveLabel>("remove_label", req).get() != nullptr);
+
+	rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
+	ASSERT_TRUE(labels.get() != nullptr);
+	EXPECT_EQ(std::vector<std::string>({"door"}), labels->labels);
+}
+
+/// A label is unique in the map: setting it on another node is refused.
+TEST_F(CoreWrapperServicesTest, refuses_a_duplicate_label)
+{
+	makeMap(2);
+	ASSERT_TRUE(setLabel(1, "kitchen"));
+	ASSERT_TRUE(setLabel(2, "kitchen"));
+
+	rtabmap_msgs::srv::ListLabels::Response::SharedPtr labels = listLabels();
+	ASSERT_TRUE(labels.get() != nullptr);
+	EXPECT_EQ(std::vector<int>({1}), labels->ids);
+}
+
+/// get_node_data with no id returns the latest node.
+TEST_F(CoreWrapperServicesTest, get_node_data_defaults_to_the_latest_node)
+{
+	makeMap(3);
+
+	rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
+			call<rtabmap_msgs::srv::GetNodeData>("get_node_data");
+	ASSERT_TRUE(res.get() != nullptr);
+	ASSERT_EQ(1u, res->data.size());
+	EXPECT_EQ(3, res->data[0].id);
+	EXPECT_NEAR(1.0, res->data[0].pose.position.x, 1e-4);
+}
+
+//==========================================================================================
+// What each map service returns, payload by payload
+//==========================================================================================
+
+/**
+ * Every kind of data a node can hold, as one of the map services returned it for node 1.
+ * The graph itself (poses, links) is returned whatever is asked for.
+ */
+struct Payloads
+{
+	bool images = false;
+	bool scans = false;
+	bool userData = false;
+	bool grids = false;
+	bool words = false;
+	bool globalDescriptors = false;
+
+	static Payloads of(const rtabmap_msgs::msg::Node & node)
+	{
+		Payloads p;
+		p.images = !node.data.left_compressed.empty() && !node.data.right_compressed.empty();
+		p.scans = !node.data.laser_scan_compressed.empty();
+		p.userData = !node.data.user_data.empty();
+		p.grids = !node.data.grid_obstacles.empty() || !node.data.grid_empty_cells.empty();
+		p.words = !node.word_id_keys.empty();
+		p.globalDescriptors = !node.data.global_descriptors.empty();
+		return p;
+	}
+
+	bool operator==(const Payloads & o) const
+	{
+		return images == o.images && scans == o.scans && userData == o.userData &&
+			   grids == o.grids && words == o.words && globalDescriptors == o.globalDescriptors;
+	}
+};
+
+std::ostream & operator<<(std::ostream & os, const Payloads & p)
+{
+	return os << "{images=" << p.images << " scans=" << p.scans << " user_data=" << p.userData
+			  << " grids=" << p.grids << " words=" << p.words
+			  << " global_descriptors=" << p.globalDescriptors << "}";
+}
+
+/// How the sensor data reaches the node.
+enum class MapInput
+{
+	RgbdAndScan,   ///< rgbd_image and scan, synchronized with odom; user data on user_data_async
+	SensorData     ///< the same data packed in one rtabmap_msgs/SensorData, user data included
+};
+
+std::string toString(MapInput input)
+{
+	return input == MapInput::RgbdAndScan ? "rgbd_and_scan" : "sensor_data";
+}
+
+/**
+ * A map whose nodes carry everything at once: an RGB-D camera, from which visual words
+ * are extracted, with a global descriptor, a 2D lidar, from which the local occupancy
+ * grid is built, and user data. Built from either input, with the same data.
+ */
+class CoreWrapperMapPayloadsBase : public CoreWrapperServicesTest
+{
+protected:
+	static constexpr int kWidth = 320;
+	static constexpr int kHeight = 240;
+	static constexpr double kCameraHeight = 0.3;
+
+	void buildMap(MapInput input, const std::vector<rclcpp::Parameter> & extra = {})
+	{
+		publishStaticTf("laser", 0.1);
+		publishOpticalTf("camera", kCameraHeight);
+
+		std::vector<rclcpp::Parameter> params = {
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_rgb", false),
+			// Set explicitly: the node switches the grid to the scan by itself only when a
+			// scan topic is subscribed, not for a scan inside sensor_data.
+			rclcpp::Parameter(Parameters::kGridSensor(), "0"),
+			rclcpp::Parameter(Parameters::kGridRangeMax(), "0")};
+		if(input == MapInput::RgbdAndScan)
+		{
+			params.push_back(rclcpp::Parameter("subscribe_rgbd", true));
+			params.push_back(rclcpp::Parameter("subscribe_scan", true));
+		}
+		else
+		{
+			params.push_back(rclcpp::Parameter("subscribe_sensor_data", true));
+		}
+		params.insert(params.end(), extra.begin(), extra.end());
+		makeNode(params);
+		info_ = collectInfo();
+		odom_ = odomPublisher();
+
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd;
+		rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan;
+		rclcpp::Publisher<rtabmap_msgs::msg::UserData>::SharedPtr userData;
+		rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr sensorData;
+		if(input == MapInput::RgbdAndScan)
+		{
+			rgbd = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+			scan = helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
+			userData = helper()->create_publisher<rtabmap_msgs::msg::UserData>("user_data_async", 1);
+			ASSERT_TRUE(waitForSubscriber(rgbd));
+			ASSERT_TRUE(waitForSubscriber(scan));
+			ASSERT_TRUE(waitForSubscriber(userData));
+		}
+		else
+		{
+			sensorData = helper()->create_publisher<rtabmap_msgs::msg::SensorData>("sensor_data", 10);
+			ASSERT_TRUE(waitForSubscriber(sensorData));
+		}
+
+		// For packing the scan the way the node converts it: in base_link, from the laser.
+		tf2_ros::Buffer tfBuffer(helper()->get_clock());
+		tfBuffer.setUsingDedicatedThread(true);   // static transform set below, nothing to wait for
+		geometry_msgs::msg::TransformStamped laserTf = makeTransform("base_link", "laser", 0.0, 0.1);
+		tfBuffer.setTransform(laserTf, "test", true);
+
+		for(int i=0; i<2; ++i)
+		{
+			const double stamp = 1.0 + i;
+			const cv::Mat rgb = texturedImage(kWidth, kHeight, 7 + i);
+			const cv::Mat depth = depthImage(kWidth, kHeight);
+			const sensor_msgs::msg::CameraInfo cameraInfo =
+					makeCameraInfo("camera", stamp, kWidth, kHeight, 250.0);
+			const sensor_msgs::msg::LaserScan scanMsg = makeRoomScan("laser", stamp, 0.5*i + 0.1);
+			const rtabmap_msgs::msg::UserData userDataMsg = makeUserData(stamp);
+			const cv::Mat descriptor = cv::Mat::ones(1, 8, CV_32FC1);
+
+			const size_t before = info_->size();
+			if(input == MapInput::RgbdAndScan)
+			{
+				userData->publish(userDataMsg);
+				spinFor(std::chrono::milliseconds(50));
+
+				rtabmap_msgs::msg::RGBDImage msg;
+				msg.header.frame_id = "camera";
+				msg.header.stamp = stampOf(stamp);
+				msg.rgb = makeImage("camera", stamp, rgb, "bgr8");
+				msg.depth = makeImage("camera", stamp, depth, "16UC1");
+				msg.rgb_camera_info = cameraInfo;
+				msg.depth_camera_info = cameraInfo;
+				msg.global_descriptor.header = msg.header;
+				msg.global_descriptor.data = rtabmap::compressData(descriptor);
+
+				sendOdom(odom_, stamp, 0.5*i);
+				rgbd->publish(msg);
+				scan->publish(scanMsg);
+			}
+			else
+			{
+				// Packed with the node's own conversions, as the odometry nodes republish
+				// what they processed on odom_sensor_data/raw.
+				rtabmap::LaserScan laserScan;
+				ASSERT_TRUE(rtabmap_conversions::convertScanMsg(
+						scanMsg, "base_link", "", stampOf(stamp), laserScan, tfBuffer, 0.0));
+				rtabmap::SensorData data(
+						laserScan, rgb, depth,
+						rtabmap_conversions::cameraModelFromROS(cameraInfo,
+								rtabmap_conversions::transformFromGeometryMsg(
+										opticalTransform("camera", kCameraHeight).transform)),
+						0, stamp,
+						rtabmap_conversions::userDataFromROS(userDataMsg));
+				data.setGlobalDescriptors(std::vector<rtabmap::GlobalDescriptor>(
+						1, rtabmap::GlobalDescriptor(0, descriptor)));
+				rtabmap_msgs::msg::SensorData msg;
+				rtabmap_conversions::sensorDataToROS(data, msg, "base_link", true);
+
+				sendOdom(odom_, stamp, 0.5*i);
+				sensorData->publish(msg);
+			}
+			ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }))
+					<< "update " << i << " was not processed";
+		}
+	}
+
+	rtabmap_msgs::msg::MapData getMapData2(const Payloads & asked)
+	{
+		rtabmap_msgs::srv::GetMap2::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetMap2::Request>();
+		req->global_map = true;
+		req->optimized = true;
+		req->with_images = asked.images;
+		req->with_scans = asked.scans;
+		req->with_user_data = asked.userData;
+		req->with_grids = asked.grids;
+		req->with_words = asked.words;
+		req->with_global_descriptors = asked.globalDescriptors;
+		rtabmap_msgs::srv::GetMap2::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetMap2>("get_map_data2", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		return res ? res->data : rtabmap_msgs::msg::MapData();
+	}
+
+	rtabmap_msgs::msg::MapData getMapData(bool graphOnly)
+	{
+		rtabmap_msgs::srv::GetMap::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
+		req->global_map = true;
+		req->optimized = true;
+		req->graph_only = graphOnly;
+		rtabmap_msgs::srv::GetMap::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetMap>("get_map_data", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		return res ? res->data : rtabmap_msgs::msg::MapData();
+	}
+
+	/// Node 1 of @p map, with the graph checked to be complete whatever was asked for.
+	static rtabmap_msgs::msg::Node node1(const rtabmap_msgs::msg::MapData & map)
+	{
+		EXPECT_EQ(2u, map.graph.poses_id.size());
+		EXPECT_EQ("map", map.header.frame_id);
+		for(const rtabmap_msgs::msg::Node & n : map.nodes)
+		{
+			if(n.id == 1)
+			{
+				return n;
+			}
+		}
+		ADD_FAILURE() << "node 1 is missing";
+		return rtabmap_msgs::msg::Node();
+	}
+
+/// All six kinds of payload.
+	static Payloads all()
+	{
+		Payloads p;
+		p.images = p.scans = p.userData = p.grids = p.words = p.globalDescriptors = true;
+		return p;
+	}
+};
+
+/// The payload tests below, run once per input.
+class CoreWrapperMapPayloadsTest :
+	public CoreWrapperMapPayloadsBase,
+	public ::testing::WithParamInterface<MapInput>
+{
+protected:
+	void SetUp() override
+	{
+		CoreWrapperMapPayloadsBase::SetUp();
+		buildMap(GetParam());
+	}
+};
+
+INSTANTIATE_TEST_SUITE_P(Inputs, CoreWrapperMapPayloadsTest,
+		::testing::Values(MapInput::RgbdAndScan, MapInput::SensorData),
+		[](const ::testing::TestParamInfo<MapInput> & info) { return toString(info.param); });
+
+/// The map these tests build does hold every kind of payload, or the tests below prove nothing.
+TEST_P(CoreWrapperMapPayloadsTest, the_map_holds_every_payload)
+{
+	EXPECT_EQ(all(), Payloads::of(node1(getMapData2(all()))));
+}
+
+/**
+ * get_map_data2 returns each kind of payload only when asked for it, so a client that
+ * only needs, say, the scans does not download the images too.
+ */
+TEST_P(CoreWrapperMapPayloadsTest, get_map_data2_returns_only_the_payloads_asked_for)
+{
+	EXPECT_EQ(Payloads(), Payloads::of(node1(getMapData2(Payloads()))));
+
+	const std::vector<std::pair<std::string, bool Payloads::*>> flags = {
+		{"with_images", &Payloads::images},
+		{"with_scans", &Payloads::scans},
+		{"with_user_data", &Payloads::userData},
+		{"with_grids", &Payloads::grids},
+		{"with_words", &Payloads::words},
+		{"with_global_descriptors", &Payloads::globalDescriptors}};
+	for(const auto & flag : flags)
+	{
+		SCOPED_TRACE(flag.first);
+		Payloads asked;
+		asked.*(flag.second) = true;
+		EXPECT_EQ(asked, Payloads::of(node1(getMapData2(asked))));
+	}
+}
+
+/**
+ * get_map_data is get_map_data2 with a single switch: everything, or with graph_only,
+ * nothing but the graph and the nodes' metadata.
+ */
+TEST_P(CoreWrapperMapPayloadsTest, get_map_data_returns_everything_unless_graph_only)
+{
+	EXPECT_EQ(all(), Payloads::of(node1(getMapData(false))));
+
+	rtabmap_msgs::msg::MapData graphOnly = getMapData(true);
+	rtabmap_msgs::msg::Node node = node1(graphOnly);
+	EXPECT_EQ(Payloads(), Payloads::of(node));
+	EXPECT_NEAR(0.0, node.pose.position.x, 1e-4) << "the nodes are still there, without data";
+}
+
+/**
+ * get_node_data selects the images, the scan, the grid and the user data separately. The
+ * visual words and the global descriptors have no switch: they always come along.
+ */
+TEST_P(CoreWrapperMapPayloadsTest, get_node_data_returns_only_the_payloads_asked_for)
+{
+	const auto getNodeData = [&](bool images, bool scan, bool grid, bool userData) {
+		rtabmap_msgs::srv::GetNodeData::Request::SharedPtr req =
+				std::make_shared<rtabmap_msgs::srv::GetNodeData::Request>();
+		req->ids = {1};
+		req->images = images;
+		req->scan = scan;
+		req->grid = grid;
+		req->user_data = userData;
+		rtabmap_msgs::srv::GetNodeData::Response::SharedPtr res =
+				call<rtabmap_msgs::srv::GetNodeData>("get_node_data", req);
+		EXPECT_TRUE(res.get() != nullptr);
+		EXPECT_TRUE(res && res->data.size() == 1u);
+		return res && !res->data.empty() ? Payloads::of(res->data[0]) : Payloads();
+	};
+
+	Payloads alwaysThere;
+	alwaysThere.words = true;
+	alwaysThere.globalDescriptors = true;
+
+	EXPECT_EQ(alwaysThere, getNodeData(false, false, false, false));
+	{
+		SCOPED_TRACE("images");
+		Payloads expected = alwaysThere;
+		expected.images = true;
+		EXPECT_EQ(expected, getNodeData(true, false, false, false));
+	}
+	{
+		SCOPED_TRACE("scan");
+		Payloads expected = alwaysThere;
+		expected.scans = true;
+		EXPECT_EQ(expected, getNodeData(false, true, false, false));
+	}
+	{
+		SCOPED_TRACE("grid");
+		Payloads expected = alwaysThere;
+		expected.grids = true;
+		EXPECT_EQ(expected, getNodeData(false, false, true, false));
+	}
+	{
+		SCOPED_TRACE("user_data");
+		Payloads expected = alwaysThere;
+		expected.userData = true;
+		EXPECT_EQ(expected, getNodeData(false, false, false, true));
+	}
+	EXPECT_EQ(all(), getNodeData(true, true, true, true));
+}
+
+class CoreWrapperMapInputsTest : public CoreWrapperMapPayloadsBase
+{
+protected:
+	static void expectSameMat(const cv::Mat & a, const cv::Mat & b, const std::string & what,
+			bool mayBeEmpty = false, double tolerance = 0.0)
+	{
+		if(!mayBeEmpty)
+		{
+			EXPECT_FALSE(a.empty()) << what << " is empty, so comparing it proves nothing";
+		}
+		ASSERT_EQ(a.empty(), b.empty()) << what;
+		if(a.empty())
+		{
+			return;
+		}
+		ASSERT_EQ(a.size(), b.size()) << what;
+		ASSERT_EQ(a.type(), b.type()) << what;
+		EXPECT_LE(cv::norm(a, b, cv::NORM_INF), tolerance) << what;
+	}
+
+	/// The words' keypoints of @p node, in pixels, sorted.
+	static std::vector<std::pair<float, float>> keypoints(const rtabmap_msgs::msg::Node & node)
+	{
+		std::vector<std::pair<float, float>> out;
+		for(const rtabmap_msgs::msg::KeyPoint & k : node.word_kpts)
+		{
+			out.push_back(std::make_pair(k.pt.x, k.pt.y));
+		}
+		std::sort(out.begin(), out.end());
+		return out;
+	}
+
+	/// The words' 3D points of @p node, sorted.
+	static std::vector<std::tuple<float, float, float>> points(const rtabmap_msgs::msg::Node & node)
+	{
+		std::vector<std::tuple<float, float, float>> out;
+		for(const rtabmap_msgs::msg::Point3f & p : node.word_pts)
+		{
+			out.push_back(std::make_tuple(p.x, p.y, p.z));
+		}
+		std::sort(out.begin(), out.end());
+		return out;
+	}
+
+	/// Node @p a and node @p b hold the same data, down to the pixel and the point.
+	static void expectSameNode(const rtabmap_msgs::msg::Node & a, const rtabmap_msgs::msg::Node & b)
+	{
+		SCOPED_TRACE("node " + std::to_string(a.id));
+		EXPECT_EQ(a.map_id, b.map_id);
+		EXPECT_DOUBLE_EQ(a.stamp, b.stamp);
+		EXPECT_NEAR(a.pose.position.x, b.pose.position.x, 1e-6);
+
+		rtabmap::SensorData da = rtabmap_conversions::sensorDataFromROS(a.data);
+		rtabmap::SensorData db = rtabmap_conversions::sensorDataFromROS(b.data);
+		cv::Mat rgbA, depthA, userA, groundA, obstaclesA, emptyA;
+		cv::Mat rgbB, depthB, userB, groundB, obstaclesB, emptyB;
+		rtabmap::LaserScan scanA, scanB;
+		da.uncompressData(&rgbA, &depthA, &scanA, &userA, &groundA, &obstaclesA, &emptyA);
+		db.uncompressData(&rgbB, &depthB, &scanB, &userB, &groundB, &obstaclesB, &emptyB);
+
+		expectSameMat(rgbA, rgbB, "rgb");
+		expectSameMat(depthA, depthB, "depth");
+		ASSERT_EQ(1u, da.cameraModels().size());
+		ASSERT_EQ(1u, db.cameraModels().size());
+		EXPECT_DOUBLE_EQ(da.cameraModels()[0].fx(), db.cameraModels()[0].fx());
+		EXPECT_DOUBLE_EQ(da.cameraModels()[0].cx(), db.cameraModels()[0].cx());
+		EXPECT_EQ(da.cameraModels()[0].imageSize(), db.cameraModels()[0].imageSize());
+		EXPECT_EQ(da.cameraModels()[0].localTransform().prettyPrint(),
+				  db.cameraModels()[0].localTransform().prettyPrint());
+
+		// Converted through the odometry frame on one side (odom_sensor_sync) and straight
+		// into base_link on the other: the same points, to float rounding.
+		expectSameMat(scanA.data(), scanB.data(), "scan", false, 1e-5);
+		EXPECT_EQ(scanA.format(), scanB.format());
+		EXPECT_EQ(scanA.maxPoints(), scanB.maxPoints());
+		EXPECT_FLOAT_EQ(scanA.rangeMax(), scanB.rangeMax());
+		EXPECT_EQ(scanA.localTransform().prettyPrint(), scanB.localTransform().prettyPrint());
+
+		expectSameMat(userA, userB, "user data");
+
+		EXPECT_FLOAT_EQ(da.gridCellSize(), db.gridCellSize());
+		expectSameMat(obstaclesA, obstaclesB, "grid obstacles", false, 1e-5);
+		expectSameMat(emptyA, emptyB, "grid empty cells", false, 1e-5);
+		expectSameMat(groundA, groundB, "grid ground", true, 1e-5);
+
+		// The same features are extracted, but not necessarily given the same word ids:
+		// matching them against the dictionary is approximate, and the latest node's ids
+		// differ from one run to the next even with the same input. So the keypoints and
+		// their 3D points are compared, as sets.
+		EXPECT_FALSE(a.word_kpts.empty());
+		EXPECT_EQ(a.word_id_keys.size(), b.word_id_keys.size());
+		EXPECT_EQ(keypoints(a), keypoints(b));
+		EXPECT_EQ(points(a), points(b));
+
+		ASSERT_EQ(1u, a.data.global_descriptors.size());
+		ASSERT_EQ(1u, b.data.global_descriptors.size());
+		EXPECT_EQ(a.data.global_descriptors[0].type, b.data.global_descriptors[0].type);
+		EXPECT_EQ(a.data.global_descriptors[0].data, b.data.global_descriptors[0].data);
+	}
+};
+
+/**
+ * sensor_data is the same map as rgbd_image and scan, given the same data: every node
+ * stores the same images, calibration, scan, user data, grid, visual words and global
+ * descriptor, whichever way it arrived.
+ */
+TEST_F(CoreWrapperMapInputsTest, sensor_data_maps_like_rgbd_and_scan)
+{
+	buildMap(MapInput::RgbdAndScan);
+	const rtabmap_msgs::msg::MapData viaTopics = getMapData2(all());
+	destroyNode();
+
+	buildMap(MapInput::SensorData, {rclcpp::Parameter("delete_db_on_start", true)});
+	const rtabmap_msgs::msg::MapData viaSensorData = getMapData2(all());
+
+	ASSERT_EQ(2u, viaTopics.nodes.size());
+	ASSERT_EQ(viaTopics.nodes.size(), viaSensorData.nodes.size());
+	for(size_t i=0; i<viaTopics.nodes.size(); ++i)
+	{
+		ASSERT_EQ(viaTopics.nodes[i].id, viaSensorData.nodes[i].id);
+		expectSameNode(viaTopics.nodes[i], viaSensorData.nodes[i]);
+	}
+}
+
+/**
+ * get_nodes_in_radius finds the nodes within a radius of either a node -- not counting
+ * that node -- or a position, which is used when node_id is 0 and it is not the origin.
+ */
+TEST_F(CoreWrapperServicesTest, finds_nodes_in_a_radius)
+{
+	makeMap(4);   // x = 0, 0.5, 1.0, 1.5
+
+	rtabmap_msgs::srv::GetNodesInRadius::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::GetNodesInRadius::Request>();
+	req->node_id = 1;
+	req->radius = 0.6f;
+	rtabmap_msgs::srv::GetNodesInRadius::Response::SharedPtr res =
+			call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
+	ASSERT_TRUE(res.get() != nullptr);
+	std::vector<int> ids = res->ids;
+	EXPECT_EQ(std::vector<int>({2}), ids);
+	ASSERT_EQ(1u, res->dists_sqr.size());
+	EXPECT_NEAR(0.25, res->dists_sqr[0], 1e-4);
+
+	req->node_id = 0;
+	req->x = 1.4f;
+	res = call<rtabmap_msgs::srv::GetNodesInRadius>("get_nodes_in_radius", req);
+	ASSERT_TRUE(res.get() != nullptr);
+	ids = res->ids;
+	std::sort(ids.begin(), ids.end());
+	EXPECT_EQ(std::vector<int>({3, 4}), ids);
+}
+
+/**
+ * In localization mode the map is not extended: updates are localized against it and
+ * then forgotten. set_mode_mapping goes back to extending it, in a new session, since
+ * nothing links where the robot is now to the map it left. Both are mirrored in the
+ * Mem/IncrementalMemory parameter.
+ */
+TEST_F(CoreWrapperServicesTest, localization_mode_stops_extending_the_map)
+{
+	makeMap(2);
+
+	ASSERT_TRUE(callEmpty("set_mode_localization"));
+	EXPECT_EQ("false", param(Parameters::kMemIncrementalMemory()));
+	ASSERT_TRUE(updateAt(10.0, 3.0));
+	ASSERT_TRUE(updateAt(11.0, 3.5));
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+
+	ASSERT_TRUE(callEmpty("set_mode_mapping"));
+	EXPECT_EQ("true", param(Parameters::kMemIncrementalMemory()));
+	ASSERT_TRUE(updateAt(12.0, 4.0));
+	EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
+}
+
+/**
+ * RTAB-Map parameters can be changed while the node runs, with `ros2 param set`: the
+ * node applies them as soon as they change.
+ */
+TEST_F(CoreWrapperServicesTest, applies_parameters_changed_at_runtime)
+{
+	makeMap(1);
+
+	ASSERT_TRUE(node_->set_parameter(
+			rclcpp::Parameter(Parameters::kRGBDLinearUpdate(), "2.0")).successful);
+	spinFor(std::chrono::milliseconds(300));   // the change arrives as a parameter event
+	ASSERT_TRUE(updateAt(2.0, 0.5));
+	ASSERT_TRUE(updateAt(3.0, 1.0));
+
+	EXPECT_EQ(1u, getGraph().graph.poses_id.size())
+			<< "0.5 m steps are now below the 2 m linear update";
+}
+
+/**
+ * backup saves the database as it is now to <database_path>.back, reloads it, and carries
+ * on in a new session, as after a restart.
+ */
+TEST_F(CoreWrapperServicesTest, backup_copies_the_database)
+{
+	makeMap(2);
+
+	ASSERT_TRUE(callEmpty("backup"));
+
+	EXPECT_TRUE(UFile::exists(databasePath() + ".back"));
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+	ASSERT_TRUE(updateAt(10.0, 2.0));
+	EXPECT_EQ(std::vector<int>({0, 0, 1}), mapIds());
+}
+
+/**
+ * load_database saves the current map and switches to another database -- a new one, or
+ * one whose map is reloaded. clear starts the target over.
+ */
+TEST_F(CoreWrapperServicesTest, load_database_switches_maps)
+{
+	makeMap(2);
+
+	rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
+	req->database_path = dir() + "/other.db";
+	req->clear = true;
+	ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
+	EXPECT_TRUE(getGraph().graph.poses_id.empty());
+	ASSERT_TRUE(updateAt(10.0, 0.0));
+	EXPECT_EQ(1, info_->back().ref_id);
+
+	req->database_path = databasePath();
+	req->clear = false;
+	ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+	EXPECT_TRUE(UFile::exists(dir() + "/other.db"));
+}
+
+/// A database path in a directory that does not exist is refused, and the map is kept.
+TEST_F(CoreWrapperServicesTest, load_database_refuses_a_missing_directory)
+{
+	makeMap(2);
+
+	rtabmap_msgs::srv::LoadDatabase::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::LoadDatabase::Request>();
+	req->database_path = dir() + "/no/such/dir/other.db";
+	ASSERT_TRUE(call<rtabmap_msgs::srv::LoadDatabase>("load_database", req).get() != nullptr);
+
+	EXPECT_EQ(2u, getGraph().graph.poses_id.size());
+}
+
+/**
+ * publish_map republishes the map on demand to whatever is subscribed -- the whole
+ * database's with global_map, and just the graph with graph_only.
+ */
+TEST_F(CoreWrapperServicesTest, publish_map_republishes_on_demand)
+{
+	makeMap(3);
+	std::shared_ptr<Collector<rtabmap_msgs::msg::MapGraph>> graph =
+			collect<rtabmap_msgs::msg::MapGraph>("mapGraph",
+					rclcpp::QoS(1).reliable().transient_local());
+	ASSERT_TRUE(waitForPublisher(graph->subscription));
+	spinFor(std::chrono::milliseconds(200));
+	const size_t before = graph->size();
+
+	rtabmap_msgs::srv::PublishMap::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::PublishMap::Request>();
+	req->global_map = true;
+	req->optimized = true;
+	req->graph_only = true;
+	ASSERT_TRUE(call<rtabmap_msgs::srv::PublishMap>("publish_map", req).get() != nullptr);
+
+	ASSERT_TRUE(spinUntil([&]() { return graph->size() > before; }));
+	EXPECT_EQ(3u, graph->back().poses_id.size());
+}
+
+/**
+ * add_link adds a constraint from outside -- a loop closure found by another process,
+ * say -- to the graph, which is then optimized with it.
+ */
+TEST_F(CoreWrapperServicesTest, add_link_adds_a_constraint)
+{
+	makeMap(3);
+
+	rtabmap_msgs::srv::AddLink::Request::SharedPtr req =
+			std::make_shared<rtabmap_msgs::srv::AddLink::Request>();
+	req->link.from_id = 3;
+	req->link.to_id = 1;
+	req->link.type = rtabmap::Link::kUserClosure;
+	req->link.transform.translation.x = -1.0;
+	req->link.transform.rotation.w = 1.0;
+	for(int i=0; i<6; ++i)
+	{
+		req->link.information[i*7] = 100.0;
+	}
+	ASSERT_TRUE(call<rtabmap_msgs::srv::AddLink>("add_link", req).get() != nullptr);
+
+	bool found = false;
+	for(const rtabmap_msgs::msg::Link & l : getGraph().graph.links)
+	{
+		found = found || (l.type == rtabmap::Link::kUserClosure &&
+				((l.from_id == 3 && l.to_id == 1) || (l.from_id == 1 && l.to_id == 3)));
+	}
+	EXPECT_TRUE(found);
+}
+
+/// The log_* services set RTAB-Map's own log level, independently from ROS's.
+TEST_F(CoreWrapperServicesTest, log_services_set_rtabmap_log_level)
+{
+	makeNode();
+	const ULogger::Level initial = ULogger::level();
+
+	ASSERT_TRUE(callEmpty("log_debug"));
+	EXPECT_EQ(ULogger::kDebug, ULogger::level());
+	ASSERT_TRUE(callEmpty("log_info"));
+	EXPECT_EQ(ULogger::kInfo, ULogger::level());
+	ASSERT_TRUE(callEmpty("log_error"));
+	EXPECT_EQ(ULogger::kError, ULogger::level());
+	ASSERT_TRUE(callEmpty("log_warning"));
+	EXPECT_EQ(ULogger::kWarning, ULogger::level());
+
+	ULogger::setLevel(initial);
+}
+
+}  // namespace
+
+}  // namespace rtabmap_slam_test
diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt
index 106d23cf..5e1b4da8 100644
--- a/rtabmap_sync/CMakeLists.txt
+++ b/rtabmap_sync/CMakeLists.txt
@@ -2,16 +2,17 @@ cmake_minimum_required(VERSION 3.5)
 project(rtabmap_sync)
 
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
-  # issues #1285 #1288
+  # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under
+  # emulated arm64 should not fail the build)
   find_library(
     builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
     PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
   find_library(
     crypto_LIB NAMES crypto
     PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
 endif()
 
@@ -196,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)
diff --git a/rtabmap_sync/README.md b/rtabmap_sync/README.md
new file mode 100644
index 00000000..7a46c4eb
--- /dev/null
+++ b/rtabmap_sync/README.md
@@ -0,0 +1,75 @@
+# 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.
+
+## Contents
+
+- [Nodes](#nodes)
+- [Library](#library)
+- [Conventions](#conventions)
+- [Build options](#build-options)
+- [License](#license)
+
+## 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.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).
diff --git a/rtabmap_sync/doc/rgb_sync.md b/rtabmap_sync/doc/rgb_sync.md
new file mode 100644
index 00000000..54b48bdc
--- /dev/null
+++ b/rtabmap_sync/doc/rgb_sync.md
@@ -0,0 +1,100 @@
+# 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
+```
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [fill_empty_depth](#fill_empty_depth)
+- [Synchronization](#synchronization)
+- [Diagnostics](#diagnostics)
+
+## 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.
diff --git a/rtabmap_sync/doc/rgbd_sync.md b/rtabmap_sync/doc/rgbd_sync.md
new file mode 100644
index 00000000..f32e13fc
--- /dev/null
+++ b/rtabmap_sync/doc/rgbd_sync.md
@@ -0,0 +1,176 @@
+# 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).
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Synchronization](#synchronization)
+- [Depth units](#depth-units)
+- [Compressing for a slow link](#compressing-for-a-slow-link)
+- [Decimation](#decimation)
+- [Diagnostics](#diagnostics)
+
+## 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.
diff --git a/rtabmap_sync/doc/rgbdx_sync.md b/rtabmap_sync/doc/rgbdx_sync.md
new file mode 100644
index 00000000..2335cd8f
--- /dev/null
+++ b/rtabmap_sync/doc/rgbdx_sync.md
@@ -0,0 +1,156 @@
+# 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
+```
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Order matters](#order-matters)
+- [Synchronization](#synchronization)
+- [Feeding it to rtabmap](#feeding-it-to-rtabmap)
+- [Diagnostics](#diagnostics)
+
+## 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.
diff --git a/rtabmap_sync/doc/stereo_sync.md b/rtabmap_sync/doc/stereo_sync.md
new file mode 100644
index 00000000..8d790179
--- /dev/null
+++ b/rtabmap_sync/doc/stereo_sync.md
@@ -0,0 +1,146 @@
+# 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
+```
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [How a stereo pair travels in an RGBDImage](#how-a-stereo-pair-travels-in-an-rgbdimage)
+- [Synchronization](#synchronization)
+- [Compressing for a slow link](#compressing-for-a-slow-link)
+- [Diagnostics](#diagnostics)
+
+## 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.
diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
index 8fb31a6d..1282a4c6 100644
--- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+++ b/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:
diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h
index 609af84b..c5082f53 100644
--- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h
+++ b/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; \
diff --git a/rtabmap_sync/include/rtabmap_sync/GetTopicName.h b/rtabmap_sync/include/rtabmap_sync/GetTopicName.h
deleted file mode 100644
index 153fbc76..00000000
--- a/rtabmap_sync/include/rtabmap_sync/GetTopicName.h
+++ /dev/null
@@ -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_ */
diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
index f9411519..1bec4c39 100644
--- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+++ b/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) {
diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp
index 571ef3a1..1215834f 100644
--- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp
+++ b/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_;
diff --git a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp
index 351c55eb..8c58a0c6 100644
--- a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp
+++ b/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_;
diff --git a/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp
index e49553dd..14454690 100644
--- a/rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp
+++ b/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:
diff --git a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp
index 8f24b785..bf5dc942 100644
--- a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp
+++ b/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_;
diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml
index 39baf6b4..5bdebf72 100644
--- a/rtabmap_sync/package.xml
+++ b/rtabmap_sync/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_sync</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's synchronization package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -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>
diff --git a/rtabmap_sync/rosdoc2.yaml b/rtabmap_sync/rosdoc2.yaml
new file mode 100644
index 00000000..c46ac697
--- /dev/null
+++ b/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: ''
+    }
diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp
index 585d85ad..adab2b2c 100644
--- a/rtabmap_sync/src/nodelets/rgb_sync.cpp
+++ b/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);
diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp
index 7c73a5af..988c3c2b 100644
--- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp
+++ b/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)
 {
diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp
index feb831a1..a688e85c 100644
--- a/rtabmap_sync/src/nodelets/stereo_sync.cpp
+++ b/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)
 {
diff --git a/rtabmap_sync/test/common_data_subscriber_fixture.hpp b/rtabmap_sync/test/common_data_subscriber_fixture.hpp
new file mode 100644
index 00000000..0b01f22f
--- /dev/null
+++ b/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_ */
diff --git a/rtabmap_sync/test/msg_builders.hpp b/rtabmap_sync/test/msg_builders.hpp
new file mode 100644
index 00000000..5b6b86bc
--- /dev/null
+++ b/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_ */
diff --git a/rtabmap_sync/test/node_test_utils.hpp b/rtabmap_sync/test/node_test_utils.hpp
new file mode 100644
index 00000000..ff1c0b78
--- /dev/null
+++ b/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_ */
diff --git a/rtabmap_sync/test/test_common_data_subscriber.cpp b/rtabmap_sync/test/test_common_data_subscriber.cpp
new file mode 100644
index 00000000..bbf2a3fe
--- /dev/null
+++ b/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
diff --git a/rtabmap_sync/test/test_common_data_subscriber_sync.cpp b/rtabmap_sync/test/test_common_data_subscriber_sync.cpp
new file mode 100644
index 00000000..ba2d0f63
--- /dev/null
+++ b/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";
+}
diff --git a/rtabmap_sync/test/test_rgb_sync.cpp b/rtabmap_sync/test/test_rgb_sync.cpp
new file mode 100644
index 00000000..cb2902a3
--- /dev/null
+++ b/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));
+}
diff --git a/rtabmap_sync/test/test_rgbd_sync.cpp b/rtabmap_sync/test/test_rgbd_sync.cpp
new file mode 100644
index 00000000..cc6c5c8d
--- /dev/null
+++ b/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));
+}
diff --git a/rtabmap_sync/test/test_rgbdx_sync.cpp b/rtabmap_sync/test/test_rgbdx_sync.cpp
new file mode 100644
index 00000000..22ab6fe5
--- /dev/null
+++ b/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));
+}
diff --git a/rtabmap_sync/test/test_stereo_sync.cpp b/rtabmap_sync/test/test_stereo_sync.cpp
new file mode 100644
index 00000000..384d194a
--- /dev/null
+++ b/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));
+}
diff --git a/rtabmap_sync/test/test_sync_diagnostic.cpp b/rtabmap_sync/test/test_sync_diagnostic.cpp
new file mode 100644
index 00000000..dfa6dffc
--- /dev/null
+++ b/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));
+}
diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt
index 718e0456..9104af74 100644
--- a/rtabmap_util/CMakeLists.txt
+++ b/rtabmap_util/CMakeLists.txt
@@ -300,4 +300,50 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest 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_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
+      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)
+      if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+        ament_target_dependencies(${test_name} ${AmentLibraries})
+      else()
+        target_link_libraries(${test_name} ${Libraries} ${PublicLibraries})
+      endif()
+    endif()
+  endmacro()
+
+  rtabmap_util_add_node_test(test_imu_to_tf)
+  rtabmap_util_add_node_test(test_disparity_to_depth)
+  rtabmap_util_add_node_test(test_rgbd_relay)
+  rtabmap_util_add_node_test(test_rgbd_split)
+  rtabmap_util_add_node_test(test_lidar_deskewing)
+  rtabmap_util_add_node_test(test_obstacles_detection)
+  rtabmap_util_add_node_test(test_point_cloud_aggregator)
+  rtabmap_util_add_node_test(test_point_cloud_assembler)
+  rtabmap_util_add_node_test(test_point_cloud_xyz)
+  rtabmap_util_add_node_test(test_pointcloud_to_depthimage)
+  rtabmap_util_add_node_test(test_point_cloud_xyzrgb)
+  rtabmap_util_add_node_test(test_db_player)
+  rtabmap_util_add_node_test(test_maps_manager)
+  rtabmap_util_add_node_test(test_map_assembler)
+endif()
+
 ament_package()
diff --git a/rtabmap_util/README.md b/rtabmap_util/README.md
new file mode 100644
index 00000000..db6badd6
--- /dev/null
+++ b/rtabmap_util/README.md
@@ -0,0 +1,118 @@
+# rtabmap_util
+
+Standalone utility nodes for [RTAB-Map](https://github.com/introlab/rtabmap) pipelines: converting between sensor representations, cleaning up point clouds, assembling maps and replaying recorded sessions.
+
+Every node is a [composable node](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Composition.html) as well as a standalone executable. Composing them into one process with their producer avoids copying images and clouds between processes, which is worth doing for anything on the sensor path.
+
+## Contents
+
+- [Nodes](#nodes)
+- [Library](#library)
+  - [MapsManager](#mapsmanager)
+- [Conventions](#conventions)
+
+## Nodes
+
+One page per node.
+
+**Sensor conversion**
+
+| Node | Description |
+|---|---|
+| [disparity_to_depth](doc/disparity_to_depth.md) | Disparity image → depth image, in meters and in millimeters. |
+| [pointcloud_to_depthimage](doc/pointcloud_to_depthimage.md) | Point cloud → depth image registered to an RGB camera. Lets a lidar feed an RGB-D pipeline. |
+| [point_cloud_xyz](doc/point_cloud_xyz.md) | Depth or disparity image → point cloud, with filtering. |
+| [point_cloud_xyzrgb](doc/point_cloud_xyzrgb.md) | RGB-D, stereo or disparity → colored point cloud. |
+| [imu_to_tf](doc/imu_to_tf.md) | IMU orientation → TF. |
+
+**RGBDImage plumbing**
+
+| Node | Description |
+|---|---|
+| [rgbd_relay](doc/rgbd_relay.md) | Republishes an `RGBDImage`, compressing or decompressing on the way. |
+| [rgbd_split](doc/rgbd_split.md) | Splits an `RGBDImage` back into standard `Image` and `CameraInfo` topics. |
+
+**Point cloud processing**
+
+| Node | Description |
+|---|---|
+| [lidar_deskewing](doc/lidar_deskewing.md) | Removes motion distortion from a lidar sweep. |
+| [point_cloud_aggregator](doc/point_cloud_aggregator.md) | Merges one cloud from each of several sensors into one. |
+| [point_cloud_assembler](doc/point_cloud_assembler.md) | Accumulates one sensor over time into a denser cloud. |
+| [obstacles_detection](doc/obstacles_detection.md) | Segments a cloud into ground and obstacles. |
+
+**Maps and replay**
+
+| Node | Description |
+|---|---|
+| [map_assembler](doc/map_assembler.md) | Rebuilds the global maps from RTAB-Map's graph, off the SLAM node's critical path. |
+| [db_player](doc/db_player.md) | Replays a recorded RTAB-Map database as live sensor topics. |
+
+## Library
+
+The package also installs a small C++ library, whose API is documented in the [C++ API reference](https://docs.ros.org/en/jazzy/p/rtabmap_util/generated/index.html) generated from the headers.
+
+`MapsManager` is the piece worth knowing about: it turns a pose graph plus per-node occupancy grids into the assembled clouds, occupancy grid, octomap and elevation map, and publishes them. Both [map_assembler](doc/map_assembler.md) and [`rtabmap_slam`](../rtabmap_slam/README.md)'s `rtabmap` node use it, which is why their map outputs and `Grid/*` parameters behave identically. It is described below.
+
+### MapsManager
+
+**Published topics.** Everything is published only when subscribed, and -- by default -- **latched**, so a subscriber joining late immediately receives the current map.
+
+In a component container with intra-process communication enabled (`use_intra_process_comms`), these publishers automatically opt out of it when `latch` is on, since intra-process communication does not support transient local durability. With `latch` off, they keep the container's setting.
+
+| Topic | Type | Description |
+|---|---|---|
+| `cloud_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground and obstacles together. |
+| `cloud_ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Ground only, colored green. |
+| `cloud_obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Obstacles only, colored red. |
+| `map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The 2D occupancy grid, the one navigation wants. |
+| `grid_prob_map` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The same grid as occupancy probabilities rather than free/occupied/unknown. |
+| `octomap_occupied_space`, `octomap_obstacles`, `octomap_ground`, `octomap_empty_space`, `octomap_global_frontier_space` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Octomap contents, one cloud per category. Requires RTAB-Map built with OctoMap. |
+| `octomap_grid` | [`nav_msgs/msg/OccupancyGrid`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/OccupancyGrid.html) | The octomap projected to 2D. |
+| `octomap_binary`, `octomap_full` | [`octomap_msgs/msg/Octomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/msg/Octomap.html) | The tree itself, for `octovis` or other octomap consumers. Serialized as a **`ColorOcTree`**, see [Octomap tree type](#octomap-tree-type). |
+| `elevation_map` | [`grid_map_msgs/msg/GridMap`](https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg) | Elevation map. Requires RTAB-Map built with `grid_map`. |
+
+**Parameters.**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `latch` | `bool` | `true` | Publish with transient-local durability so late subscribers get the current map. |
+| `map_filter_radius` | `double` | `0.0` | Skip nodes closer together than this, in meters. A cheap way to thin a dense graph. `0` disables. |
+| `map_filter_angle` | `double` | `30.0` | With `map_filter_radius`, nodes are only merged if they also differ by less than this angle, in degrees. |
+| `map_always_update` | `bool` | `false` | Also assemble the latest sensor data, not yet a node, so the maps update even when the robot stands still and no node is added. |
+| `map_empty_ray_tracing` | `bool` | `true` | For that latest data, fill the 2D scan's rays with empty cells (`Grid/Scan2dUnknownSpaceFilled`). |
+| `map_cleanup` | `bool` | `true` | Free the cached clouds when nobody is subscribed. |
+| `cloud_output_voxelized` | `bool` | `true` | Voxelize the assembled clouds at `Grid/CellSize`. |
+| `cloud_subtract_filtering` | `bool` | `false` | Drop points that duplicate ones already in the map. Slower, smaller output. |
+| `cloud_subtract_filtering_min_neighbors` | `int` | `2` | Neighbors needed for a point to count as a duplicate. |
+| `octomap_tree_depth` | `int` | `16` | Depth the octomap clouds are generated at. Lower means coarser and faster. Maximum 16. |
+
+`map_always_update` and `map_empty_ray_tracing` only apply to the latest sensor data, not yet committed as a node, which only the `rtabmap` node has: they do nothing in `map_assembler`.
+
+Every RTAB-Map **`Grid/*`**, **`GridGlobal/*`**, **`StereoBM/*`** and **`StereoSGBM/*`** parameter is also exposed, all documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The split between the first two is worth knowing: **`Grid/*`** decides how each node's local grid is built from its sensor data -- the same segmentation [obstacles_detection](doc/obstacles_detection.md#parameters) does, and the parameters listed there apply here too -- while **`GridGlobal/*`** decides how those local grids are merged into the global map, so it covers the map's minimum size, its occupancy threshold, and how far the graph must move before the whole map is rebuilt.
+
+#### Octomap tree type
+
+RTAB-Map keeps a color per voxel, so the tree it publishes on `octomap_binary` and `octomap_full` reports its `id` as **`ColorOcTree`**, not the plain `OcTree` many examples assume.
+
+That is deliberate and interoperable: `octomap_msgs::binaryMsgToMap()` and `fullMsgToMap()` branch on that `id` and hand you back an `octomap::ColorOcTree`, and `octovis` opens it without complaint. What does break is code that assumes the other branch:
+
+```cpp
+octomap::AbstractOcTree * tree = octomap_msgs::binaryMsgToMap(msg);
+octomap::OcTree * octree = dynamic_cast<octomap::OcTree *>(tree);   // null
+octomap::ColorOcTree * octree = dynamic_cast<octomap::ColorOcTree *>(tree);   // ok
+```
+
+`ColorOcTree` does not derive from `OcTree` -- both derive from `OccupancyOcTreeBase` -- so cast to `ColorOcTree`, or to `octomap::OccupancyOcTreeBase<...>` if you only need occupancy and want to accept either.
+
+## Conventions
+
+A few things recur across these nodes.
+
+**`qos` parameters.** Most nodes expose a `qos` integer selecting the reliability of their subscriptions: `0` system default, `1` reliable, `2` best effort. It has to be compatible with the publisher or **no messages arrive at all** and nothing says why. Sensor drivers commonly publish best effort.
+
+**`approx_sync`.** Nodes taking several inputs match them by nearest stamp by default. Set it to `false` when the inputs are hardware-synchronized and carry identical stamps: the exact policy is cheaper and cannot mismatch. With approximate sync, `approx_sync_max_interval` is worth setting as a guard against silently pairing stale data.
+
+**`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.
diff --git a/rtabmap_util/doc/db_player.md b/rtabmap_util/doc/db_player.md
new file mode 100644
index 00000000..23bd3e58
--- /dev/null
+++ b/rtabmap_util/doc/db_player.md
@@ -0,0 +1,127 @@
+# db_player
+
+Replays a recorded RTAB-Map database as live sensor topics.
+
+Point it at a `.db` file and it publishes the images, scans, odometry and transforms that were recorded into it, at the rate they were captured. Everything downstream sees a running robot.
+
+That makes it the tool for offline work: re-run SLAM with different parameters on the same data, debug a failure you cannot reproduce on the robot, or develop a node without hardware. Unlike a rosbag, the database is what RTAB-Map itself wrote, so it is always available after a mapping session.
+
+> **The executable is named `data_player`**, not `db_player`. The composable node is `rtabmap_util::DbPlayer`.
+
+## Contents
+
+- [Usage](#usage)
+- [Published Topics](#published-topics)
+- [Published Transforms](#published-transforms)
+- [Services](#services)
+- [Parameters](#parameters)
+- [Simulated time](#simulated-time)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util data_player --ros-args \
+  -p database:=~/.ros/rtabmap.db \
+  -p rate:=1.0 \
+  -p frame_id:=base_link
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::DbPlayer',
+    name='db_player',
+    parameters=[{'database': '/path/to/rtabmap.db', 'rate': 1.0,
+                 'frame_id': 'base_link'}])
+```
+
+## Published Topics
+
+**Which topics exist depends on what the database contains.** The node inspects the first frame and only advertises what it can actually publish, so a lidar-only database has no image topics at all.
+
+| Topic | Type | Published when |
+|---|---|---|
+| `rgb/image`, `rgb/camera_info` | [`Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html), [`CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Single RGB-D camera. |
+| `depth/image`, `depth/camera_info` | `Image`, `CameraInfo` | Single RGB-D camera. |
+| `left/image`, `left/camera_info` | `Image`, `CameraInfo` | Single stereo pair. |
+| `right/image`, `right/camera_info` | `Image`, `CameraInfo` | Single stereo pair. |
+| `image` | `Image` | Images with no calibration. |
+| `rgbd_image0`, `rgbd_image1`, … | [`RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Multiple RGB-D cameras, one topic each. |
+| `stereo_image0`, `stereo_image1`, … | `RGBDImage` | Multiple stereo pairs, one topic each. |
+| `scan` | [`LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | A 2D laser scan. |
+| `scan_cloud` | [`PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A 3D laser scan. |
+| `odom` | [`Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Odometry poses, with their covariance. |
+| `imu` | [`Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Gravity was recorded. Orientation only. |
+| `global_pose` | [`PoseWithCovarianceStamped`](https://docs.ros.org/en/jazzy/p/geometry_msgs/msg/PoseWithCovarianceStamped.html) | A prior pose was recorded. |
+| `gps/fix` | [`NavSatFix`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/NavSatFix.html) | GPS was recorded. |
+| `env_sensor` | [`EnvSensor`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/EnvSensor.html) | Environmental sensors were recorded. |
+| `/clock` | [`Clock`](https://docs.ros.org/en/jazzy/p/rosgraph_msgs/msg/Clock.html) | `publish_clock` is set. See [Simulated time](#simulated-time). |
+
+Everything except `/tf` and `/clock` is published only when it has a subscriber.
+
+## Published Transforms
+
+Broadcast on every frame unless `publish_tf` is false.
+
+| Transform | Published when |
+|---|---|
+| `odom_frame_id` → `frame_id` | Odometry is available. |
+| `frame_id` → `camera_frame_id` | A camera is calibrated. Multi-camera setups get a numeric suffix; stereo gets `left_`/`right_` prefixes, with the right frame offset by the baseline. |
+| `frame_id` → `scan_frame_id` | A scan is present. |
+| `frame_id` → `imu_frame_id` | An IMU is present. |
+| `ground_truth_frame_id` → `ground_truth_base_frame_id` | Ground truth was recorded. |
+
+## Services
+
+| Service | Type | Description |
+|---|---|---|
+| `~/pause` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Pause playback. |
+| `~/resume` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Resume it. |
+
+When run as the standalone executable, the **space bar** toggles pause as well.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `database` | `string` | `""` | **Required.** Path to the `.db` file. `~` is expanded, relative paths resolve against the working directory. The node throws on start-up if it is unset or unreadable. |
+| `rate` | `double` | `1.0` | Playback speed as a multiple of the recorded rate. `2.0` is twice as fast, `0.5` half. |
+| `start_id` | `int` | `0` | Skip to this node id. `0` starts at the beginning. |
+| `ignore_odom` | `bool` | `false` | Do not publish odometry or its transform, so you can run your own odometry against the raw sensor data. |
+| `publish_tf` | `bool` | `true` | Broadcast the transforms above. Turn it off if a robot state publisher already provides them. |
+| `publish_clock` | `bool` | `false` | Publish `/clock`. See [Simulated time](#simulated-time). |
+| `frame_id` | `string` | `"base_link"` | Robot base frame. |
+| `odom_frame_id` | `string` | `"odom"` | Odometry frame. |
+| `camera_frame_id` | `string` | `"camera_optical_link"` | Camera optical frame. |
+| `scan_frame_id` | `string` | `"base_laser_link"` | Lidar frame. |
+| `imu_frame_id` | `string` | `"imu_link"` | IMU frame. |
+| `ground_truth_frame_id` | `string` | `"world"` | Ground truth parent frame. |
+| `ground_truth_base_frame_id` | `string` | `"base_link_gt"` | Ground truth child frame. |
+| `qos` | `int` | `0` | Reliability of all publishers unless overridden below. |
+| `qos_camera_info`, `qos_odom`, `qos_scan`, `qos_scan_cloud`, `qos_global_pose`, `qos_gps`, `qos_imu`, `qos_env_sensor` | `int` | value of `qos` | Per-topic overrides. |
+
+**2D scan geometry** — only used when the recorded scan has no angle metadata of its own, which happens for scans converted from a 3D lidar.
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `scan_angle_min` | `double` | `-π` | |
+| `scan_angle_max` | `double` | `π` | |
+| `scan_angle_increment` | `double` | `π/720` | |
+| `scan_range_min` | `double` | `0.0` | |
+| `scan_range_max` | `double` | `60.0` | |
+
+## Simulated time
+
+With `publish_clock` the node publishes `/clock` from the recorded stamps. Start every other node with `use_sim_time:=true` and the whole system runs on the database's timeline instead of the wall clock, so playback speed no longer affects behavior — a good idea when replaying faster than real time, and essential for reproducible runs.
+
+```bash
+ros2 run rtabmap_util data_player --ros-args -p database:=map.db -p publish_clock:=true
+ros2 launch rtabmap_launch rtabmap.launch.py use_sim_time:=true
+```
+
+## Notes
+
+Playback ends when the last node has been published, and the standalone executable exits at that point.
+
+The database is opened read-only as far as playback is concerned, so replaying the same file while RTAB-Map maps into another one is safe.
diff --git a/rtabmap_util/doc/disparity_to_depth.md b/rtabmap_util/doc/disparity_to_depth.md
new file mode 100644
index 00000000..40942705
--- /dev/null
+++ b/rtabmap_util/doc/disparity_to_depth.md
@@ -0,0 +1,63 @@
+# disparity_to_depth
+
+Converts a disparity image into a depth image.
+
+Most of ROS handles depth, while a stereo pipeline produces disparity. This node bridges the two: for every pixel it computes `depth = baseline * focal / disparity`, taking the baseline and focal length from the incoming [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) itself, so no camera info is needed.
+
+Pixels whose disparity falls outside the message's own `min_disparity`/`max_disparity` are written as zero, which is the ROS convention for "no reading".
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util disparity_to_depth --ros-args \
+  -r disparity:=/stereo/disparity \
+  -r depth:=/stereo/depth
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::DisparityToDepth',
+    name='disparity_to_depth',
+    remappings=[('disparity', '/stereo/disparity'),
+                ('depth', '/stereo/depth')])
+```
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | The disparity image must be `32FC1`; anything else is rejected with an error. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `depth` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**. |
+| `depth_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**, the compact form most RGB-D drivers publish. |
+
+Both are computed only if something is subscribed to them, so leaving one unused costs nothing. Both keep the header of the input disparity image.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
+| `qos_sub` | `int` | value of `qos` | Reliability of the `disparity` subscription alone. |
+| `qos_pub` | `int` | value of `qos` | Reliability of the `depth` and `depth_raw` publishers alone. |
+| `queue_sub` | `int` | `1` | Queue depth of the `disparity` subscription. Must be at least 1. |
+| `queue_pub` | `int` | `1` | Queue depth of both publishers. Must be at least 1. |
+
+## Notes
+
+Depth beyond 65.535 m cannot be represented in the `16UC1` output and wraps around; use the `32FC1` `depth` topic for long-range stereo.
+
+This node performs no filtering or hole-filling. A noisy disparity image gives a noisy depth image.
diff --git a/rtabmap_util/doc/imu_to_tf.md b/rtabmap_util/doc/imu_to_tf.md
new file mode 100644
index 00000000..3fecd7a4
--- /dev/null
+++ b/rtabmap_util/doc/imu_to_tf.md
@@ -0,0 +1,241 @@
+# imu_to_tf
+
+Broadcasts the orientation of an IMU as a TF transform.
+
+The node subscribes to a [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) topic, takes the `orientation` field and broadcasts it on `/tf` as the rotation of `fixed_frame_id` → the IMU frame. Set `base_frame_id` and that frame becomes the child instead, with the orientation re-expressed in it from the IMU's mounting, so the transform says how the *robot* is oriented rather than how the sensor is. Either way `fixed_frame_id` is the parent, and nothing else of the message is used: the transform's translation is always zero, and the angular velocity and linear acceleration are ignored.
+
+It exists so that a consumer that needs an oriented frame — a lidar deskewing node, a point cloud assembler, RViz — can get one from an IMU alone, without running odometry.
+
+## Contents
+
+- [Usage](#usage)
+  - [When the IMU has no orientation](#when-the-imu-has-no-orientation)
+  - [A stabilized frame for lidar deskewing and odometry](#a-stabilized-frame-for-lidar-deskewing-and-odometry)
+  - [A rotation guess for visual odometry](#a-rotation-guess-for-visual-odometry)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Published Transforms](#published-transforms)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Mounting offset](#mounting-offset)
+- [Notes](#notes)
+
+## Usage
+
+As a standalone node:
+
+```bash
+ros2 run rtabmap_util imu_to_tf --ros-args \
+  -r imu/data:=/imu \
+  -p fixed_frame_id:=odom
+```
+
+As a composable node, in the same process as its producer or consumer:
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::ImuToTF',
+    name='imu_to_tf',
+    parameters=[{'fixed_frame_id': 'odom'}],
+    remappings=[('imu/data', '/imu')])
+```
+
+### When the IMU has no orientation
+
+The node reads `orientation` and nothing else, and many IMUs do not fill it in — they publish only angular velocity and linear acceleration. Fuse them into an orientation first, with a filter such as [`imu_filter_madgwick`](https://github.com/CCNYRoboticsLab/imu_tools), and point this node at the filter's output:
+
+```python
+Node(
+    package='imu_filter_madgwick', executable='imu_filter_madgwick_node',
+    parameters=[{'use_mag': False, 'world_frame': 'enu', 'publish_tf': False}],
+    remappings=[('imu/data_raw', '/camera/imu')]),   # publishes /imu/data
+
+Node(
+    package='rtabmap_util', executable='imu_to_tf',
+    parameters=[{'fixed_frame_id': 'odom'}],
+    remappings=[('imu/data', '/imu/data')]),
+```
+
+Set `publish_tf: False` on the filter. It can broadcast a transform of its own, and two nodes publishing orientation for the same frame is exactly the conflict described in [Notes](#notes). `use_mag: False` keeps it off the magnetometer, which is rarely trustworthy indoors or near motors.
+
+A quick way to tell whether you need the filter at all:
+
+```bash
+ros2 topic echo /camera/imu --field orientation --once
+```
+
+All zeros, or an `orientation_covariance` whose first element is `-1`, means the driver is not estimating orientation and this node has nothing to publish.
+
+### A stabilized frame for lidar deskewing and odometry
+
+The way it is used in [`rtabmap_examples/launch/lidar3d.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d.launch.py). A 3D lidar needs a fixed frame to deskew against and ICP odometry benefits from a motion guess, but before odometry is running there is no `odom` frame to use. An IMU can supply one — for rotation.
+
+Point `fixed_frame_id` at a frame that does not exist anywhere else, named after the base frame:
+
+```python
+Node(
+    package='rtabmap_util', executable='imu_to_tf',
+    parameters=[{'fixed_frame_id': 'base_link_stabilized',
+                 'base_frame_id': 'base_link',
+                 'wait_for_transform_duration': 0.001}],
+    remappings=[('imu/data', '/imu/data')])
+```
+
+This publishes `base_link_stabilized` → `base_link` carrying the robot's orientation and nothing else. Because the node never publishes a translation, `base_link_stabilized` stays glued to the robot and only its *orientation* is meaningful over time: it is a gravity-leveled version of the base frame rather than a world frame. That is exactly what the two consumers need.
+
+[lidar_deskewing](lidar_deskewing.md) then corrects the rotation of each sweep:
+
+```python
+Node(
+    package='rtabmap_util', executable='lidar_deskewing',
+    parameters=[{'fixed_frame_id': 'base_link_stabilized'}],
+    remappings=[('input_cloud', '/lidar/points')])
+```
+
+and ICP odometry takes the same frame as its motion guess, with its own deskewing turned off since it is already done:
+
+```python
+Node(
+    package='rtabmap_odom', executable='icp_odometry',
+    parameters=[{'frame_id': 'base_link',
+                 'odom_frame_id': 'icp_odom',
+                 'guess_frame_id': 'base_link_stabilized',
+                 'deskewing': False}],
+    remappings=[('scan_cloud', '/lidar/points/deskewed')])
+```
+
+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:
+
+```mermaid
+flowchart LR
+    IMU["IMU driver"]
+    IMUT(["imu/data"])
+    I2T["imu_to_tf"]
+    LIDAR["lidar driver"]
+    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 |
+|---|---|
+| `map` → `icp_odom` | `rtabmap` |
+| `icp_odom` → `base_link_stabilized` | `icp_odometry` |
+| `base_link_stabilized` → `base_link` | **this node**, from the IMU orientation |
+| `base_link` → `lidar_link`, `imu_link` | your robot description, static |
+
+Note what `icp_odometry` publishes: because `guess_frame_id` is set it broadcasts the *correction* `icp_odom` → `base_link_stabilized` rather than `icp_odom` → `base_link`. That is what makes the two nodes compose — the stabilized frame slots into the chain and every frame keeps exactly one parent. Without `guess_frame_id` the odometry would publish straight to `base_link` and fight this node over it.
+
+Only rotation is compensated, and the two errors behave differently over a sweep:
+
+| Error | How it scales | Worst when |
+|---|---|---|
+| Rotation, corrected here | grows with range | turning fast, looking far |
+| Translation, left over | same at every range, grows with speed | driving fast, looking close |
+
+Moving slowly, or looking far, the leftover translation stays under the lidar's own range noise and can be ignored. Fast and close it is the bigger of the two, and it shifts the cloud rather than blurring it, so it turns into odometry drift.
+
+Once something publishes a real `odom` → `base_link` — wheel or visual odometry, or an EKF such as [`robot_localization`](https://github.com/cra-ros-pkg/robot_localization) fusing that same IMU with wheel odometry — point both `fixed_frame_id` and `guess_frame_id` at `odom` instead and drop this node. Translation then gets compensated too.
+
+### A rotation guess for visual odometry
+
+The same stabilized frame is useful to a camera, for a different reason. Visual odometry predicts where each feature from the previous frame should land in the current one and searches around that prediction; on a fast rotation the prediction is far off, matches are lost and odometry breaks exactly when the motion is hardest.
+
+An IMU fixes the prediction. Run this node as above to publish `base_link_stabilized` → `base_link`, then hand that frame to the odometry as its guess:
+
+```python
+Node(
+    package='rtabmap_odom', executable='rgbd_odometry',
+    parameters=[{'frame_id': 'base_link',
+                 'guess_frame_id': 'base_link_stabilized'}],
+    remappings=[('rgb/image', '/camera/color/image_raw'),
+                ('depth/image', '/camera/depth/image_rect_raw'),
+                ('rgb/camera_info', '/camera/color/camera_info')])
+```
+
+With [`rtabmap_launch`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_launch) the same thing is one argument:
+
+```bash
+ros2 launch rtabmap_launch rtabmap.launch.py odom_guess_frame_id:=base_link_stabilized
+```
+
+The TF chain is the one from the previous section with `rgbd_odometry` in place of `icp_odometry`; it publishes the same `odom` → `base_link_stabilized` correction, so the frames still form one tree.
+
+A rotation-only guess is enough here, because feature matching cares about where things appear, not where they are. Turning the camera slides every feature across the image by the same amount, near or far. Moving it slides them too, but far less, and less the further away they are — generally little enough to stay inside the window the matcher searches. So rotation is the part a guess has to get right, and that is exactly what the IMU supplies. It is also why an IMU far too drifty to give you a *pose* still makes a good guess: only the rotation over a single frame interval is being used, long before drift has time to accumulate.
+
+## Subscribed Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `imu/data` | [`sensor_msgs/msg/Imu`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Imu.html) | Only `orientation` and `header` are read. The subscription has a queue depth of 1; its reliability comes from the `qos` parameter. |
+
+## Published Topics
+
+None. The node only broadcasts transforms.
+
+## Published Transforms
+
+| Transform | Description |
+|---|---|
+| `fixed_frame_id` → IMU frame | Broadcast when `base_frame_id` is empty. The child frame is the `header.frame_id` of the incoming message. |
+| `fixed_frame_id` → `base_frame_id` | Broadcast when `base_frame_id` is set. The orientation is re-expressed in the base frame first, see [Mounting offset](#mounting-offset). |
+
+The transform carries a rotation only; its translation is always zero. It is stamped with the IMU message's stamp, not the current time.
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| `base_frame_id` → IMU frame | Only when `base_frame_id` is set and differs from the IMU's `header.frame_id`. This is the fixed mounting of the IMU on the robot, normally published by your robot description. |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `fixed_frame_id` | `string` | `"odom"` | Parent frame of the broadcast transform. |
+| `base_frame_id` | `string` | `""` | Frame to report the orientation in. Empty broadcasts the IMU frame itself, which is the cheapest option when nothing else needs the base frame oriented. |
+| `qos` | `int` | `0` | Reliability of the `imu/data` subscription: `0` system default, `1` reliable, `2` best effort. Must match the publisher, or no message arrives. |
+| `wait_for_transform_duration` | `double` | `0.1` | Seconds to wait for the `base_frame_id` → IMU transform before giving up on a message. Only used when `base_frame_id` is set. |
+
+## Mounting offset
+
+When `base_frame_id` is set, the node looks up the mounting transform `base_frame_id` → IMU frame and re-expresses the orientation in the base frame. The **yaw of the mounting is deliberately discarded**: only its roll and pitch are applied.
+
+That is what you want from an absolute orientation source. An IMU bolted on facing sideways still measures the same absolute heading as one facing forward, so its yaw must reach the base frame untouched; its roll and pitch, on the other hand, do have to be rotated into the base frame to be meaningful.
+
+A message is **dropped** — logged as an error, nothing broadcast — if that mounting transform is not available within `wait_for_transform_duration`.
+
+## Notes
+
+Only one node may publish a given TF edge. If odometry is already publishing `odom` → `base_link`, do not point this node at the same pair — give it a frame of its own, as in [the stabilized frame above](#a-stabilized-frame-for-lidar-deskewing-and-odometry), or leave `base_frame_id` empty. Two publishers on one edge make the transform flicker between them.
+
+The node does not integrate or filter anything — whatever orientation the message carries is what gets broadcast. See [When the IMU has no orientation](#when-the-imu-has-no-orientation) if your driver does not estimate one.
diff --git a/rtabmap_util/doc/lidar_deskewing.md b/rtabmap_util/doc/lidar_deskewing.md
new file mode 100644
index 00000000..64dafa66
--- /dev/null
+++ b/rtabmap_util/doc/lidar_deskewing.md
@@ -0,0 +1,106 @@
+# lidar_deskewing
+
+Removes the motion distortion from a lidar scan.
+
+A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving robot every point in that sweep is measured from a slightly different pose. The result is a *skewed* cloud: straight walls come out bent, and registration against it drifts.
+
+This node uses TF to find where the sensor actually was when each point was taken, and moves every point into the pose at the start of the sweep. A straight wall comes back straight.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Requirements](#requirements)
+- [Behavior when TF is missing](#behavior-when-tf-is-missing)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util lidar_deskewing --ros-args \
+  -p fixed_frame_id:=odom \
+  -r input_cloud:=/velodyne_points
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::LidarDeskewing',
+    name='lidar_deskewing',
+    parameters=[{'fixed_frame_id': 'odom'}],
+    remappings=[('input_cloud', '/velodyne_points')])
+```
+
+## Subscribed Topics
+
+Connect one of the two.
+
+| Topic | Type | Description |
+|---|---|---|
+| `input_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Must carry a **per-point time channel**, see [Requirements](#requirements). |
+| `input_scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | Per-point times come from `time_increment`. |
+
+## Published Topics
+
+Output names are derived from the **resolved** input names, so remapping the input moves the output with it. With `input_cloud` remapped to `/velodyne_points` the output is `/velodyne_points/deskewed`.
+
+| Topic | Type | Description |
+|---|---|---|
+| `<input_cloud>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The deskewed cloud, same frame and stamp as the input. |
+| `<input_scan>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A `LaserScan` cannot represent a deskewed sweep — the points no longer lie on a regular angular grid — so the scan input also produces a cloud. |
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| `fixed_frame_id` → sensor frame, across the sweep | Must be available for the whole span of the sweep, at both its first and last stamp. |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `fixed_frame_id` | `string` | `""` | **Required.** Frame the motion is measured against, usually `odom`. |
+| `wait_for_transform` | `double` | `0.01` | Seconds to wait for the transforms spanning the sweep. Raise it if odometry lags the lidar. |
+| `slerp` | `bool` | `false` | Interpolate between the poses at the start and end of the sweep instead of looking up TF per point. Much cheaper, and accurate enough at constant velocity. |
+| `queue_size` | `int` | `1` | Queue depth of the input subscriptions. |
+| `qos` | `int` | `0` | Reliability of the input subscriptions: `0` system default, `1` reliable, `2` best effort. |
+
+## Requirements
+
+For `input_cloud`, the cloud **must have a per-point time field**. Without one the node cannot know when each point was taken and cannot deskew.
+
+The field has to be named `t`, `time`, `stamps` or `timestamp` — anything else is not recognized, whatever it contains. Its type decides how the value is read:
+
+| Type | Meaning |
+|---|---|
+| `uint32` | nanoseconds since the cloud's own stamp |
+| `float32` | seconds since the cloud's own stamp |
+| `float64` | an absolute timestamp; seconds, milliseconds, microseconds and nanoseconds are told apart by magnitude |
+
+Common drivers that satisfy this out of the box: **Ouster** (`t`), **Velodyne** (`time`), **RoboSense** (`timestamp`) and **Livox** (`timestamp`). Livox needs its PointCloud2 output rather than the default `CustomMsg` format, which this node cannot subscribe to at all.
+
+To check what your driver actually publishes:
+
+```bash
+ros2 topic echo /your/points --field fields --once
+```
+
+If none of the four names is in that list, look for a driver option to add per-point timestamps before anything else.
+
+The `fixed_frame_id` → sensor transform must cover the whole sweep, which means **odometry has to be at least as recent as the lidar**. If it lags, raise `wait_for_transform`.
+
+## Behavior when TF is missing
+
+The two inputs deliberately differ:
+
+- A **cloud** is republished **unchanged** with a warning. Deskewing is an improvement, not a precondition, and dropping frames would break the pipeline behind it.
+- A **scan** is **dropped**, because converting it to a cloud is only worth doing as part of deskewing.
+
+## Notes
+
+Deskewing matters most when rotating: at 1 rad/s a 100 ms sweep spans nearly 6°, and the far end of the scan is badly misplaced. Pure translation at walking speed is a few centimeters, which matters at close range.
+
+Put this node before ICP odometry or [point_cloud_assembler](point_cloud_assembler.md), not after. Anything registering against a skewed cloud has already paid for the distortion.
diff --git a/rtabmap_util/doc/map_assembler.md b/rtabmap_util/doc/map_assembler.md
new file mode 100644
index 00000000..2fd4e96f
--- /dev/null
+++ b/rtabmap_util/doc/map_assembler.md
@@ -0,0 +1,95 @@
+# map_assembler
+
+Rebuilds the global maps from RTAB-Map's graph, in a separate process.
+
+RTAB-Map publishes its graph and the per-node sensor data on `mapData`; turning that into a point cloud, an occupancy grid or an octomap costs real CPU. This node does that work, so the SLAM node does not have to and the mapping loop stays responsive.
+
+It also lets you produce maps RTAB-Map is not currently configured to publish, or several differently-configured maps at once, without restarting SLAM.
+
+The assembling itself is done by [`MapsManager`](../README.md#mapsmanager), which is shared with `rtabmap_slam` — the outputs and every `Grid/*` parameter behave identically in both.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Services](#services)
+- [Parameters](#parameters)
+- [Start-up](#start-up)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util map_assembler --ros-args \
+  -p Grid/CellSize:=0.05 -p Grid/RangeMax:=8.0 -p cloud_output_voxelized:=true
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::MapAssembler',
+    name='map_assembler',
+    parameters=[{'Grid/CellSize': '0.05', 'Grid/RangeMax': '8.0',
+                 'cloud_output_voxelized': True}])
+```
+
+In a component container with intra-process communication enabled (`use_intra_process_comms`), the map publishers automatically opt out of it when `latch` is on (the default), since intra-process communication does not support transient local durability. With `latch` off, they keep the container's setting. See [`MapsManager`](../README.md#mapsmanager).
+
+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 |
+|---|---|---|
+| `mapData` | [`rtabmap_msgs/msg/MapData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/MapData.html) | The graph, plus the sensor data of any newly added node. Published by `rtabmap`. |
+
+## Published Topics
+
+The maps assembled by [`MapsManager`](../README.md#mapsmanager): point clouds, occupancy grids, octomap and elevation map, published only when subscribed and latched by default. The topics are listed [there](../README.md#mapsmanager).
+
+## Services
+
+| Service | Type | Description |
+|---|---|---|
+| `~/reset` | [`std_srvs/srv/Empty`](https://docs.ros.org/en/jazzy/p/std_srvs/srv/Empty.html) | Drop the cached nodes and every assembled map. |
+| `~/octomap_binary` | [`octomap_msgs/srv/GetOctomap`](https://docs.ros.org/en/jazzy/p/octomap_msgs/srv/GetOctomap.html) | Build and return the octomap on demand. |
+| `~/octomap_full` | `octomap_msgs/srv/GetOctomap` | The same, with occupancy probabilities. |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `initialize_from_rtabmap_timeout` | `double` | `5.0` | Seconds to wait for rtabmap's `get_map_data` service on start-up, which is how the node catches up on a map that already exists. Set to `0` to skip the call and subscribe immediately, which is what you want when `map_assembler` starts *before* rtabmap. |
+| `rtabmap` | `string` | `"rtabmap"` | Name of the rtabmap node whose `get_map_data` service to call. |
+| `regenerate_local_grids` | `bool` | `false` | Discard the occupancy grids stored with each node and rebuild them from the raw sensor data. Use it to change `Grid/*` parameters on an existing map without re-running SLAM. Costs CPU per node. |
+| `config_path` | `string` | `""` | An RTAB-Map `.ini` file to load parameters from, instead of listing them individually. |
+
+**Map assembly**: the parameters of [`MapsManager`](../README.md#mapsmanager), and every RTAB-Map `Grid/*`, `GridGlobal/*`, `StereoBM/*` and `StereoSGBM/*` parameter, as described there. Two of them do nothing here: `map_always_update` and `map_empty_ray_tracing`.
+
+## Start-up
+
+`map_assembler` normally starts alongside rtabmap and builds its maps from the `mapData` messages that follow. If it starts **after** rtabmap it would miss everything already mapped, so on start-up it calls rtabmap's `get_map_data` service once to fetch the existing map.
+
+That call blocks the subscription to `mapData` until it returns or times out, which is wasted time when rtabmap is not running yet. Set `initialize_from_rtabmap_timeout` to `0` in that case.
+
+If rtabmap is started later in localization mode, call its `publish_maps` service with `graph_only=false` so `map_assembler` receives the data it missed.
+
+## Notes
+
+`regenerate_local_grids` is the parameter to reach for when a recorded map's grids were built with settings you now want to change. Without it, `Grid/*` changes only affect nodes added from then on, because each node's grid is stored with it.
diff --git a/rtabmap_util/doc/obstacles_detection.md b/rtabmap_util/doc/obstacles_detection.md
new file mode 100644
index 00000000..2c2be26f
--- /dev/null
+++ b/rtabmap_util/doc/obstacles_detection.md
@@ -0,0 +1,165 @@
+# obstacles_detection
+
+Segments a point cloud into ground and obstacles.
+
+The node takes a cloud, works out which points belong to the floor and which stick up from it, and publishes the two apart. Downstream that feeds navigation: obstacles into a costmap, ground into a traversability check.
+
+The segmentation is RTAB-Map's own [`LocalGridMaker`](https://introlab.github.io/rtabmap/api/latest/classrtabmap_1_1LocalGridMaker.html), so it is configured through the same `Grid/*` parameters as RTAB-Map itself and produces the same result the SLAM node would.
+
+## Contents
+
+- [Usage](#usage)
+  - [Feeding a nav2 costmap](#feeding-a-nav2-costmap)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Levelling on a slope](#levelling-on-a-slope)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util obstacles_detection --ros-args \
+  -r cloud:=/camera/cloud \
+  -p frame_id:=base_link \
+  -p Grid/MaxObstacleHeight:=2.0
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::ObstaclesDetection',
+    name='obstacles_detection',
+    parameters=[{'frame_id': 'base_link', 'Grid/MaxObstacleHeight': '2.0'}],
+    remappings=[('cloud', '/camera/cloud')])
+```
+
+### Feeding a nav2 costmap
+
+The usual reason to run this node: nav2's costmap wants to be told separately what is floor and what is in the way. A depth camera gives neither directly, so the chain is depth image → cloud → segmented cloud → costmap.
+
+[point_cloud_xyz](point_cloud_xyz.md) projects the depth image, with `decimation` and `voxel_size` set to keep the cost down, and this node splits the result:
+
+```python
+Node(
+    package='rtabmap_util', executable='point_cloud_xyz',
+    parameters=[{'decimation': 2, 'max_depth': 3.0, 'voxel_size': 0.02}],
+    remappings=[('depth/image', '/camera/depth/image_raw'),
+                ('depth/camera_info', '/camera/camera_info'),
+                ('cloud', '/camera/cloud')]),
+
+Node(
+    package='rtabmap_util', executable='obstacles_detection',
+    parameters=[{'frame_id': 'base_link'}],
+    remappings=[('cloud', '/camera/cloud'),
+                ('ground', '/camera/ground'),
+                ('obstacles', '/camera/obstacles')]),
+```
+
+The two outputs then become two observation sources on the costmap's voxel layer:
+
+```yaml
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      plugins: ["voxel_layer", "inflation_layer"]
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: ground obstacles
+        ground:
+          topic: /camera/ground
+          data_type: "PointCloud2"
+          max_obstacle_height: 0.4
+          marking: False        # the floor is not an obstacle...
+          clearing: True        # ...but seeing it proves the space is free
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+        obstacles:
+          topic: /camera/obstacles
+          data_type: "PointCloud2"
+          max_obstacle_height: 0.4
+          marking: True
+          clearing: True
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+```
+
+The `marking`/`clearing` split is the whole point. Ground points only clear: they tell the costmap that the space the camera looked through is free, without writing an obstacle at floor level. Obstacle points do both, so an obstacle that moves away is cleared by the next observation instead of lingering.
+
+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 |
+|---|---|---|
+| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The cloud to segment, in any frame that TF can relate to `frame_id`. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `ground` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as floor. In the **input** cloud's frame. |
+| `obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Points classified as obstacles. In the **input** cloud's frame. |
+| `proj_obstacles` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The obstacles flattened onto the ground plane, in `frame_id`. This is the 2D footprint a planar costmap wants. |
+
+Each output is computed only if something is subscribed to it.
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| `frame_id` → cloud frame | Where the sensor sits on the robot. Segmentation happens in `frame_id`, so this is what makes "up" meaningful. |
+| `map_frame_id` → `frame_id` | Only when `map_frame_id` is set. See [Levelling on a slope](#levelling-on-a-slope). |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `frame_id` | `string` | `"base_link"` | The robot frame. Its xy plane is the ground plane the segmentation works against. |
+| `map_frame_id` | `string` | `""` | See [Levelling on a slope](#levelling-on-a-slope). |
+| `wait_for_transform` | `double` | `0.2` | Seconds to wait for a transform before dropping the cloud. |
+| `qos` | `int` | `0` | Reliability of the subscription and the publishers: `0` system default, `1` reliable, `2` best effort. |
+
+Every RTAB-Map **`Grid/*`** parameter is also exposed as a ROS parameter of this node, and they are what actually control the segmentation. They are documented in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html).
+
+The one to decide first is `Grid/NormalsSegmentation`, which picks between two ways of finding the ground. Left on, it is segmented from surface normals, which copes with slopes and steps. Turned off, it is a plain height threshold: much cheaper, and exact when the floor really is flat, but `Grid/MaxGroundHeight` then has to be set, since it *is* that threshold.
+
+## Levelling on a slope
+
+Segmentation is done in `frame_id`, so if the robot is pitched or rolled — on a ramp, or with a suspension that dips — the ground plane tilts with it and the floor ahead can be classified as an obstacle.
+
+Setting `map_frame_id` makes the node take the robot's pose in that frame and apply its **roll and pitch**, so segmentation happens against a level plane rather than the robot's own tilt.
+
+Height is a separate matter: the robot's **z** in the map frame is ignored unless `Grid/MapFrameProjection` is also set to `true`. That is usually what you want — a height threshold should be measured from the robot, not from an arbitrary map origin — but if you are mapping a multi-level building and want the thresholds relative to the map, enable it.
+
+## Notes
+
+If `obstacles` comes back empty on an obviously cluttered scene, check `Grid/RangeMax` first. It is **not unlimited by default**, and everything beyond it is discarded before segmentation even runs.
+
+The second thing to check is `Grid/MinClusterSize` against your cloud density. A sparse lidar can produce clusters smaller than the default, in which case every obstacle is thrown away as noise. Either lower it or raise `Grid/ClusterRadius`.
diff --git a/rtabmap_util/doc/point_cloud_aggregator.md b/rtabmap_util/doc/point_cloud_aggregator.md
new file mode 100644
index 00000000..f13ba552
--- /dev/null
+++ b/rtabmap_util/doc/point_cloud_aggregator.md
@@ -0,0 +1,129 @@
+# point_cloud_aggregator
+
+Merges one cloud from each of several sensors into a single cloud.
+
+A robot with two or three lidars, or a ring of depth cameras, produces one cloud per sensor. This node waits for a matching set, transforms them all into a common frame and publishes a single cloud, so everything downstream sees the robot's full field of view as one measurement.
+
+The sensors do not have to fire together: the clouds are matched by nearest stamp, and setting `fixed_frame_id` compensates for the robot having moved between them. See [Sensors that do not fire together](#sensors-that-do-not-fire-together).
+
+It combines **several sensors into one frame**. To combine **one sensor over many frames**, use [point_cloud_assembler](point_cloud_assembler.md).
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Converting back to a LaserScan](#converting-back-to-a-laserscan)
+- [Sensors that do not fire together](#sensors-that-do-not-fire-together)
+- [Diagnostics](#diagnostics)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util point_cloud_aggregator --ros-args \
+  -p count:=3 -p frame_id:=base_link -p fixed_frame_id:=odom \
+  -r cloud1:=/lidar_front/points/deskewed \
+  -r cloud2:=/lidar_left/points/deskewed \
+  -r cloud3:=/lidar_right/points/deskewed
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::PointCloudAggregator',
+    name='point_cloud_aggregator',
+    parameters=[{'count': 3, 'frame_id': 'base_link', 'fixed_frame_id': 'odom'}],
+    remappings=[('cloud1', '/lidar_front/points/deskewed'),
+                ('cloud2', '/lidar_left/points/deskewed'),
+                ('cloud3', '/lidar_right/points/deskewed')])
+```
+
+With 2D or 3D lidars, feed the aggregator **deskewed** clouds: run a [lidar_deskewing](lidar_deskewing.md) node per sensor first, which is where the `/deskewed` topics above come from. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews.
+
+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 |
+|---|---|---|
+| `cloud1` … `cloud4` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Only the first `count` are subscribed. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `combined_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id`, or in `cloud1`'s frame if `frame_id` is empty. Stamped with `cloud1`. |
+
+Nothing is computed unless `combined_cloud` has a subscriber.
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| target frame → each cloud's frame | Where each sensor sits. The target is `frame_id`, or `cloud1`'s frame when that is empty. |
+| `fixed_frame_id` → each cloud's frame, at each stamp | Only when `fixed_frame_id` is set. See [Sensors that do not fire together](#sensors-that-do-not-fire-together). |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `count` | `int` | `2` | How many clouds to combine, 2 to 4. Determines how many `cloudN` topics are subscribed. |
+| `frame_id` | `string` | `""` | Frame to express the combined cloud in. Empty uses `cloud1`'s frame, which is the cheapest option since that cloud then needs no transform, but see [Converting back to a LaserScan](#converting-back-to-a-laserscan). |
+| `fixed_frame_id` | `string` | `""` | Frame to compensate motion against, usually `odom`. See below. |
+| `approx_sync` | `bool` | `true` | Match the clouds by nearest stamp. Set false when the sensors are hardware-triggered and share exact stamps. |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. A good guard against silently merging stale data. |
+| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the set. |
+| `xyz_output` | `bool` | `false` | Strip everything but XYZ from the output. Useful when the inputs disagree on their extra fields. |
+| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `qos` | `int` | `0` | Reliability of the cloud subscriptions: `0` system default, `1` reliable, `2` best effort. |
+
+## Converting back to a LaserScan
+
+Some consumers still want a 2D `LaserScan` — `slam_toolbox`, `amcl`, or a costmap layer configured for one. [`pointcloud_to_laserscan`](https://docs.ros.org/en/jazzy/p/pointcloud_to_laserscan/) flattens the combined cloud into one:
+
+```python
+Node(
+    package='pointcloud_to_laserscan', executable='pointcloud_to_laserscan_node',
+    parameters=[{'target_frame': 'base_link', 'min_height': -0.1, 'max_height': 0.5}],
+    remappings=[('cloud_in', '/combined_cloud')])
+```
+
+**Set `frame_id` to the robot center when you do this.** A `LaserScan` is a set of ranges measured outward from one origin, so the conversion is only meaningful about a point the consumer thinks of as the robot. Leaving `frame_id` empty puts the combined cloud in `cloud1`'s frame — a sensor bolted somewhere on the edge of the robot — and every range then comes out measured from that corner. With three lidars merged, the result is a scan centerd on whichever one happened to be `cloud1`.
+
+One case where you should *not* combine first: if the clouds are only going into a nav2 costmap, give nav2 each sensor as its own observation source instead. A costmap clears free space by ray tracing outward from where the observation was made, and it takes that origin from the cloud's own frame. Merge everything into one cloud at `base_link` and every point looks as though it were seen from the robot center, so space gets cleared along lines no sensor ever looked down — including straight through whatever the other sensors can see.
+
+## Sensors that do not fire together
+
+With `approx_sync` the clouds carry different stamps, and on a moving robot each was captured from a different pose. Merging them by their static mounting transforms alone smears the result.
+
+Setting `fixed_frame_id` fixes that: the node asks TF where each sensor was at its own stamp, relative to that fixed frame, and places each cloud accordingly. Two lidars 30 ms apart on a robot turning at 1 rad/s are nearly 2° apart — clearly visible as a doubled wall.
+
+Leave it empty only when the sensors are genuinely synchronized, or when the robot is stationary.
+
+## Diagnostics
+
+The node publishes to `/diagnostics` and warns if no combined cloud has been produced for a while — usually a sign that one of the `cloudN` topics is silent, or that the stamps are too far apart to sync.
diff --git a/rtabmap_util/doc/point_cloud_assembler.md b/rtabmap_util/doc/point_cloud_assembler.md
new file mode 100644
index 00000000..bad5c617
--- /dev/null
+++ b/rtabmap_util/doc/point_cloud_assembler.md
@@ -0,0 +1,210 @@
+# point_cloud_assembler
+
+Accumulates the clouds of one sensor over time into a denser cloud.
+
+A single sweep is sparse, or narrow, or both. This node keeps the recent ones, places each where the sensor was when it was captured, and publishes the union.
+
+That is used for two quite different things. With a **narrow field of view** — a depth camera reduced to a fake scan, say — accumulating a second's worth of sweeps as the robot moves is what makes the sensor usable for SLAM at all. With a 3D lidar it is about **enriching what already works**: each node gets a denser, less occluded cloud, which registers better and puts many more points in the database, so an offline export later has the resolution to be worth having.
+
+It combines **one sensor over many frames**. To combine **several sensors into one frame**, use [point_cloud_aggregator](point_cloud_aggregator.md) — or, if you want them merely accumulated rather than matched into sets, remap them all onto this node's `cloud` topic. Nothing stops several publishers sharing it, and each cloud is placed by its own stamp and frame like any other; the publish trigger then covers them together — `max_clouds` counts across all the sensors, and a given `assembling_time` gathers correspondingly more clouds.
+
+## Contents
+
+- [Usage](#usage)
+  - [Denser clouds for SLAM, and keeping every point](#denser-clouds-for-slam-and-keeping-every-point)
+  - [Widening a narrow field of view](#widening-a-narrow-field-of-view)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Where the poses come from](#where-the-poses-come-from)
+- [Following odometry's keyframes](#following-odometrys-keyframes)
+- [Notes](#notes)
+- [Diagnostics](#diagnostics)
+
+## Usage
+
+Assemble 10 sweeps, using TF for the poses:
+
+```bash
+ros2 run rtabmap_util point_cloud_assembler --ros-args \
+  -r cloud:=/velodyne_points/deskewed \
+  -p max_clouds:=10 -p fixed_frame_id:=odom -p voxel_size:=0.05
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::PointCloudAssembler',
+    name='point_cloud_assembler',
+    parameters=[{'max_clouds': 10, 'fixed_frame_id': 'odom', 'voxel_size': 0.05}],
+    remappings=[('cloud', '/velodyne_points/deskewed')])
+```
+
+With a lidar, feed it **deskewed** clouds from a [lidar_deskewing](lidar_deskewing.md) node rather than the driver's raw output: accumulating skewed sweeps accumulates their distortion too. For a 2D lidar publishing `LaserScan` that node is needed regardless — this one only takes `PointCloud2`, and `lidar_deskewing` converts to one as it deskews.
+
+### Denser clouds for SLAM, and keeping every point
+
+From [`lidar3d_assemble.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d_assemble.launch.py). Here the input is a real 3D lidar, and assembling buys both resolution and coverage: a node built from a second of sweeps is denser between the rings and sees around what a single sweep was occluded by, so it registers better.
+
+It also decides how much of the lidar survives. RTAB-Map stores one cloud per node and updates at around 1 Hz, while the lidar and odometry run at 10 — and running the mapping node at 10 Hz is not practical. Nine sweeps in ten therefore never reach the database. The `assembling_time: 1.0` below hands each node the whole second instead, so **nothing is thrown away**: the database keeps every point the lidar returned, which is what makes this the approach for survey scanning and a full-resolution offline export.
+
+```python
+Node(
+    package='rtabmap_util', executable='point_cloud_assembler',
+    parameters=[{'assembling_time': 1.0,
+                 'fixed_frame_id': ''}],      # '' selects the odom topic
+    remappings=[('cloud', '/lidar/points/deskewed'),
+                ('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
+
+From [`turtlebot3_rgbd_fake_scan.launch.py`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_rgbd_fake_scan.launch.py). A depth camera sees perhaps 60° across, and [`depthimage_to_laserscan`](https://docs.ros.org/en/jazzy/p/depthimage_to_laserscan/) reduces that to a fake scan thinner still. One of those is too little to localize against; twenty of them, accumulated as the robot drives and turns, cover a useful arc.
+
+```python
+Node(
+    package='rtabmap_util', executable='point_cloud_assembler',
+    parameters=[{'max_clouds': 20,
+                 'circular_buffer': True,
+                 'linear_update': 0.3,
+                 'angular_update': 0.5,
+                 'voxel_size': 0.05,
+                 'frame_id': 'base_link'}],
+    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 |
+|---|---|---|
+| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The sweeps to accumulate. Ideally deskewed, see [Usage](#usage). |
+| `odom` | [`nav_msgs/msg/Odometry`](https://docs.ros.org/en/jazzy/p/nav_msgs/msg/Odometry.html) | Only when `fixed_frame_id` is empty. See [Where the poses come from](#where-the-poses-come-from). |
+| `odom_info` | [`rtabmap_msgs/msg/OdomInfo`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/OdomInfo.html) | Only when `subscribe_odom_info` is true. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `assembled_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In `frame_id` if set, otherwise the frame of the newest cloud. Stamped with the newest cloud. |
+
+Nothing is accumulated unless `assembled_cloud` has a subscriber.
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| `fixed_frame_id` → cloud frame, at each stamp | Only in TF mode, i.e. when `fixed_frame_id` is set. |
+| `frame_id` → cloud frame | Only when `frame_id` is set. |
+
+## Parameters
+
+**What triggers a publish** — set exactly one of these
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `max_clouds` | `int` | `0` | Publish once this many clouds have been collected. |
+| `assembling_time` | `double` | `0.0` | Publish once this many seconds have been collected. |
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `circular_buffer` | `bool` | `false` | Keep a rolling window instead of clearing after each publish, so a full assembled cloud is published for **every** input rather than one in `max_clouds`. Costs more, gives smooth output. |
+| `skip_clouds` | `int` | `0` | Drop this many input clouds between the ones kept. |
+| `linear_update` | `double` | `0.0` | Only accumulate a cloud if the sensor has moved this far, in meters, since the last one kept. `0` disables. |
+| `angular_update` | `double` | `0.0` | Same for rotation, in radians. `0` disables. |
+
+**Poses**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `fixed_frame_id` | `string` | `"odom"` | Frame the sweeps are placed in, via TF. **Set it to `""` to use the `odom` topic instead.** |
+| `frame_id` | `string` | `""` | Frame to express the output in. Empty uses the newest cloud's frame. |
+| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping a cloud. |
+| `subscribe_odom_info` | `bool` | `false` | Keep only the clouds odometry marked as keyframes. Needs the `odom` topic mode, see [Following odometry's keyframes](#following-odometrys-keyframes). |
+
+**Filtering**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `range_min` | `double` | `0.0` | Drop points nearer than this to the sensor, in meters. Good for removing the robot itself. `0` disables. |
+| `range_max` | `double` | `0.0` | Drop points further than this, in meters. `0` disables. |
+| `voxel_size` | `double` | `0.0` | Downsample the assembled cloud to one point per voxel, in meters. `0` disables. Strongly recommended, otherwise the cloud grows linearly with `max_clouds`. |
+| `noise_radius` | `double` | `0.0` | Radius outlier removal on the output, in meters. `0` disables. |
+| `noise_min_neighbors` | `int` | `5` | Neighbors needed within `noise_radius`. |
+| `remove_z` | `bool` | `false` | Flatten the output to 2D by zeroing z. |
+
+**Plumbing**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer, in odom-topic mode. |
+| `qos` | `int` | `0` | Reliability of the cloud subscription. |
+| `qos_odom` | `int` | value of `qos` | Reliability of the `odom` and `odom_info` subscriptions. |
+
+## Where the poses come from
+
+Each sweep has to be placed where the sensor was when it was captured, and there are two ways to get that pose:
+
+- **TF** (default). `fixed_frame_id` is set, and the node looks the pose up per cloud. Simple, and it works with any odometry source.
+- **The `odom` topic**. Set `fixed_frame_id` to `""` and the node synchronizes each cloud with an `Odometry` message instead. Use this when odometry is not published to TF, or when you need the pose that exactly matches the cloud rather than an interpolated one.
+
+Because `fixed_frame_id` **defaults to `"odom"`**, the `odom` topic is not subscribed unless you clear it explicitly. Setting `subscribe_odom_info` alone is not enough.
+
+## Following odometry's keyframes
+
+With `subscribe_odom_info` the node also takes `odom_info` and keeps a cloud only when that message reports a keyframe was added; the ones in between are dropped.
+
+This is a better-informed version of `linear_update` and `angular_update`. Those are fixed distances you have to guess at, whereas odometry decides a keyframe from how much of the current scan still matches the last one — `Odom/ScanKeyFrameThr` for ICP, `Odom/KeyFrameThr` for visual odometry. It therefore adapts to the scene, keeping more clouds where the geometry changes quickly and fewer down a featureless corridor, and the assembled cloud ends up built from exactly the frames odometry itself considered distinct.
+
+It only has an effect in the `odom` topic mode. With `fixed_frame_id` set the node subscribes to the cloud on its own and never sees `odom_info`, so clear `fixed_frame_id` as well — see [Where the poses come from](#where-the-poses-come-from).
+
+## Notes
+
+Set `voxel_size`. Without it the assembled cloud is the plain union of every sweep, points and all, and both memory and downstream cost grow with `max_clouds`. A voxel size near the sensor's resolution costs almost no fidelity.
+
+`circular_buffer` changes the output rate, not just the contents: without it you get one assembled cloud per `max_clouds` inputs, with it you get one per input.
+
+## Diagnostics
+
+The node publishes to `/diagnostics` and warns if no assembled cloud has been produced for a while — typically a missing transform or a silent input topic.
diff --git a/rtabmap_util/doc/point_cloud_xyz.md b/rtabmap_util/doc/point_cloud_xyz.md
new file mode 100644
index 00000000..9b88ce25
--- /dev/null
+++ b/rtabmap_util/doc/point_cloud_xyz.md
@@ -0,0 +1,104 @@
+# point_cloud_xyz
+
+Projects a depth or disparity image into a point cloud.
+
+The node takes a depth image and its calibration and produces a [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html), with optional decimation, range limits, voxel and radius filtering, and normal estimation — the same preprocessing RTAB-Map would do internally, done once and shared.
+
+[`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/)'s own `point_cloud_xyz` does the bare projection; this node exists for the filtering, and for accepting disparity directly.
+
+See [point_cloud_xyzrgb](point_cloud_xyzrgb.md) for the colored equivalent.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Organized output](#organized-output)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util point_cloud_xyz --ros-args \
+  -r depth/image:=/camera/depth/image_raw \
+  -r depth/camera_info:=/camera/depth/camera_info \
+  -p decimation:=4 -p max_depth:=5.0 -p voxel_size:=0.05
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::PointCloudXYZ',
+    name='point_cloud_xyz',
+    parameters=[{'decimation': 4, 'max_depth': 5.0, 'voxel_size': 0.05}],
+    remappings=[('depth/image', '/camera/depth/image_raw'),
+                ('depth/camera_info', '/camera/depth/camera_info')])
+```
+
+## Subscribed Topics
+
+The node listens on two independent input sets and uses whichever one is being published. Only one of them should be connected.
+
+**Depth**
+
+| Topic | Type | Description |
+|---|---|---|
+| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1` (meters), `16UC1` (millimeters) or `mono16`. Goes through [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/), see `depth_transport` parameter below. |
+| `depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+
+**Disparity**
+
+| Topic | Type | Description |
+|---|---|---|
+| `disparity/image` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. The 16-bit form is fixed point, 16 units per pixel of disparity. |
+| `disparity/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | In the frame of the input image, stamped with it. Carries `normal_*` fields when normals are enabled. |
+
+Nothing is computed unless `cloud` has a subscriber.
+
+## Parameters
+
+**Synchronization**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `approx_sync` | `bool` | `true` | Match image and camera info by nearest stamp. Set false when they are published with identical stamps, which is stricter and cheaper. |
+| `approx_sync_max_interval` | `double` | `0.0` | With `approx_sync`, reject pairs further apart than this many seconds. `0` disables the check. |
+| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions: `0` system default, `1` reliable, `2` best effort. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. |
+| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`, e.g. `compressedDepth`. |
+
+**Projection and filtering**, applied in this order
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. `2` gives a quarter of the points. The image dimensions must divide by it. |
+| `roi_ratios` | `string` | `""` | Crop before projecting, as four ratios `"left right top bottom"`, e.g. `"0.1 0.1 0 0.2"`. |
+| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. |
+| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. |
+| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel of this size, in meters. `0` disables. |
+| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. |
+| `noise_filter_min_neighbors` | `int` | `5` | Neighbors a point needs within `noise_filter_radius` to survive. |
+| `normal_k` | `int` | `0` | Estimate normals from this many nearest neighbors. `0` disables. |
+| `normal_radius` | `double` | `0.0` | Estimate normals from all neighbors within this radius, in meters. `0` disables. |
+| `filter_nans` | `bool` | `false` | See [Organized output](#organized-output). |
+
+## Organized output
+
+By default the cloud stays **organized**: one point per pixel, in image order, with out-of-range points set to NaN rather than removed. That layout is what lets consumers treat the cloud as an image, and it is why a cloud with `max_depth` set still reports the full point count.
+
+Set `filter_nans` to `true` to drop the invalid points instead. The cloud becomes unorganized and its size reflects what is actually in range — including being empty when nothing is.
+
+Voxel and radius filtering also produce unorganized clouds, since both remove points.
+
+## Notes
+
+`decimation` is by far the cheapest way to cut the cost of everything downstream, and on a depth image it loses very little: neighboring pixels of a surface are nearly redundant. Reach for it before `voxel_size`.
diff --git a/rtabmap_util/doc/point_cloud_xyzrgb.md b/rtabmap_util/doc/point_cloud_xyzrgb.md
new file mode 100644
index 00000000..444d3418
--- /dev/null
+++ b/rtabmap_util/doc/point_cloud_xyzrgb.md
@@ -0,0 +1,130 @@
+# point_cloud_xyzrgb
+
+Projects an RGB-D frame, a stereo pair or a disparity image into a colored point cloud.
+
+The colored counterpart of [point_cloud_xyz](point_cloud_xyz.md): same filtering, same parameters, but every point carries the color of the pixel it came from. It accepts four different input sets, so it can sit at the end of an RGB-D, stereo or disparity pipeline without anything in between.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Stereo matching](#stereo-matching)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util point_cloud_xyzrgb --ros-args \
+  -r rgb/image:=/camera/color/image_raw \
+  -r depth/image:=/camera/aligned_depth_to_color/image_raw \
+  -r rgb/camera_info:=/camera/color/camera_info \
+  -p decimation:=4 -p voxel_size:=0.05
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::PointCloudXYZRGB',
+    name='point_cloud_xyzrgb',
+    parameters=[{'decimation': 4, 'voxel_size': 0.05}],
+    remappings=[('rgb/image', '/camera/color/image_raw'),
+                ('depth/image', '/camera/aligned_depth_to_color/image_raw'),
+                ('rgb/camera_info', '/camera/color/camera_info')])
+```
+
+## Subscribed Topics
+
+Four independent input sets; connect exactly one.
+
+**RGB-D**
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `mono8`, `mono16`, `bgr8`, `rgb8`, `bgra8`, `rgba8` or `bayer_grbg8`. |
+| `depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | `32FC1`, `16UC1` or `mono16`, **registered to the color camera**. |
+| `rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+
+**Stereo**
+
+| Topic | Type | Description |
+|---|---|---|
+| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. |
+| `right/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Rectified. |
+| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+| `right/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Its `P(0,3)` carries the baseline. |
+
+Dense matching is done on the fly with OpenCV's block matcher; see [Stereo matching](#stereo-matching).
+
+**Disparity**
+
+| Topic | Type | Description |
+|---|---|---|
+| `left/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | Supplies the color. |
+| `disparity` | [`stereo_msgs/msg/DisparityImage`](https://docs.ros.org/en/jazzy/p/stereo_msgs/msg/DisparityImage.html) | `32FC1` or `16SC1`. |
+| `left/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+
+**Bundled**
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | A whole frame in one message, RGB-D or stereo. No synchronization needed, so this is the most reliable input. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | `XYZRGB`, or `XYZRGBNormal` when normals are enabled. |
+
+Nothing is computed unless `cloud` has a subscriber.
+
+## Parameters
+
+Identical to [point_cloud_xyz](point_cloud_xyz.md#parameters), with [`image_transport`](https://docs.ros.org/en/jazzy/p/image_transport/) added and the `Stereo*` family below.
+
+**Synchronization**
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `approx_sync` | `bool` | `true` | Match the inputs by nearest stamp. Set false when they share exact stamps. |
+| `approx_sync_max_interval` | `double` | `0.0` | Reject sets spanning more than this many seconds. `0` disables. |
+| `topic_queue_size` | `int` | `1` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `qos` | `int` | `0` | Reliability of the image and disparity subscriptions. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscriptions. |
+| `image_transport` | `string` | `"raw"` | `image_transport` plugin for the color, left and right images. |
+| `depth_transport` | `string` | `"raw"` | `image_transport` plugin for `depth/image`. |
+
+**Projection and filtering**, applied in this order
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `decimation` | `int` | `1` | Keep one pixel in `decimation`, in each direction. |
+| `roi_ratios` | `string` | `""` | Crop before projecting, `"left right top bottom"`. **Ignored for stereo input**, which warns if you set it. |
+| `min_depth` | `double` | `0.0` | Discard points nearer than this, in meters. `0` disables. |
+| `max_depth` | `double` | `0.0` | Discard points further than this, in meters. `0` disables. |
+| `voxel_size` | `double` | `0.0` | Downsample to one point per voxel, in meters. `0` disables. |
+| `noise_filter_radius` | `double` | `0.0` | Radius outlier removal, in meters. `0` disables. |
+| `noise_filter_min_neighbors` | `int` | `5` | Neighbors needed within `noise_filter_radius`. |
+| `normal_k` | `int` | `0` | Estimate normals from this many neighbors. `0` disables. |
+| `normal_radius` | `double` | `0.0` | Estimate normals within this radius. `0` disables. |
+| `filter_nans` | `bool` | `false` | Drop invalid points instead of leaving them NaN, giving an unorganized cloud. See [point_cloud_xyz](point_cloud_xyz.md#organized-output). |
+
+## Stereo matching
+
+The stereo and `rgbd_image`-with-stereo inputs run OpenCV's block matcher, configured through RTAB-Map's `StereoBM/*` parameters, which are exposed as ROS parameters of this node:
+
+```bash
+-p StereoBM/NumDisparities:=64 -p StereoBM/BlockSize:=15
+```
+
+The full list is in RTAB-Map's [parameter reference](https://introlab.github.io/rtabmap/api/latest/parameters.html). The two that matter most are `StereoBM/NumDisparities` (must exceed the largest disparity you expect, and must not exceed the image width) and `StereoBM/BlockSize`.
+
+If you already have a disparity image, feed the disparity input instead — it skips the matching entirely.
+
+## Notes
+
+For RGB-D input the depth **must be registered to the color camera**: the node pairs pixel `(u,v)` of the color image with pixel `(u,v)` of the depth image and uses one calibration for both. Unregistered depth gives a cloud whose colors are offset from its geometry. Most drivers offer an aligned depth stream for this reason.
+
+An `rgbd_image` carrying only color and no depth is valid and yields an empty cloud rather than an error.
diff --git a/rtabmap_util/doc/pointcloud_to_depthimage.md b/rtabmap_util/doc/pointcloud_to_depthimage.md
new file mode 100644
index 00000000..f1c0b5cc
--- /dev/null
+++ b/rtabmap_util/doc/pointcloud_to_depthimage.md
@@ -0,0 +1,124 @@
+# pointcloud_to_depthimage
+
+Projects a point cloud into a camera to make a depth image registered to it.
+
+Given a cloud (from a 3D lidar or a ToF camera) and the `camera_info` of an RGB camera, this node projects the points into that camera and outputs the depth image it would have produced if it were an RGB-D sensor: same intrinsics, same size, pixel `(u,v)` of the depth image lining up with pixel `(u,v)` of the color image. The result plugs into anything that consumes depth images: RTAB-Map's RGB-D pipeline, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/), obstacle avoidance built for depth cameras.
+
+Two typical setups:
+
+* **Lidar + one or more RGB cameras.** The natural way to feed a lidar into an RGB-D SLAM setup: the lidar supplies the geometry, the cameras the appearance. Run one instance per camera, each subscribing to the same cloud but to that camera's `camera_info`; a 3D lidar usually covers all of them at once. The resulting RGB-D streams can then be combined with [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`/`rgbdx_sync` and given to RTAB-Map through its `rgbd_cameras` parameter.
+* **ToF camera + RGB camera, not synchronized.** Two separate sensors, each with its own clock and its own pose, so their frames line up neither in time nor in space. Projecting the ToF cloud into the RGB camera registers the depth to the color image, and setting `fixed_frame_id` to a high-rate odometry frame — VIO, or an IMU-driven odometry running well above the camera rate — compensates the motion between the two stamps at the same time. See [Motion compensation](#motion-compensation).
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Required Transforms](#required-transforms)
+- [Parameters](#parameters)
+- [Motion compensation](#motion-compensation)
+- [Hole filling](#hole-filling)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util pointcloud_to_depthimage --ros-args \
+  -r cloud:=/velodyne_points \
+  -r camera_info:=/camera/color/camera_info \
+  -p fixed_frame_id:=odom -p decimation:=4 -p fill_holes_size:=2
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::PointCloudToDepthImage',
+    name='pointcloud_to_depthimage',
+    parameters=[{'fixed_frame_id': 'odom', 'decimation': 4, 'fill_holes_size': 2}],
+    remappings=[('cloud', '/velodyne_points'),
+                ('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 |
+|---|---|---|
+| `cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The geometry to project. An empty cloud yields an all-zero image rather than nothing. |
+| `camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | Defines the target camera: its intrinsics, its size, and through its `frame_id` its pose. Normally the RGB camera the depth image is being registered to. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`32FC1`) | Depth in **meters**, in the camera info's frame. |
+| `image_raw` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) (`16UC1`) | The same depth in **millimeters**. |
+| `image/camera_info`, `image_raw/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | The input calibration, rescaled if `decimation` is set. |
+| `cloud_transformed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The input cloud in the camera frame. Debugging aid; only published when hole filling is on and something subscribes. |
+
+Nothing is computed unless one of the two image topics has a subscriber.
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| cloud frame → camera frame | Where the cloud's sensor sits relative to the camera. |
+| `fixed_frame_id` → cloud frame, at both stamps | Only when `fixed_frame_id` is set, which is how motion between the two stamps is measured. See [Motion compensation](#motion-compensation). |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `fixed_frame_id` | `string` | `""` | Frame the sensor's motion is measured against, usually `odom`. **Required when `approx` is true.** See [Motion compensation](#motion-compensation). |
+| `approx` | `bool` | `true` | Match cloud and camera info by nearest stamp. Set false when the two share exact stamps, in which case `fixed_frame_id` is unnecessary. |
+| `wait_for_transform` | `double` | `0.1` | Seconds to wait for a transform before dropping the frame. |
+| `decimation` | `int` | `1` | Render at 1/`decimation` of the camera info's resolution. The published camera info is scaled to match. Must divide both the width and the height exactly, otherwise it is ignored with an error and the image comes out full size. See [Hole filling](#hole-filling). |
+| `fill_holes_size` | `int` | `0` | Radius, in pixels, for filling gaps between projected points. `0` disables. See [Hole filling](#hole-filling). |
+| `fill_holes_error` | `double` | `0.1` | Largest depth difference, in meters, across which a hole may be filled. |
+| `fill_iterations` | `int` | `1` | How many times to repeat the filling pass. |
+| `upscale` | `bool` | `false` | Interpolate the depth image back to full resolution after rendering. Only has an effect when `decimation` is greater than 1, and only needed when the consumer requires full resolution. See [Hole filling](#hole-filling). |
+| `upscale_depth_error_ratio` | `double` | `0.02` | Relative depth difference tolerated across a block when upscaling. Above it the block is left empty rather than interpolated across an edge. |
+| `topic_queue_size` | `int` | `10` | Queue depth of each input subscription. |
+| `sync_queue_size` | `int` | `10` | Queue depth of the synchronizer. |
+| `qos` | `int` | `0` | Reliability of the cloud subscription. |
+| `qos_camera_info` | `int` | value of `qos` | Reliability of the camera info subscription. |
+
+## Motion compensation
+
+The cloud's sensor and the camera almost never fire at the same instant, and on a moving robot that offset matters: projecting a cloud captured 40 ms earlier into the camera's current pose puts everything in the wrong place.
+
+When `fixed_frame_id` is set, the node asks TF how the cloud's frame moved between the two stamps and folds that displacement into the projection, so the cloud is placed where the camera was **at its own stamp**. Driving forward at 1 m/s with a 40 ms offset moves everything 4 cm — enough to matter at close range.
+
+The lookup is only as good as the frame it measures against: TF interpolates between the samples it has, so the source publishing `fixed_frame_id` should run well above the sensor rate. A VIO or wheel odometry at 100+ Hz gives a meaningful displacement over a 40 ms gap; a 1 Hz SLAM output does not.
+
+Without `fixed_frame_id` the stamp difference is silently ignored, which is why the node logs a fatal error if `approx` is true and no fixed frame is given. If the transform cannot be found the frame is dropped rather than projected wrongly.
+
+## Hole filling
+
+A lidar cloud is far sparser than a camera image, so a direct projection is mostly gaps: individual pixels with depth, surrounded by zeros.
+
+**Start with `decimation`.** Rendering at a coarser resolution puts more points in each pixel, so the wide gaps between lidar rings largely disappear instead of having to be filled in afterwards. `decimation: 4` is a reasonable starting point for a 3D lidar against a full-resolution camera. The published camera info is scaled to match, so consumers that read it keep working at the smaller size.
+
+**Then close what is left with `fill_holes_size`.** It spreads each point over a small neighborhood, but only across depth differences smaller than `fill_holes_error`, so it fills a surface without bridging the gap between a foreground object and the wall behind it. Start at `2` and raise it only if the image is still speckled; too large and thin structures get fattened.
+
+`upscale` is for the specific case where the consumer needs the depth image back at the camera's full resolution — pairing it pixel-for-pixel with the full-size color image, for instance. It interpolates each decimated block bilinearly from its corners, and only where all four have depth and agree to within `upscale_depth_error_ratio`, so it stops at depth discontinuities rather than stretching a foreground object onto the wall behind it. Leave it off otherwise: it restores resolution the lidar never measured, at full-resolution cost.
+
+## Notes
+
+The output is dense in *layout* but sparse in *content*: pixels with no return are zero, the ROS convention for no reading. Consumers that assume every pixel is valid will need to handle that.
diff --git a/rtabmap_util/doc/rgbd_relay.md b/rtabmap_util/doc/rgbd_relay.md
new file mode 100644
index 00000000..f3574b7d
--- /dev/null
+++ b/rtabmap_util/doc/rgbd_relay.md
@@ -0,0 +1,121 @@
+# rgbd_relay
+
+Republishes an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html), optionally compressing or decompressing it on the way through.
+
+An `RGBDImage` can carry its images raw or compressed. This node converts between the two so that the expensive form crosses the network only where it has to: compress before a wifi link, decompress on the other side.
+
+With both `compress` and `uncompress` left false the message is forwarded untouched, which makes the node a plain relay — useful to give a topic a second name, or to bridge two incompatible QoS profiles with `qos_sub` and `qos_pub`. See [Bridging QoS profiles](#bridging-qos-profiles).
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Bridging QoS profiles](#bridging-qos-profiles)
+- [Notes](#notes)
+
+## Usage
+
+Compress before sending over a slow link:
+
+```bash
+ros2 run rtabmap_util rgbd_relay --ros-args \
+  -r rgbd_image:=/camera/rgbd_image \
+  -p compress:=true
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::RGBDRelay',
+    name='rgbd_relay',
+    parameters=[{'compress': True}],
+    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 |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. |
+
+## Published Topics
+
+| Topic | Type | Description |
+|---|---|---|
+| `rgbd_image_relay` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_pub`, 1 by default. Published only when someone is subscribed. |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth. Fields already compressed on input are passed through as-is. |
+| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. |
+| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
+| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. |
+| `qos_pub` | `int` | value of `qos` | Reliability of the `rgbd_image_relay` publisher alone. |
+| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. |
+| `queue_pub` | `int` | `1` | Queue depth of the `rgbd_image_relay` publisher. Must be at least 1. |
+
+## Bridging QoS profiles
+
+A subscriber that asks for **reliable** will not connect to a publisher offering **best effort** — the request cannot be satisfied, so the two silently never match. A best-effort subscriber, on the other hand, connects to either.
+
+That is a real problem when a camera driver publishes best effort and the consumer insists on reliable. Set the two sides of the relay separately and it forwards across the gap:
+
+```bash
+ros2 run rtabmap_util rgbd_relay --ros-args \
+  -r rgbd_image:=/camera/rgbd_image \
+  -p qos_sub:=2 \
+  -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`.
+
+## Notes
+
+Setting neither `compress` nor `uncompress` forwards the message unchanged and skips all image handling — the cheapest path by a wide margin.
+
+Setting both is allowed and produces a message carrying each image twice, raw and compressed. That is rarely what you want.
+
+Depth is compressed as **PNG**, a stereo right image as **JPEG**.
diff --git a/rtabmap_util/doc/rgbd_split.md b/rtabmap_util/doc/rgbd_split.md
new file mode 100644
index 00000000..fac1b1b2
--- /dev/null
+++ b/rtabmap_util/doc/rgbd_split.md
@@ -0,0 +1,97 @@
+# rgbd_split
+
+Splits an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) back into the standard ROS image topics.
+
+`RGBDImage` bundles color, depth and both camera infos into one message so they arrive together, which is what RTAB-Map wants. Everything else in the ROS ecosystem — RViz, `image_view`, [`depth_image_proc`](https://docs.ros.org/en/jazzy/p/depth_image_proc/) — expects separate `Image` and `CameraInfo` topics. This node unpacks the bundle for them.
+
+It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_sync`, and of its `stereo_sync` when `stereo` is set — those two are what produce an `RGBDImage` in the first place.
+
+## Contents
+
+- [Usage](#usage)
+- [Subscribed Topics](#subscribed-topics)
+- [Published Topics](#published-topics)
+- [Parameters](#parameters)
+- [Stereo messages](#stereo-messages)
+- [Notes](#notes)
+
+## Usage
+
+```bash
+ros2 run rtabmap_util rgbd_split --ros-args -r rgbd_image:=/camera/rgbd_image
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::RGBDSplit',
+    name='rgbd_split',
+    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 |
+|---|---|---|
+| `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Queue depth `queue_sub`, 5 by default. Raw or compressed images are both accepted. |
+
+## Published Topics
+
+The output topics are named after the **resolved** input topic, so remapping `rgbd_image` moves the outputs with it. With `rgbd_image` remapped to `/camera/rgbd_image` they are `/camera/rgbd_image/rgb/image` and so on. Setting `stereo: true` renames the two halves `left` and `right`, see [Stereo messages](#stereo-messages).
+
+| Topic | Type | Description |
+|---|---|---|
+| `<rgbd_image>/rgb/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The color image, decompressed if needed. |
+| `<rgbd_image>/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
+| `<rgbd_image>/depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The depth image, or the right image of a stereo pair. |
+| `<rgbd_image>/depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | For a stereo pair this is the right camera, and its `P(0,3)` carries the baseline. |
+
+Each half is only unpacked if something is subscribed to it, so subscribing to color alone does not pay for depth decompression.
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. |
+| `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. |
+| `qos_pub` | `int` | value of `qos` | Reliability of the four output publishers alone. |
+| `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. |
+| `queue_pub` | `int` | `1` | Queue depth of every publisher. Must be at least 1. |
+| `stereo` | `bool` | `false` | Name the outputs `left`/`right` instead of `rgb`/`depth`. See [Stereo messages](#stereo-messages). |
+
+## Stereo messages
+
+The node handles **stereo** `RGBDImage` messages as well as RGB-D ones. In a stereo message the "depth" slot holds the right image, and the second camera info carries the baseline; the depth topics then carry the right camera, correctly typed as `mono8` or `bgr8` rather than mislabeled as depth.
+
+That works, but the topic names lie. Set `stereo: true` and the outputs are named for what they hold:
+
+| `stereo` | Output topics |
+|---|---|
+| `false` (default) | `<rgbd_image>/rgb/image`, `<rgbd_image>/rgb/camera_info`, `<rgbd_image>/depth/image`, `<rgbd_image>/depth/camera_info` |
+| `true` | `<rgbd_image>/left/image`, `<rgbd_image>/left/camera_info`, `<rgbd_image>/right/image`, `<rgbd_image>/right/camera_info` |
+
+```bash
+ros2 run rtabmap_util rgbd_split --ros-args \
+  -r rgbd_image:=/camera/rgbd_image \
+  -p stereo:=true
+```
+
+Only the names change — the message contents and the order of the two halves are the same either way, so the `rgb` slot always becomes the left image. The two namings are exclusive: with `stereo: true` nothing is published on `rgb`/`depth`.
+
+The node checks the setting against what actually arrives, going by the encoding of the second half: `16UC1`, `32FC1` and `mono16` are depth, anything else is an image. If the two disagree it logs a warning **once** and keeps forwarding — a mismatch makes the topic name misleading, not the data wrong, so it is never worth dropping a frame over.
+
+## Notes
+
+If a message has no `frame_id` on one of its sub-messages, the node fills it in from the other one so the output is always usable by TF.
diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h
index f6dd3c61..b144fab5 100644
--- a/rtabmap_util/include/rtabmap_util/MapsManager.h
+++ b/rtabmap_util/include/rtabmap_util/MapsManager.h
@@ -57,22 +57,131 @@ class GridMap;
 
 namespace rtabmap_util {
 
+/**
+ * @brief Turns a pose graph into the map topics, and publishes them.
+ *
+ * Given a set of node poses and the sensor data behind them, MapsManager assembles the
+ * ground and obstacle point clouds, the 2D occupancy grid, the octomap and the elevation
+ * map, and publishes whichever of them somebody is subscribed to.
+ *
+ * It is shared by rtabmap_slam's `rtabmap` node and rtabmap_util's `map_assembler`, which
+ * is why those two produce identical maps from identical parameters.
+ *
+ * @par Lifecycle
+ * Callers follow a fixed order:
+ * 1. init() to declare the ROS parameters and advertise the topics,
+ * 2. backwardCompatibilityParameters() to pick up parameters that have since moved into
+ *    the RTAB-Map library, then setParameters() to apply the whole set,
+ * 3. updateMapCaches() whenever the graph changes, then publishMaps().
+ *
+ * @par Laziness
+ * Nothing is assembled or published without a subscriber, and with `map_cleanup` set the
+ * caches are released once the last one goes away. A node can therefore call
+ * updateMapCaches() and publishMaps() unconditionally on every graph update and pay
+ * nothing while nobody is listening.
+ *
+ */
 class MapsManager {
 public:
 	MapsManager();
 	virtual ~MapsManager();
+
+	/**
+	 * @brief Declares the ROS parameters and advertises the map topics on @p node.
+	 *
+	 * Must be called before anything else, and exactly once per node: the parameters are
+	 * declared here, and declaring them twice throws.
+	 *
+	 * @param node               node to advertise on and read parameters from
+	 * @param name               prefix used in the log lines, normally the node's name
+	 * @param usePublicNamespace unused, kept for source compatibility
+	 */
 	void init(rclcpp::Node & node, const std::string & name, bool usePublicNamespace);
+
+	/// Drops every cached local grid, assembled cloud and global map.
 	void clear();
+
+	/// @return True if any map topic has at least one subscriber.
 	bool hasSubscribers() const;
+
+	/// @return True if the map topics are latched, i.e. delivered to late subscribers.
 	bool isLatching() const {return latching_;}
+
+	/**
+	 * @brief Whether the map changed on the last updateMapCaches().
+	 *
+	 * @note Reports true when nothing is subscribed to the grid topics. The answer comes
+	 *       from OccupancyGrid::update(), which only runs when a grid is wanted, so with
+	 *       nobody listening the safe assumption is that the graph moved.
+	 */
 	bool isMapUpdated() const;
+
+	/**
+	 * @brief Copies parameters that moved from rtabmap_ros into the RTAB-Map library.
+	 *
+	 * Reads the old ROS parameter names off @p node and, for each one that is set, writes
+	 * its value into @p parameters under the RTAB-Map name that replaced it, with a
+	 * warning. Call it before setParameters().
+	 *
+	 * @param[in]     node       node to read the legacy parameters from
+	 * @param[in,out] parameters parameter set to fill in
+	 */
 	void backwardCompatibilityParameters(rclcpp::Node & node, rtabmap::ParametersMap & parameters) const;
+
+	/**
+	 * @brief Applies the RTAB-Map parameters, rebuilding the map objects.
+	 *
+	 * The occupancy grid, octomap and elevation map are recreated, so anything already
+	 * assembled is lost; the cached local grids are kept.
+	 */
 	void setParameters(const rtabmap::ParametersMap & parameters);
+
+	/**
+	 * @brief Installs an already assembled 2D map, e.g. one loaded from a database.
+	 *
+	 * @param map      the grid, `CV_8SC1` with -1 unknown, 0 free, 100 occupied
+	 * @param xMin     world x of the map's origin, in meters
+	 * @param yMin     world y of the map's origin, in meters
+	 * @param cellSize resolution, in meters
+	 * @param poses    poses of the nodes @p map was assembled from
+	 * @param memory   optional memory to load the missing local grids from, so the map can
+	 *                 keep growing from where it left off
+	 *
+	 * @warning @p poses must not be empty. The grid is kept only together with the nodes
+	 *          it came from, so that the manager knows which are already in it; a call
+	 *          with no poses is ignored with a warning.
+	 */
 	void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
 
+	/**
+	 * @brief Applies the `map_filter_radius`/`map_filter_angle` thinning to @p poses.
+	 * @return The poses that survive, or all of them when filtering is disabled.
+	 */
 	std::map<int, rtabmap::Transform> getFilteredPoses(
 			const std::map<int, rtabmap::Transform> & poses);
 
+	/**
+	 * @brief Brings the local grid cache and the global maps up to date with the graph.
+	 *
+	 * For every pose not already mapped, the local occupancy grid is taken from the
+	 * signature or the memory, or regenerated from the sensor data when the node carries
+	 * none, and added to the cache. The global maps are then reassembled.
+	 *
+	 * @param poses         node poses; landmarks (negative ids) are ignored, and id 0 is
+	 *                      the not-yet-committed node, kept only if `map_always_update`
+	 * @param memory        memory to load node data from, may be null if @p signatures
+	 *                      carries everything
+	 * @param updateGrid    force the occupancy grid to be updated
+	 * @param updateOctomap force the octomap to be updated
+	 * @param signatures    node data, keyed by id, for nodes not in @p memory
+	 * @return The poses actually mapped, after filtering.
+	 *
+	 * @note With @p updateGrid and @p updateOctomap both false, what gets updated is
+	 *       decided by which topics have subscribers. That is also the only way the
+	 *       elevation map is ever built, as it has no flag of its own.
+	 * @note At least one of @p memory and @p signatures must be non-empty, and @p poses
+	 *       must not be empty; otherwise an error is logged and nothing is returned.
+	 */
 	std::map<int, rtabmap::Transform> updateMapCaches(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rtabmap::Memory * memory,
@@ -80,25 +189,49 @@ public:
 			bool updateOctomap,
 			const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
 
+	/**
+	 * @brief Publishes every map topic that has a subscriber.
+	 *
+	 * @param poses      the same poses updateMapCaches() returned
+	 * @param stamp      stamp for all published messages
+	 * @param mapFrameId frame id for all published messages
+	 */
 	void publishMaps(
 			const std::map<int, rtabmap::Transform> & poses,
 			const rclcpp::Time & stamp,
 			const std::string & mapFrameId);
 
+	/**
+	 * @brief The 2D occupancy grid as a ternary map.
+	 * @param[out] xMin         world x of the map's origin, in meters
+	 * @param[out] yMin         world y of the map's origin, in meters
+	 * @param[out] gridCellSize resolution, in meters
+	 * @return `CV_8SC1`, -1 unknown, 0 free, 100 occupied. Empty if nothing is assembled.
+	 */
 	cv::Mat getGridMap(
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
 
+	/**
+	 * @brief The 2D occupancy grid as probabilities.
+	 * @param[out] xMin         world x of the map's origin, in meters
+	 * @param[out] yMin         world y of the map's origin, in meters
+	 * @param[out] gridCellSize resolution, in meters
+	 * @return `CV_8SC1`, -1 unknown, otherwise 0-100. Empty if nothing is assembled.
+	 */
 	cv::Mat getGridProbMap(
 			float & xMin,
 			float & yMin,
 			float & gridCellSize);
 
 #ifdef RTABMAP_OCTOMAP
+	/// @return The octomap, owned by this object. Never null.
 	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
 #endif
+	/// @return The global occupancy grid, owned by this object. Never null.
 	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
+	/// @return The local grid segmenter, owned by this object. Never null.
 	const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
 
 private:
diff --git a/rtabmap_util/include/rtabmap_util/map_assembler.hpp b/rtabmap_util/include/rtabmap_util/map_assembler.hpp
index 8c87adeb..7972d57f 100644
--- a/rtabmap_util/include/rtabmap_util/map_assembler.hpp
+++ b/rtabmap_util/include/rtabmap_util/map_assembler.hpp
@@ -62,6 +62,9 @@ private:
 
 	void timerCallback();
 
+	/// Subscribes to "mapData"; the node is live from here on.
+	void subscribeToMapData();
+
 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
 	void octomapBinaryCallback(
@@ -79,6 +82,7 @@ private:
 private:
 	MapsManager mapsManager_;
 	std::map<int, rtabmap::Signature> nodes_;
+	int lastNodeAdded_;
 	std::map<int, rtabmap::Transform> optimizedPoses_;
 	std::string mapFrameId_;
 	std::string rtabmapNodeName_;
@@ -99,6 +103,7 @@ private:
 #endif
 #endif
 	bool localGridsRegenerated_;
+	double initializeFromRtabmapTimeout_;
 };
 
 }
\ No newline at end of file
diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp
index ff72354a..6ae2820d 100644
--- a/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp
+++ b/rtabmap_util/include/rtabmap_util/point_cloud_aggregator.hpp
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/sync_policies/approximate_time.hpp>
 #include <message_filters/subscriber.hpp>
 #include <message_filters/sync_policies/exact_time.hpp>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 namespace rtabmap_util
 {
@@ -66,8 +67,7 @@ private:
 						 const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_2);
 	void combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs);
 
-	std::thread * warningThread_;
-	bool callbackCalled_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ExactSync4Policy;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> ApproxSync4Policy;
diff --git a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp
index cc756894..359cccbe 100644
--- a/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp
+++ b/rtabmap_util/include/rtabmap_util/point_cloud_assembler.hpp
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_conversions/MsgConversion.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap_sync/SyncDiagnostic.h>
 
 namespace rtabmap_util
 {
@@ -73,8 +74,7 @@ private:
 	void callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg);
 
 private:
-	std::thread * warningThread_;
-	bool callbackCalled_;
+	std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
 
 	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloudSub_;
 	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
diff --git a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp
index d31a736a..6478dc5e 100644
--- a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp
+++ b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp
@@ -48,6 +48,9 @@ public:
 	void callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const;
 
 private:
+	/// True when the outputs are named left/right rather than rgb/depth.
+	bool stereo_;
+
 	rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
 
 	image_transport::Publisher rgbPub_;
diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml
index 2db0550d..b42de754 100644
--- a/rtabmap_util/package.xml
+++ b/rtabmap_util/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_util</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's various useful nodes and nodelets.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
@@ -36,7 +36,10 @@
   <depend>rtabmap_sync</depend>
   <!-- <depend>grid_map_ros</depend> Not available on Lyrical -->
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>
diff --git a/rtabmap_util/rosdoc2.yaml b/rtabmap_util/rosdoc2.yaml
new file mode 100644
index 00000000..6b7fbdda
--- /dev/null
+++ b/rtabmap_util/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_util
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_util --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_util 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_util',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }
diff --git a/rtabmap_util/src/DbPlayerNode.cpp b/rtabmap_util/src/DbPlayerNode.cpp
index cd892cea..9b178149 100644
--- a/rtabmap_util/src/DbPlayerNode.cpp
+++ b/rtabmap_util/src/DbPlayerNode.cpp
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rclcpp/rclcpp.hpp"
 
+#include <stdexcept>
+
 #ifndef _WIN32
 #include <sys/ioctl.h>
 #include <termios.h>
@@ -83,7 +85,19 @@ int main(int argc, char **argv)
 	rclcpp::NodeOptions options;
 	options.arguments(arguments);
 
-	auto node = std::make_shared<rtabmap_util::DbPlayer>(options);
+	std::shared_ptr<rtabmap_util::DbPlayer> node;
+	try
+	{
+		node = std::make_shared<rtabmap_util::DbPlayer>(options);
+	}
+	catch(const std::exception & e)
+	{
+		// The node reports what went wrong before throwing; keep the process exit clean
+		// rather than letting an uncaught exception abort.
+		UERROR("%s", e.what());
+		rclcpp::shutdown();
+		return -1;
+	}
 
 	rclcpp::Rate pauseRate(10);
 
diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp
index 827d43c1..848676da 100644
--- a/rtabmap_util/src/MapsManager.cpp
+++ b/rtabmap_util/src/MapsManager.cpp
@@ -24,6 +24,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include "rtabmap_util/MapsManager.h"
 
 #include <rtabmap/utilite/ULogger.h>
@@ -135,40 +136,47 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
 
 	// mapping topics
 	latched_.clear();
-	gridMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	// Intra-process communication doesn't support transient local durability: when latching,
+	// disable it on these publishers, otherwise keep the node's setting.
+	rclcpp::PublisherOptions pubOptions;
+	if(latching_)
+	{
+		pubOptions.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+	}
+	gridMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&gridMapPub_, false));
-	gridProbMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	gridProbMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&gridProbMapPub_, false));
-	cloudMapPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	cloudMapPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&cloudMapPub_, false));
-	cloudObstaclesPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	cloudObstaclesPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&cloudObstaclesPub_, false));
-	cloudGroundPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	cloudGroundPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&cloudGroundPub_, false));
 
 #ifdef RTABMAP_OCTOMAP
 #ifdef WITH_OCTOMAP_MSGS
-	octoMapPubBin_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapPubBin_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapPubBin_, false));
-	octoMapPubFull_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapPubFull_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapPubFull_, false));
 #endif
-	octoMapCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); // FIXME latching option in ROS2?
+	octoMapCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions); // FIXME latching option in ROS2?
 	latched_.insert(std::make_pair((void*)&octoMapCloud_, false));
-	octoMapFrontierCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapFrontierCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false));
-	octoMapObstacleCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapObstacleCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false));
-	octoMapGroundCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapGroundCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapGroundCloud_, false));
-	octoMapEmptySpace_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapEmptySpace_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapEmptySpace_, false));
-	octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&octoMapProj_, false));
 #endif
 
 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
-	elevationMapPub_ = node.create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
+	elevationMapPub_ = node.create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), pubOptions);
 	latched_.insert(std::make_pair((void*)&elevationMapPub_, false));
 #endif
 }
@@ -273,6 +281,12 @@ void MapsManager::set2DMap(
 		const std::map<int, rtabmap::Transform> & poses,
 		const rtabmap::Memory * memory)
 {
+	if(!map.empty() && poses.empty())
+	{
+		UWARN("Ignoring the 2D map (%dx%d): no poses were given. Pass the poses of the "
+			  "nodes the map was assembled from.", map.cols, map.rows);
+		return;
+	}
 	occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
 	//update cache in case the map should be updated
 	if(memory && 
@@ -381,7 +395,7 @@ std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Trans
 		double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
 		return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
 	}
-	return std::map<int, Transform>();
+	return poses;
 }
 
 std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
@@ -443,6 +457,12 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		return std::map<int, rtabmap::Transform>();
 	}
 
+	if(posesIn.empty())
+	{
+		UERROR("Poses are empty, cannot update map caches!");
+		return std::map<int, rtabmap::Transform>();
+	}
+
 	// process only nodes (exclude landmarks)
 	std::map<int, rtabmap::Transform> poses;
 	if(posesIn.begin()->first < 0)
@@ -1033,7 +1053,7 @@ void MapsManager::publishMaps(
 			if(cloudGroundPub_->get_subscription_count())
 			{
 				sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
-				pcl::toROSMsg(*assembledGround_, *cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*assembledGround_, *cloudMsg);
 				cloudMsg->header.stamp = stamp;
 				cloudMsg->header.frame_id = mapFrameId;
 				cloudGroundPub_->publish(std::move(cloudMsg));
@@ -1042,7 +1062,7 @@ void MapsManager::publishMaps(
 			if(cloudObstaclesPub_->get_subscription_count())
 			{
 				sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
-				pcl::toROSMsg(*assembledObstacles_, *cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(*assembledObstacles_, *cloudMsg);
 				cloudMsg->header.stamp = stamp;
 				cloudMsg->header.frame_id = mapFrameId;
 				cloudObstaclesPub_->publish(std::move(cloudMsg));
@@ -1052,7 +1072,7 @@ void MapsManager::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB> cloud = *assembledObstacles_ + *assembledGround_;
 				sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
-				pcl::toROSMsg(cloud, *cloudMsg);
+				rtabmap_conversions::toPointCloud2Msg(cloud, *cloudMsg);
 				cloudMsg->header.stamp = stamp;
 				cloudMsg->header.frame_id = mapFrameId;
 
@@ -1160,7 +1180,7 @@ void MapsManager::publishMaps(
 				pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
 				pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
 				pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
-				pcl::toROSMsg(cloudOccupiedSpace, msg);
+				rtabmap_conversions::toPointCloud2Msg(cloudOccupiedSpace, msg);
 				msg.header.frame_id = mapFrameId;
 				msg.header.stamp = stamp;
 				octoMapCloud_->publish(msg);
@@ -1170,7 +1190,7 @@ void MapsManager::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB> cloudFrontier;
 				pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier);
-				pcl::toROSMsg(cloudFrontier, msg);
+				rtabmap_conversions::toPointCloud2Msg(cloudFrontier, msg);
 				msg.header.frame_id = mapFrameId;
 				msg.header.stamp = stamp;
 				octoMapFrontierCloud_->publish(msg);
@@ -1180,7 +1200,7 @@ void MapsManager::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
 				pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
-				pcl::toROSMsg(cloudObstacles, msg);
+				rtabmap_conversions::toPointCloud2Msg(cloudObstacles, msg);
 				msg.header.frame_id = mapFrameId;
 				msg.header.stamp = stamp;
 				octoMapObstacleCloud_->publish(msg);
@@ -1190,7 +1210,7 @@ void MapsManager::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
 				pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
-				pcl::toROSMsg(cloudGround, msg);
+				rtabmap_conversions::toPointCloud2Msg(cloudGround, msg);
 				msg.header.frame_id = mapFrameId;
 				msg.header.stamp = stamp;
 				octoMapGroundCloud_->publish(msg);
@@ -1200,7 +1220,7 @@ void MapsManager::publishMaps(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB> cloudEmptySpace;
 				pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace);
-				pcl::toROSMsg(cloudEmptySpace, msg);
+				rtabmap_conversions::toPointCloud2Msg(cloudEmptySpace, msg);
 				msg.header.frame_id = mapFrameId;
 				msg.header.stamp = stamp;
 				octoMapEmptySpace_->publish(msg);
@@ -1414,6 +1434,7 @@ void MapsManager::publishMaps(
 		msg->header.frame_id = mapFrameId;
 		msg->header.stamp = stamp;
 		elevationMapPub_->publish(std::move(msg));
+		latched_.at(&elevationMapPub_) = true;
 	}
 	if(elevationMapPub_->get_subscription_count() == 0)
 	{
diff --git a/rtabmap_util/src/nodelets/db_player.cpp b/rtabmap_util/src/nodelets/db_player.cpp
index a78ca5fa..cacd6796 100644
--- a/rtabmap_util/src/nodelets/db_player.cpp
+++ b/rtabmap_util/src/nodelets/db_player.cpp
@@ -28,6 +28,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_util/db_player.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
+#include <stdexcept>
+
 #include <image_transport/image_transport.hpp>
 
 #include <rtabmap_conversions/MsgConversion.h>
@@ -106,13 +108,14 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
     qosGlobalPose_ =  this->declare_parameter("qos_global_pose", qos_);
     qosGps_ =         this->declare_parameter("qos_gps", qos_);
     qosImu_ =         this->declare_parameter("qos_imu", qos_);
+    qosEnvSensor_ =   this->declare_parameter("qos_env_sensor", qos_);
 
     // A general 360 lidar with 0.5 deg increment
     scanAngleMin_ =         this->declare_parameter("scan_angle_min", -M_PI);
     scanAngleMax_ =         this->declare_parameter("scan_angle_max", M_PI);
     scanAngleIncrement_ =   this->declare_parameter("scan_angle_increment", M_PI / 720.0);
     scanRangeMin_ =         this->declare_parameter("scan_range_min", 0.0);
-    scanRangeMax_ =         this->declare_parameter("scan_range_max", 60);
+    scanRangeMax_ =         this->declare_parameter("scan_range_max", 60.0);
 
     RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
     RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
@@ -136,8 +139,11 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
 
     if(databasePath.empty())
     {
+        // Throwing rather than exiting: this node can be loaded in a component container
+        // next to others, and taking the whole process down with it would be rude.
         RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
-        exit(-1);
+        throw std::invalid_argument(
+            "db_player: parameter \"database\" must be set (path to a RTAB-Map database).");
     }
 
     databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
@@ -151,7 +157,8 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
     if(!reader_->init())
     {
         RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
-        exit(-1);
+        throw std::runtime_error(
+            uFormat("db_player: cannot open database \"%s\".", databasePath.c_str()));
     }
 
     const std::string servicePrefix = get_name() + std::string("/");
@@ -159,7 +166,7 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
     resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
     if(publishTf) {
-        tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+        tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
     }
 
     if(publishClock)
@@ -300,8 +307,12 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
 
     if(!odom.data().laserScanRaw().isEmpty())
     {
-        if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
+        // The publisher has to match the scan being replayed, not just whichever one has
+        // not been created yet: a 2D database must never advertise "scan_cloud".
+        if(odom.data().laserScanRaw().is2d())
         {
+          if(!scanPub_.get())
+          {
             scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_));
             if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
             {
@@ -316,6 +327,7 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
                 RCLCPP_INFO(get_logger(), "  scan_range_min=%f", scanRangeMin_);
                 RCLCPP_INFO(get_logger(), "  scan_range_max=%f", scanRangeMax_);
             }
+          }
         }
         else if(!scanCloudPub_.get())
         {
@@ -569,7 +581,6 @@ bool DbPlayer::publishNextFrame()
         envSensorPub_->get_subscription_count() > 0 &&
         !odom.data().envSensors().empty())
     {
-        rtabmap_msgs::msg::EnvSensor msg;
         for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter)
         {
             rtabmap_msgs::msg::EnvSensor msg;
diff --git a/rtabmap_util/src/nodelets/disparity_to_depth.cpp b/rtabmap_util/src/nodelets/disparity_to_depth.cpp
index e537b313..3c582c7f 100644
--- a/rtabmap_util/src/nodelets/disparity_to_depth.cpp
+++ b/rtabmap_util/src/nodelets/disparity_to_depth.cpp
@@ -28,6 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_util/disparity_to_depth.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+
 #include <image_transport/image_transport.hpp>
 
 #ifdef PRE_ROS_IRON
@@ -44,16 +47,27 @@ DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// Each side can be set independently so the node can bridge a producer and a
+	// consumer that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 1);
+	int queuePub = this->declare_parameter("queue_pub", 1);
+
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
 
 #ifdef PRE_ROS_LYRICAL
-	pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	pub16u_ = image_transport::create_publisher(this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());	
+	pub32f_ = image_transport::create_publisher(this, "depth", pubQos.get_rmw_qos_profile());
+	pub16u_ = image_transport::create_publisher(this, "depth_raw", pubQos.get_rmw_qos_profile());
 #else
-	pub32f_ = image_transport::create_publisher(*this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
-	pub16u_ = image_transport::create_publisher(*this, "depth_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	pub32f_ = image_transport::create_publisher(*this, "depth", pubQos);
+	pub16u_ = image_transport::create_publisher(*this, "depth_raw", pubQos);
 #endif
 
-	sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
+	sub_ = create_subscription<stereo_msgs::msg::DisparityImage>("disparity", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&DisparityToDepth::callback, this, std::placeholders::_1));
 }
 
 DisparityToDepth::~DisparityToDepth(){}
diff --git a/rtabmap_util/src/nodelets/imu_to_tf.cpp b/rtabmap_util/src/nodelets/imu_to_tf.cpp
index 48c1db40..9234e03c 100644
--- a/rtabmap_util/src/nodelets/imu_to_tf.cpp
+++ b/rtabmap_util/src/nodelets/imu_to_tf.cpp
@@ -41,7 +41,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) :
 {
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
 	tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
-	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+	tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
 
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
diff --git a/rtabmap_util/src/nodelets/lidar_deskewing.cpp b/rtabmap_util/src/nodelets/lidar_deskewing.cpp
index d15e189b..518b6f8e 100644
--- a/rtabmap_util/src/nodelets/lidar_deskewing.cpp
+++ b/rtabmap_util/src/nodelets/lidar_deskewing.cpp
@@ -61,7 +61,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
 			msg->header.frame_id,
 			fixedFrameId_,
 			msg->header.stamp,
-			rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds(msg->ranges.size()*msg->time_increment),
+			rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec) + rclcpp::Duration::from_seconds((msg->ranges.empty()?0:msg->ranges.size()-1)*msg->time_increment),
 			*tfBuffer_,
 			waitForTransformDuration_);
 	if(tmpT.isNull())
diff --git a/rtabmap_util/src/nodelets/map_assembler.cpp b/rtabmap_util/src/nodelets/map_assembler.cpp
index f966533c..40a01570 100644
--- a/rtabmap_util/src/nodelets/map_assembler.cpp
+++ b/rtabmap_util/src/nodelets/map_assembler.cpp
@@ -52,13 +52,20 @@ namespace rtabmap_util
 {
 MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 		Node("map_assembler", options),
+		lastNodeAdded_(-1),
 		rtabmapNodeName_("rtabmap"),
-		localGridsRegenerated_(false)
+		localGridsRegenerated_(false),
+		initializeFromRtabmapTimeout_(5.0)
 {
 	std::string configPath;
 	configPath = this->declare_parameter("config_path", configPath);
 	localGridsRegenerated_ = this->declare_parameter("regenerate_local_grids", localGridsRegenerated_);
 	rtabmapNodeName_ = this->declare_parameter("rtabmap", rtabmapNodeName_);
+	// Seconds to wait for rtabmap's get_map_data service on start-up, which is how
+	// map_assembler catches up on a map that already exists. Set it to 0 to skip the call
+	// entirely: the subscription to "mapData" is then created right away instead of after
+	// the wait, which is what you want when map_assembler starts before rtabmap.
+	initializeFromRtabmapTimeout_ = this->declare_parameter("initialize_from_rtabmap_timeout", initializeFromRtabmapTimeout_);
 
 	//parameters
 	rtabmap::ParametersMap parameters;
@@ -174,6 +181,7 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 	}
 
 	RCLCPP_INFO(this->get_logger(), "%s: regenerate_local_grids          = %s", this->get_name(), localGridsRegenerated_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "%s: initialize_from_rtabmap_timeout = %fs (0=don't ask rtabmap for the map)", this->get_name(), initializeFromRtabmapTimeout_);
 	mapsManager_.init(*this, this->get_name(), true);
 	mapsManager_.backwardCompatibilityParameters(*this, parameters);
 	mapsManager_.setParameters(parameters);
@@ -188,13 +196,29 @@ MapAssembler::MapAssembler(const rclcpp::NodeOptions & options) :
 #endif
 #endif
 
-	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
+	if(initializeFromRtabmapTimeout_ > 0.0)
+	{
+		std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
 
-	// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
-	serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-	timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-	client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
-	timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
+		// We cannot call the service and wait in the constructor, lets call it later and subscribe afterwards
+		serviceCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+		timerCbGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+		client_ = this->create_client<rtabmap_msgs::srv::GetMap>(getMapSrv, rclcpp::ServicesQoS(), serviceCbGroup_); // Put it in a different group than the timer
+		timer_ = this->create_wall_timer(1s, std::bind(&MapAssembler::timerCallback, this), timerCbGroup_);
+	}
+	else
+	{
+		subscribeToMapData();
+	}
+}
+
+void MapAssembler::subscribeToMapData()
+{
+	rclcpp::SubscriptionOptions options;
+	// Null unless we came through the timer, in which case the node's default group is used.
+	options.callback_group = timerCbGroup_;
+	mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1),
+		std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
 }
 
 MapAssembler::~MapAssembler() {}
@@ -212,7 +236,8 @@ void MapAssembler::timerCallback()
 	std::string getMapSrv = rtabmapNodeName_+"/get_map_data";
 	RCLCPP_INFO(this->get_logger(), "Calling service \"%s\"...", getMapSrv.c_str());
 	
-	if(client_->wait_for_service(5s))
+	if(client_->wait_for_service(
+			std::chrono::duration<double>(initializeFromRtabmapTimeout_)))
 	{
 		auto request = std::make_shared<rtabmap_msgs::srv::GetMap::Request>();
 		request->global_map = false;
@@ -236,18 +261,16 @@ void MapAssembler::timerCallback()
 	}
 	else
 	{
-		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for 5 seconds, "
+		RCLCPP_WARN(this->get_logger(), "Service \"%s\" not available after waiting for %f seconds, "
 				"may not be a problem if rtabmap is started afterwards. If rtabmap "
 				"is started after in localization mode, call %s/publish_maps "
 				"service with graph_only=false to make sure map_assembler has all the data.",
 					getMapSrv.c_str(),
+					initializeFromRtabmapTimeout_,
 					rtabmapNodeName_.c_str());
 	}
 
-	rclcpp::SubscriptionOptions options;
-	options.callback_group =  timerCbGroup_;
-	mapDataSub_ = create_subscription<rtabmap_msgs::msg::MapData>("mapData", rclcpp::QoS(1), 
-		std::bind(&MapAssembler::mapDataReceivedCallback, this, std::placeholders::_1), options);
+	subscribeToMapData();
 }
 
 void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::ConstSharedPtr msg)
@@ -256,12 +279,28 @@ void MapAssembler::mapDataReceivedCallback(const rtabmap_msgs::msg::MapData::Con
 }
 void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
 {
+	if(msg.graph.poses.empty() && msg.nodes.empty())
+	{
+		// empty map, nothing to update
+		return;
+	}
+
 	UTimer timer;
 
 	std::map<int, rtabmap::Transform> poses;
 	std::multimap<int, rtabmap::Link> constraints;
 	rtabmap::Transform mapOdom;
 	rtabmap_conversions::mapGraphFromROS(msg.graph, poses, constraints, mapOdom);
+
+	// If the last node added to the cache is not in the graph anymore, it has been
+	// discarded by rtabmap (e.g., too small motion), so remove it from the cache.
+	if(lastNodeAdded_>0 && poses.find(lastNodeAdded_) == poses.end())
+	{
+		RCLCPP_DEBUG(get_logger(), "map_assembler: Removing node %d from cache (discarded by rtabmap)", lastNodeAdded_);
+		nodes_.erase(lastNodeAdded_);
+	}
+	lastNodeAdded_ = -1;
+
 	for(unsigned int i=0; i<msg.nodes.size(); ++i)
 	{
 		if(msg.nodes[i].data.left_compressed.size() ||
@@ -274,19 +313,13 @@ void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
 				data.sensorData().setOccupancyGrid(cv::Mat(), cv::Mat(), cv::Mat(), 0, cv::Point3f());
 			}
 			uInsert(nodes_, std::make_pair(msg.nodes[i].id, data));
+			if(msg.nodes[i].id > lastNodeAdded_)
+			{
+				lastNodeAdded_ = msg.nodes[i].id;
+			}
 		}
 	}
 
-	// create a tmp signature with latest sensory data
-	if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
-	{
-		rtabmap::Signature tmpS = nodes_.at(poses.rbegin()->first);
-		rtabmap::SensorData tmpData = tmpS.sensorData();
-		tmpData.setId(0);
-		uInsert(nodes_, std::make_pair(0, rtabmap::Signature(0, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), rtabmap::Transform(), tmpData)));
-		poses.insert(std::make_pair(0, poses.rbegin()->second));
-	}
-
 	// Update maps
 	if(!nodes_.empty())
 	{
@@ -297,6 +330,12 @@ void MapAssembler::processMapData(const rtabmap_msgs::msg::MapData & msg)
 				false,
 				nodes_);
 	}
+	else
+	{
+		// No data cached, republish the maps already assembled, applying
+		// the same pose filtering than updateMapCaches() would do.
+		poses = mapsManager_.getFilteredPoses(poses);
+	}
 	double updateTime = timer.ticks();
 
 	mapFrameId_ = msg.header.frame_id;
@@ -313,6 +352,9 @@ void MapAssembler::reset(const std::shared_ptr<rmw_request_id_t>,
 {
 	RCLCPP_INFO(this->get_logger(), "map_assembler: reset!");
 	mapsManager_.clear();
+	nodes_.clear();
+	lastNodeAdded_ = -1;
+	optimizedPoses_.clear();
 }
 
 #ifdef WITH_OCTOMAP_MSGS
@@ -326,7 +368,10 @@ void MapAssembler::octomapBinaryCallback(
 	res->map.header.frame_id = mapFrameId_;
 	res->map.header.stamp = now();
 
-	mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+	if(!optimizedPoses_.empty() && !nodes_.empty())
+	{
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+	}
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
 	if(octomap->octree()->size())
@@ -342,7 +387,10 @@ void MapAssembler::octomapFullCallback(
 	res->map.header.frame_id = mapFrameId_;
 	res->map.header.stamp = now();
 
-	mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+	if(!optimizedPoses_.empty() && !nodes_.empty())
+	{
+		mapsManager_.updateMapCaches(optimizedPoses_, 0, false, true, nodes_);
+	}
 
 	const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
 	if(octomap->octree()->size())
diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp
index 0f697caf..50cb8787 100644
--- a/rtabmap_util/src/nodelets/obstacles_detection.cpp
+++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_util/obstacles_detection.hpp>
 
 #include <pcl/point_cloud.h>
@@ -159,7 +160,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 			uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::fromROSMsg(*cloudMsg, *inputCloud);
+	rtabmap_conversions::fromPointCloud2Msg(*cloudMsg, *inputCloud);
 	if(inputCloud->isOrganized())
 	{
 		std::vector<int> indices;
@@ -273,7 +274,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 	if(groundPub_->get_subscription_count())
 	{
 		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-		pcl::toROSMsg(*groundCloud, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*groundCloud, *rosCloud);
 		rosCloud->header = cloudMsg->header;
 
 		//publish the message
@@ -283,7 +284,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 	if(obstaclesPub_->get_subscription_count())
 	{
 		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-		pcl::toROSMsg(*obstaclesCloud, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*obstaclesCloud, *rosCloud);
 		rosCloud->header = cloudMsg->header;
 
 		//publish the message
@@ -293,7 +294,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
 	if(projObstaclesPub_->get_subscription_count())
 	{
 		sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
-		pcl::toROSMsg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*obstaclesCloudWithoutFlatSurfaces, *rosCloud);
 		rosCloud->header.stamp = cloudMsg->header.stamp;
 		rosCloud->header.frame_id = frameId_;
 
diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
index a8cc5333..3b95c71b 100644
--- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
@@ -41,8 +41,6 @@ namespace rtabmap_util
 
 PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options) :
 	Node("point_cloud_aggregator", options),
-	warningThread_(0),
-	callbackCalled_(false),
 	exactSync4_(0),
 	approxSync4_(0),
 	exactSync3_(0),
@@ -162,23 +160,16 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
 	}
 
 
-	warningThread_ = new std::thread([&](){
-		rclcpp::Rate r(1.0/5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				RCLCPP_WARN(this->get_logger(), "%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-						"header are set. %s%s",
-						this->get_name(),
-						approx?"":"Parameter \"approx_sync\" is false, which means that input "
-							"topics should have all the exact timestamp for the callback to be called.",
-						subscribedTopicsMsg.c_str());
-			}
-		}
-	});
+	syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+	syncDiagnostic_->init(cloudSub_1_.getSubscriber()->get_topic_name(),
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+				"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
+				"header are set. %s%s",
+				this->get_name(),
+				approx?"":"Parameter \"approx_sync\" is false, which means that input "
+					"topics should have all the exact timestamp for the callback to be called.",
+				subscribedTopicsMsg.c_str()));
+
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg.c_str());
 }
 
@@ -190,13 +181,6 @@ PointCloudAggregator::~PointCloudAggregator()
 	delete approxSync3_;
 	delete exactSync2_;
 	delete approxSync2_;
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void PointCloudAggregator::clouds4_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg_1,
@@ -234,8 +218,8 @@ void PointCloudAggregator::clouds2_callback(const sensor_msgs::msg::PointCloud2:
 }
 void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::PointCloud2::ConstSharedPtr> & cloudMsgs)
 {
-	callbackCalled_ = true;
 	UASSERT(cloudMsgs.size() > 1);
+	syncDiagnostic_->tickInput(cloudMsgs[0]->header.stamp);
 	if(cloudPub_->get_subscription_count())
 	{
 		pcl::PCLPointCloud2::Ptr output(new pcl::PCLPointCloud2);
@@ -420,6 +404,7 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
 		rosCloud->header.frame_id = frameId;
 		cloudPub_->publish(std::move(rosCloud));
 	}
+	syncDiagnostic_->tickOutput(cloudMsgs[0]->header.stamp);
 }
 }
 
diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp
index 81302ac1..975dec03 100644
--- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp
@@ -45,8 +45,6 @@ namespace rtabmap_util
 
 PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 	Node("point_cloud_assembler", options),
-	warningThread_(0),
-	callbackCalled_(false),
 	exactSync_(0),
 	exactInfoSync_(0),
 	maxClouds_(0),
@@ -170,22 +168,14 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
 							syncOdomSub_.getSubscriber()->get_topic_name());
 	}
 
-	warningThread_ = new std::thread([&](){
-		rclcpp::Rate r(1.0/5.0);
-		while(!callbackCalled_)
-		{
-			r.sleep();
-			if(!callbackCalled_)
-			{
-				RCLCPP_WARN(this->get_logger(),
-						"%s: Did not receive data since 5 seconds! Make sure the input topics are "
-						"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
-						"header are set. %s",
-						get_name(),
-						subscribedTopicsMsg_.c_str());
-			}
-		}
-	});
+	syncDiagnostic_.reset(new rtabmap_sync::SyncDiagnostic(this, 0.5));
+	syncDiagnostic_->init(
+		cloudSub_?cloudSub_->get_topic_name():syncCloudSub_.getSubscriber()->get_topic_name(),
+		uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
+				"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
+				"header are set. %s",
+				get_name(),
+				subscribedTopicsMsg_.c_str()));
 
 	RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
 }
@@ -194,20 +184,12 @@ PointCloudAssembler::~PointCloudAssembler()
 {
 	delete exactSync_;
 	delete exactInfoSync_;
-
-	if(warningThread_)
-	{
-		callbackCalled_=true;
-		warningThread_->join();
-		delete warningThread_;
-	}
 }
 
 void PointCloudAssembler::callbackCloudOdom(
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg,
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
 {
-	callbackCalled_ = true;
 	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
 	if(!odom.isNull())
 	{
@@ -270,7 +252,6 @@ void PointCloudAssembler::callbackCloudOdomInfo(
 			const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
-	callbackCalled_ = true;
 	rtabmap::Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
 	if(!odom.isNull())
 	{
@@ -293,7 +274,7 @@ void PointCloudAssembler::callbackCloudOdomInfo(
 
 void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
 {
-	callbackCalled_ = true;
+	syncDiagnostic_->tickInput(cloudMsg->header.stamp);
 	if(cloudPub_->get_subscription_count())
 	{
 		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
@@ -487,6 +468,7 @@ void PointCloudAssembler::callbackCloud(const sensor_msgs::msg::PointCloud2::Con
 					rosCloud.header.frame_id = frameId_;
 				}
 				cloudPub_->publish(rosCloud);
+				syncDiagnostic_->tickOutput(cloudMsg->header.stamp);
 				if(circularBuffer_)
 				{
 					if(!isMoving)
diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp
index 98532d7e..6c01e60a 100644
--- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_util/point_cloud_xyz.hpp>
 
 #include <rtabmap_conversions/MsgConversion.h>
@@ -345,7 +346,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_
 		{
 			pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
 		}
-		pcl::toROSMsg(*pclCloudNormal, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud);
 	}
 	else
 	{
@@ -353,7 +354,7 @@ if(!pclCloud->empty() && (pclCloud->is_dense || !indices->empty()) && (normalK_
 		{
 			pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
 		}
-		pcl::toROSMsg(*pclCloud, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud);
 	}
 	rosCloud->header.stamp = header.stamp;
 	rosCloud->header.frame_id = header.frame_id;
diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
index f4dd9b39..16fac502 100644
--- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap_conversions/PointCloudConversion.h>
 #include <rtabmap_util/point_cloud_xyzrgb.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
@@ -538,7 +539,7 @@ void PointCloudXYZRGB::processAndPublish(
 		{
 			pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
 		}
-		pcl::toROSMsg(*pclCloudNormal, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*pclCloudNormal, *rosCloud);
 	}
 	else
 	{
@@ -546,7 +547,7 @@ void PointCloudXYZRGB::processAndPublish(
 		{
 			pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
 		}
-		pcl::toROSMsg(*pclCloud, *rosCloud);
+		rtabmap_conversions::toPointCloud2Msg(*pclCloud, *rosCloud);
 	}
 	rosCloud->header.stamp = header.stamp;
 	rosCloud->header.frame_id = header.frame_id;
diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
index 30c0cab1..44edcc35 100644
--- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
@@ -164,9 +164,10 @@ void PointCloudToDepthImage::callback(
 
 		if(cloudDisplacement.isNull())
 		{
-			RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
-				pointCloud2Msg->header.frame_id.c_str(), 
-				cameraInfoMsg->header.frame_id.c_str(),
+			RCLCPP_ERROR(this->get_logger(), "Could not find how %s moved between the cloud (%f) and the camera info (%f) stamps, accordingly to %s, aborting!",
+				pointCloud2Msg->header.frame_id.c_str(),
+				cloudStamp,
+				infoStamp,
 				fixedFrameId_.c_str());
 			return;
 		}
diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp
index 45749ccd..d3bcb301 100644
--- a/rtabmap_util/src/nodelets/rgbd_relay.cpp
+++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap_util
 {
@@ -54,11 +55,20 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// The two sides can be set independently so the relay can bridge a publisher
+	// and a subscriber that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 5);
+	int queuePub = this->declare_parameter("queue_pub", 1);
 	compress_ = this->declare_parameter("compress", compress_);
 	uncompress_ = this->declare_parameter("uncompress", uncompress_);
 
-	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
-	rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDRelay::callback, this, std::placeholders::_1));
+	rgbdImagePub_ = create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub));
 }
 
 void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const
@@ -125,7 +135,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				// already raw, just copy pointer
 				output->rgb = input->rgb;
 			}
-			if(!input->rgb_compressed.data.empty())
+			else if(!input->rgb_compressed.data.empty())
 			{
 				cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(output->rgb);
 			}
@@ -135,20 +145,37 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				// already raw, just copy pointer
 				output->depth = input->depth;
 			}
-			else if(input->depth_compressed.format.compare("jpg")==0)
+			else if(!input->depth_compressed.data.empty())
 			{
-				// right stereo image
-				cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(output->depth);
-			}
-			else
-			{
-				// dpeth image
+				// Decode first, then pick the encoding from what actually came out.
+				// Branching on the "jpg"/"png" format string instead would abort on a
+				// right image compressed as PNG, which nothing forbids.
 				auto cvImg = std::make_unique<cv_bridge::CvImage>();
 				cvImg->header = input->depth_compressed.header;
 				cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data);
-				UASSERT(cvImg->image.empty() || cvImg->image.type() == CV_32FC1 || cvImg->image.type() == CV_16UC1);
-				cvImg->encoding = cvImg->image.empty()?"":cvImg->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
-				cvImg->toImageMsg(output->depth);
+				if(cvImg->image.empty())
+				{
+					RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
+							rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
+				}
+				else
+				{
+					switch(cvImg->image.type())
+					{
+						case CV_32FC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
+						case CV_16UC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
+						case CV_8UC1:  cvImg->encoding = sensor_msgs::image_encodings::MONO8; break;
+						case CV_8UC3:  cvImg->encoding = sensor_msgs::image_encodings::BGR8; break;
+						default:
+							RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg->image.type());
+							cvImg->image = cv::Mat();
+							break;
+					}
+					if(!cvImg->image.empty())
+					{
+						cvImg->toImageMsg(output->depth);
+					}
+				}
 			}
 		}
 
diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp
index e8c401e7..7d3ad63e 100644
--- a/rtabmap_util/src/nodelets/rgbd_split.cpp
+++ b/rtabmap_util/src/nodelets/rgbd_split.cpp
@@ -26,6 +26,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap_util/rgbd_split.hpp>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <sensor_msgs/image_encodings.hpp>
 
 #ifdef PRE_ROS_IRON
 #include <cv_bridge/cv_bridge.h>
@@ -37,24 +41,55 @@ namespace rtabmap_util
 {
 
 RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
-	Node("rgbd_split", options)
+	Node("rgbd_split", options),
+	stereo_(false)
 {
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	qos = this->declare_parameter("qos", qos);
+	// Each side can be set independently so the node can bridge a producer and a
+	// consumer that don't agree on reliability. Both default to qos.
+	int qosSub = this->declare_parameter("qos_sub", qos);
+	int qosPub = this->declare_parameter("qos_pub", qos);
+	int queueSub = this->declare_parameter("queue_sub", 5);
+	int queuePub = this->declare_parameter("queue_pub", 1);
+	// A stereo RGBDImage carries the right image in the depth slot, so name the outputs
+	// left/right instead of rgb/depth to say what they really are.
+	stereo_ = this->declare_parameter("stereo", false);
 
 	RCLCPP_INFO(this->get_logger(), "%s: qos         = %d", get_name(), qos);
+	RCLCPP_INFO(this->get_logger(), "%s: queue_sub   = %d", get_name(), queueSub);
+	RCLCPP_INFO(this->get_logger(), "%s: queue_pub   = %d", get_name(), queuePub);
+	RCLCPP_INFO(this->get_logger(), "%s: stereo      = %s", get_name(), stereo_?"true":"false");
 
-	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
+	UASSERT_MSG(queueSub >= 1 && queuePub >= 1,
+			uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str());
+
+	rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSub).reliability((rmw_qos_reliability_policy_t)qosSub), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
+
+	const std::string base = rgbdImageSub_->get_topic_name();
+	const std::string firstName = stereo_?"/left":"/rgb";
+	const std::string secondName = stereo_?"/right":"/depth";
+	const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub);
 
 #ifdef PRE_ROS_LYRICAL
-	rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
-	depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());	
+	rgbPub_ = image_transport::create_publisher(this, base + firstName + "/image", pubQos.get_rmw_qos_profile());
+	depthPub_ = image_transport::create_publisher(this, base + secondName + "/image", pubQos.get_rmw_qos_profile());
 #else
-	rgbPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
-	depthPub_ = image_transport::create_publisher(*this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos);
+	depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos);
 #endif
-	rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1);
-	depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1);
+	rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + firstName + "/camera_info", pubQos);
+	depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(base + secondName + "/camera_info", pubQos);
+
+	// Resolved names: the outputs are derived from the input topic, so a remapping of
+	// "rgbd_image" moves all four with it. Print them so it is clear what to subscribe to.
+	RCLCPP_INFO(this->get_logger(), "%s: subscribed to:\n   %s", get_name(), rgbdImageSub_->get_topic_name());
+	RCLCPP_INFO(this->get_logger(), "%s: publishing:\n   %s,\n   %s,\n   %s,\n   %s",
+			get_name(),
+			rgbPub_.getTopic().c_str(),
+			rgbInfoPub_->get_topic_name(),
+			depthPub_.getTopic().c_str(),
+			depthInfoPub_->get_topic_name());
 }
 
 
@@ -100,7 +135,36 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 #ifdef CV_BRIDGE_HYDRO
 			ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this.");
 #else
-			cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
+			// Decode first, then pick the encoding from what actually came out. Going
+			// by the "jpg"/"png" format string instead would mislabel a depth PNG as
+			// mono8 (cv_bridge cannot infer 16-bit from it), and would abort outright on
+			// a right image compressed as PNG, which nothing forbids.
+			cv_bridge::CvImage cvImg;
+			cvImg.header = input->depth_compressed.header;
+			cvImg.image = rtabmap::uncompressImage(input->depth_compressed.data);
+			if(cvImg.image.empty())
+			{
+				RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").",
+						rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str());
+			}
+			else
+			{
+				switch(cvImg.image.type())
+				{
+					case CV_32FC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; break;
+					case CV_16UC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; break;
+					case CV_8UC1:  cvImg.encoding = sensor_msgs::image_encodings::MONO8; break;
+					case CV_8UC3:  cvImg.encoding = sensor_msgs::image_encodings::BGR8; break;
+					default:
+						RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg.image.type());
+						cvImg.image = cv::Mat();
+						break;
+				}
+			}
+			if(!cvImg.image.empty())
+			{
+				cvImg.toImageMsg(outputImage);
+			}
 #endif
 		}
 		if(outputCameraInfo.header.frame_id.empty()) {
@@ -119,6 +183,37 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
 				outputImage.header = outputCameraInfo.header;
 			}
 		}
+		// The "depth" slot of an RGBDImage holds either a depth image or the right image
+		// of a stereo pair, and "stereo" decides which name it goes out under. Warn when
+		// the two disagree: the topic name would be lying to every consumer downstream.
+		// Both directions only warn and keep forwarding -- publishing a right image on
+		// the depth topic is what this node has always done, and setups rely on it.
+		if(!outputImage.data.empty())
+		{
+			const bool isDepth =
+					outputImage.encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
+					outputImage.encoding == sensor_msgs::image_encodings::TYPE_32FC1 ||
+					outputImage.encoding == sensor_msgs::image_encodings::MONO16;
+			if(stereo_ && isDepth)
+			{
+				RCLCPP_WARN_ONCE(this->get_logger(),
+						"Parameter \"stereo\" is true, so the second half is published as \"%s\", "
+						"but the received image is a depth image (encoding=\"%s\"), not the right "
+						"image of a stereo pair. Set \"stereo\" to false to publish it as depth. "
+						"(This warning is printed only once)",
+						depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
+			}
+			else if(!stereo_ && !isDepth)
+			{
+				RCLCPP_WARN_ONCE(this->get_logger(),
+						"Parameter \"stereo\" is false, so the second half is published as \"%s\", "
+						"but the received image is not a depth image (encoding=\"%s\"): it looks "
+						"like the right image of a stereo pair. Set \"stereo\" to true to publish "
+						"it under a name that says so. (This warning is printed only once)",
+						depthPub_.getTopic().c_str(), outputImage.encoding.c_str());
+			}
+		}
+
 		depthPub_.publish(outputImage);
 		depthInfoPub_->publish(outputCameraInfo);
 	}
diff --git a/rtabmap_util/test/db_builders.hpp b/rtabmap_util/test/db_builders.hpp
new file mode 100644
index 00000000..bc6340d1
--- /dev/null
+++ b/rtabmap_util/test/db_builders.hpp
@@ -0,0 +1,366 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_UTIL_DB_BUILDERS_HPP_
+#define RTABMAP_UTIL_DB_BUILDERS_HPP_
+
+/**
+ * @file
+ * @brief Synthetic RTAB-Map databases for the db_player tests.
+ *
+ * db_player replays whatever a database happens to contain, and which topics it even
+ * creates depends on the payloads it finds. Rather than ship a recorded database, each
+ * scenario is written here with DBDriver so the expected values are visible right next
+ * to the assertions.
+ *
+ * @note Only the *compressed* buffers of a SensorData are persisted, so every payload is
+ *       compressed before being handed to the driver. Saving raw-only data silently
+ *       writes empty blobs.
+ */
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/EnvSensor.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+
+#include <gtest/gtest.h>
+
+#include <opencv2/core/core.hpp>
+
+#include <functional>
+#include <string>
+#include <vector>
+
+namespace rtabmap_util_test {
+
+//============================================================================
+// What every synthetic database contains, and what the tests assert against
+//============================================================================
+
+constexpr int kDbFrames = 3;              ///< nodes in each database
+constexpr double kFirstStamp = 1000.0;    ///< stamp of node 1, seconds
+constexpr double kStampStep = 0.05;       ///< seconds between consecutive nodes
+constexpr float kPoseStep = 0.5f;         ///< meters along x between odometry poses
+constexpr double kOdomVariance = 0.25;    ///< diagonal of the odometry covariance
+
+constexpr int kImageWidth = 80;
+constexpr int kImageHeight = 60;
+constexpr double kFx = 100.0;
+constexpr double kFy = 100.0;
+constexpr double kCx = 40.0;
+constexpr double kCy = 30.0;
+constexpr double kBaseline = 0.12;
+constexpr uint16_t kDepthMillimeters = 1500;
+
+constexpr double kGpsLongitude = -71.9;
+constexpr double kGpsLatitude = 45.4;
+constexpr double kGpsAltitude = 123.0;
+constexpr double kGpsError = 2.5;
+constexpr double kEnvSensorValue = 21.5;
+
+/// The odometry pose of node @p id, one step further along x than the previous one.
+inline rtabmap::Transform poseOf(int id)
+{
+	return rtabmap::Transform(kPoseStep * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+}
+
+/// The stamp of node @p id.
+inline double stampOfNode(int id)
+{
+	return kFirstStamp + kStampStep * double(id - 1);
+}
+
+/// Where the camera sits on the robot: 10 cm forward, 20 cm up, looking forward.
+inline rtabmap::Transform cameraLocalTransform()
+{
+	return rtabmap::Transform(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f) *
+			rtabmap::CameraModel::opticalRotation();
+}
+
+/// Where the lidar sits on the robot.
+inline rtabmap::Transform scanLocalTransform()
+{
+	return rtabmap::Transform(0.05f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f);
+}
+
+/// The ground truth pose of node @p id, offset from the odometry pose so they differ.
+inline rtabmap::Transform groundTruthOf(int id)
+{
+	return rtabmap::Transform(kPoseStep * float(id - 1), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+}
+
+/// The prior (global) pose of node @p id.
+inline rtabmap::Transform globalPoseOf(int id)
+{
+	return rtabmap::Transform(kPoseStep * float(id - 1), 2.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+}
+
+//============================================================================
+// A database file that cleans itself up
+//============================================================================
+
+/// A uniquely named database path under the test temp directory, erased on destruction.
+class TempDatabase
+{
+public:
+	explicit TempDatabase(const std::string & tag)
+	{
+		static int counter = 0;
+		path_ = std::string(::testing::TempDir()) +
+				uFormat("rtabmap_util_db_player_%s_%d_%d.db", tag.c_str(), (int)getpid(), ++counter);
+		UFile::erase(path_.c_str());
+	}
+
+	~TempDatabase() { UFile::erase(path_.c_str()); }
+
+	TempDatabase(const TempDatabase &) = delete;
+	TempDatabase & operator=(const TempDatabase &) = delete;
+
+	const std::string & path() const { return path_; }
+
+private:
+	std::string path_;
+};
+
+//============================================================================
+// Writing the databases
+//============================================================================
+
+/// Builds the sensor payload of node @p id; see the writeXxxDatabase() functions.
+typedef std::function<rtabmap::SensorData(int id, double stamp)> DataBuilder;
+
+/// Adds anything beyond the payload: links, ground truth, and so on.
+typedef std::function<void(int id, rtabmap::Signature & signature)> NodeDecorator;
+
+/**
+ * @brief Writes @p frames consecutive nodes sharing the plumbing every replay needs.
+ *
+ * Nodes are numbered from 1, stamped kStampStep apart (db_player replays at the database
+ * stamps, so a node without one aborts the read), posed kPoseStep apart along x, and
+ * joined by the neighbor links that carry the odometry covariance.
+ */
+inline void writeDatabase(
+		const std::string & path, int frames,
+		const DataBuilder & makeData,
+		const NodeDecorator & decorate = NodeDecorator())
+{
+	rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
+	ASSERT_NE(driver, nullptr);
+	ASSERT_TRUE(driver->openConnection(path, /*overwritten=*/true)) << "cannot create " << path;
+
+	for(int id=1; id<=frames; ++id)
+	{
+		const double stamp = stampOfNode(id);
+		rtabmap::Signature * s = new rtabmap::Signature(
+				id, /*mapId=*/0, /*weight=*/1, stamp, /*label=*/"",
+				poseOf(id), rtabmap::Transform(), makeData(id, stamp));
+
+		if(id > 1)
+		{
+			// The backward neighbor link is where DBReader reads the odometry
+			// covariance from: it publishes the inverse of this information matrix.
+			const rtabmap::Transform motion(kPoseStep, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+			s->addLink(rtabmap::Link(id, id-1, rtabmap::Link::kNeighbor, motion.inverse(),
+					cv::Mat::eye(6, 6, CV_64FC1) / kOdomVariance));
+		}
+		if(decorate)
+		{
+			decorate(id, *s);
+		}
+
+		driver->asyncSave(s);          // the driver takes ownership
+		driver->emptyTrashes(false);
+	}
+
+	driver->closeConnection(true);
+	delete driver;
+}
+
+/// A color image whose pixels identify the node, so a test can tell frames apart.
+inline cv::Mat makeRgb(int id)
+{
+	return cv::Mat(kImageHeight, kImageWidth, CV_8UC3, cv::Scalar(id, 2*id, 3*id));
+}
+
+inline cv::Mat makeDepth()
+{
+	return cv::Mat(kImageHeight, kImageWidth, CV_16UC1, cv::Scalar(kDepthMillimeters));
+}
+
+inline rtabmap::CameraModel rgbdCameraModel()
+{
+	return rtabmap::CameraModel(kFx, kFy, kCx, kCy, cameraLocalTransform(), 0.0,
+			cv::Size(kImageWidth, kImageHeight));
+}
+
+inline rtabmap::StereoCameraModel stereoCameraModel()
+{
+	return rtabmap::StereoCameraModel(kFx, kFy, kCx, kCy, kBaseline, cameraLocalTransform(),
+			cv::Size(kImageWidth, kImageHeight));
+}
+
+/// RGB + registered depth from a single camera.
+inline void writeRgbdDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames, [](int id, double stamp) {
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
+				rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
+		return data;
+	});
+}
+
+/// A rectified mono stereo pair.
+inline void writeStereoDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames, [](int id, double stamp) {
+		const cv::Mat left(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(id));
+		const cv::Mat right(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(2*id));
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setStereoImage(rtabmap::compressImage2(left, ".png"),
+				rtabmap::compressImage2(right, ".png"), stereoCameraModel());
+		return data;
+	});
+}
+
+/// A color image with no calibration at all, which db_player replays on "image".
+inline void writeImageOnlyDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames, [](int id, double stamp) {
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"), cv::Mat(),
+				std::vector<rtabmap::CameraModel>());
+		return data;
+	});
+}
+
+//============================================================================
+// Laser scans
+//============================================================================
+
+constexpr int kScanBins = 20;
+constexpr float kScanAngleMin = -1.0f;
+constexpr float kScanAngleMax = 1.0f;
+constexpr float kScanAngleIncrement = 0.1f;   // (max-min)/kScanBins
+constexpr float kScanRangeMin = 0.1f;
+constexpr float kScanRangeMax = 10.0f;
+
+/// The range measured in bin @p bin of the 2D scan.
+inline float scanRangeOf(int bin) { return 1.0f + 0.1f * float(bin); }
+
+/**
+ * @brief A 2D scan with one point at the center of every bin.
+ *
+ * db_player re-bins the cartesian points back into a LaserScan message, so putting each
+ * point at a bin center makes the expected index exact rather than a rounding coin flip.
+ */
+inline rtabmap::LaserScan makeScan2d()
+{
+	cv::Mat points(1, kScanBins, CV_32FC2);
+	for(int bin=0; bin<kScanBins; ++bin)
+	{
+		const float angle = kScanAngleMin + (float(bin) + 0.5f) * kScanAngleIncrement;
+		const float range = scanRangeOf(bin);
+		points.at<cv::Vec2f>(0, bin) = cv::Vec2f(range * std::cos(angle), range * std::sin(angle));
+	}
+	return rtabmap::LaserScan(rtabmap::compressData2(points), rtabmap::LaserScan::kXY,
+			kScanRangeMin, kScanRangeMax, kScanAngleMin, kScanAngleMax, kScanAngleIncrement,
+			scanLocalTransform());
+}
+
+constexpr int kScanCloudPoints = 50;
+
+inline rtabmap::LaserScan makeScan3d()
+{
+	cv::Mat points(1, kScanCloudPoints, CV_32FC3);
+	for(int i=0; i<kScanCloudPoints; ++i)
+	{
+		points.at<cv::Vec3f>(0, i) = cv::Vec3f(1.0f + 0.01f*float(i), 0.02f*float(i), 0.5f);
+	}
+	return rtabmap::LaserScan(rtabmap::compressData2(points), /*maxPoints=*/0, /*maxRange=*/0.0f,
+			rtabmap::LaserScan::kXYZ, scanLocalTransform());
+}
+
+/// A 2D lidar only, no camera.
+inline void writeScan2dDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames, [](int id, double stamp) {
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setLaserScan(makeScan2d());
+		return data;
+	});
+}
+
+/// A 3D lidar only, no camera.
+inline void writeScan3dDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames, [](int id, double stamp) {
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setLaserScan(makeScan3d());
+		return data;
+	});
+}
+
+//============================================================================
+// Everything else db_player can replay
+//============================================================================
+
+/// The gravity orientation stored as a link, which is how DBReader rebuilds an IMU.
+inline rtabmap::Transform gravityTransform()
+{
+	return rtabmap::Transform(0.0f, 0.0f, 0.0f, 0.1f, 0.2f, 0.0f);
+}
+
+/**
+ * @brief RGB-D plus the optional channels: ground truth, prior pose, GPS, gravity and an
+ *        environmental sensor.
+ *
+ * @note The prior's information matrix must not leave a huge rotational variance, or
+ *       DBReader drops the global pose on the assumption GPS already provided the prior.
+ */
+inline void writeRichDatabase(const std::string & path, int frames = kDbFrames)
+{
+	writeDatabase(path, frames,
+		[](int id, double stamp) {
+			rtabmap::SensorData data;
+			data.setId(id);
+			data.setStamp(stamp);
+			data.setRGBDImage(rtabmap::compressImage2(makeRgb(id), ".png"),
+					rtabmap::compressImage2(makeDepth(), ".png"), rgbdCameraModel());
+			data.setGPS(rtabmap::GPS(stamp, kGpsLongitude, kGpsLatitude, kGpsAltitude,
+					kGpsError, /*bearing=*/0.0));
+			rtabmap::EnvSensors sensors;
+			sensors.insert(std::make_pair(rtabmap::EnvSensor::kAmbientTemperature,
+					rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature,
+							kEnvSensorValue, stamp)));
+			data.setEnvSensors(sensors);
+			return data;
+		},
+		[](int id, rtabmap::Signature & s) {
+			s.setGroundTruthPose(groundTruthOf(id));
+			s.addLink(rtabmap::Link(id, id, rtabmap::Link::kPosePrior, globalPoseOf(id),
+					cv::Mat::eye(6, 6, CV_64FC1) * 100.0));
+			s.addLink(rtabmap::Link(id, id, rtabmap::Link::kGravity, gravityTransform(),
+					cv::Mat::eye(6, 6, CV_64FC1)));
+		});
+}
+
+}  // namespace rtabmap_util_test
+
+#endif /* RTABMAP_UTIL_DB_BUILDERS_HPP_ */
diff --git a/rtabmap_util/test/msg_builders.hpp b/rtabmap_util/test/msg_builders.hpp
new file mode 100644
index 00000000..5a23bab7
--- /dev/null
+++ b/rtabmap_util/test/msg_builders.hpp
@@ -0,0 +1,217 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_UTIL_MSG_BUILDERS_HPP_
+#define RTABMAP_UTIL_MSG_BUILDERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <rtabmap_msgs/msg/rgbd_image.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 <functional>
+#include <string>
+#include <vector>
+
+namespace rtabmap_util_test {
+
+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, int height,
+		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;
+}
+
+/// An RGB-D message with raw bgr8 color and 16UC1 depth.
+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 = makeImage(frameId, stamp, cv::Mat(height, width, CV_8UC3, rgbColor), "bgr8");
+	msg.depth = makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_16UC1, cv::Scalar(depthValue)), "16UC1");
+	msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	msg.depth_camera_info = makeCameraInfo(frameId, stamp, width, height);
+	return msg;
+}
+
+/**
+ * @brief An RGB-D message carrying a stereo pair instead of depth.
+ *
+ * The "depth" slot holds the mono8 right image and the second camera info carries the
+ * baseline in P(0,3), which is what makes consumers treat the pair as stereo rather
+ * than as color plus depth.
+ */
+inline rtabmap_msgs::msg::RGBDImage makeStereoRGBDImage(
+		const std::string & frameId, double stamp, int width = 8, int height = 8,
+		double baseline = 0.12, double fx = 100.0)
+{
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = stampOf(stamp);
+	msg.rgb = makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_8UC3, cv::Scalar(10, 20, 30)), "bgr8");
+	msg.depth = makeImage(frameId, stamp,
+			cv::Mat(height, width, CV_8UC1, cv::Scalar(60)), "mono8");   // right image
+	msg.rgb_camera_info = makeCameraInfo(frameId, stamp, width, height, 0.0, fx);
+	msg.depth_camera_info =
+			makeCameraInfo(frameId, stamp, width, height, -fx*baseline, fx);
+	return msg;
+}
+
+/**
+ * @brief A dense unorganized XYZ float cloud.
+ *
+ * @note This writes the points exactly as given: it does not model sensor motion. To
+ *       build a cloud that deskewing can actually correct, use makeSkewedWallScan(),
+ *       which derives the distortion from the same trajectory the TF describes.
+ *
+ * @param withTimeChannel add a FLOAT32 "t" channel of per-point offsets, as a spinning
+ *                        lidar publishes, so the cloud can be deskewed.
+ */
+inline sensor_msgs::msg::PointCloud2 makeXYZCloud(
+		const std::string & frameId, double stamp,
+		const std::vector<cv::Point3f> & points,
+		bool withTimeChannel = false, double sweepDuration = 0.099)
+{
+	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;
+
+	const int fieldCount = withTimeChannel ? 4 : 3;
+	cloud.fields.resize(fieldCount);
+	const char * names[4] = {"x", "y", "z", "t"};
+	for(int i=0; i<fieldCount; ++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 = 4 * fieldCount;
+	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;
+		if(withTimeChannel)
+		{
+			p[3] = points.size() > 1
+					? float(sweepDuration * double(i) / double(points.size() - 1))
+					: 0.0f;
+		}
+	}
+	return cloud;
+}
+
+/**
+ * @brief The raw scan of a flat wall captured while the sensor moves straight at it.
+ *
+ * Sample @p i is taken at `i * step` seconds into the sweep, by which time the sensor
+ * has closed in by `displacement(elapsed)`. Expressed in the sensor frame at capture
+ * time the wall therefore appears to slide closer: a straight wall is recorded bent.
+ * Deskewing with the same motion must flatten it back to @p wallDistance.
+ *
+ * @param frameId       sensor frame
+ * @param stamp         stamp of the first sample, which is also the message stamp
+ * @param sampleCount   number of samples along the wall
+ * @param sweepDuration seconds from the first sample to the last
+ * @param wallDistance  distance to the wall at the first sample, in meters
+ * @param displacement  distance travelled as a function of seconds since the first
+ *                      sample; must match the motion published to TF
+ */
+inline sensor_msgs::msg::PointCloud2 makeSkewedWallScan(
+		const std::string & frameId, double stamp,
+		size_t sampleCount, double sweepDuration, float wallDistance,
+		const std::function<double(double)> & displacement)
+{
+	std::vector<cv::Point3f> points;
+	points.reserve(sampleCount);
+	for(size_t i=0; i<sampleCount; ++i)
+	{
+		const double elapsed =
+				sampleCount > 1 ? sweepDuration * double(i) / double(sampleCount - 1) : 0.0;
+		points.push_back(cv::Point3f(
+				wallDistance - float(displacement(elapsed)),                  // the skew
+				-1.0f + 2.0f * float(i) / float(sampleCount > 1 ? sampleCount - 1 : 1),
+				0.0f));
+	}
+	return makeXYZCloud(frameId, stamp, points, /*withTimeChannel=*/true, sweepDuration);
+}
+
+/**
+ * @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, so it also
+ * works on clouds produced by laser_geometry, which lay their fields out differently.
+ */
+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_util_test
+
+#endif /* RTABMAP_UTIL_MSG_BUILDERS_HPP_ */
diff --git a/rtabmap_util/test/node_test_utils.hpp b/rtabmap_util/test/node_test_utils.hpp
new file mode 100644
index 00000000..1031c1c1
--- /dev/null
+++ b/rtabmap_util/test/node_test_utils.hpp
@@ -0,0 +1,320 @@
+/*
+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_UTIL_NODE_TEST_UTILS_HPP_
+#define RTABMAP_UTIL_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace rtabmap_util_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_util_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();
+	}
+
+	/**
+	 * @brief Runs every node of the fixture on a multi-threaded executor until @p done.
+	 *
+	 * A node whose callback waits on another of its own callbacks -- a service call made
+	 * from a timer, say -- makes no progress under spinUntil(), because the second
+	 * callback cannot run while the first is still on the stack. Such nodes put the two
+	 * callbacks in different callback groups precisely so a multi-threaded executor can
+	 * overlap them; this hands them the threads to do it, then puts the nodes back on the
+	 * usual single-threaded executor.
+	 *
+	 * @warning Callbacks run on executor threads for the duration, so do not have any
+	 *          Collector subscribed while this runs: the test thread would read its
+	 *          messages while another thread appends to them. Use it to get a node
+	 *          through its start-up handshake, before subscribing to anything.
+	 */
+	bool spinMultiThreadedUntil(
+			const std::function<bool()> & done,
+			std::chrono::milliseconds timeout = std::chrono::milliseconds(15000))
+	{
+		rclcpp::executors::MultiThreadedExecutor booting(rclcpp::ExecutorOptions(), 4);
+		for(const rclcpp::Node::SharedPtr & node : nodes_)
+		{
+			executor_->remove_node(node);
+			booting.add_node(node);
+		}
+		executor_->remove_node(helper_);
+		booting.add_node(helper_);
+
+		std::thread spinner([&booting]() { booting.spin(); });
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + timeout;
+		while(rclcpp::ok() && !done() && std::chrono::steady_clock::now() < deadline)
+		{
+			std::this_thread::sleep_for(std::chrono::milliseconds(5));
+		}
+		const bool result = done();
+		booting.cancel();
+		spinner.join();
+
+		booting.remove_node(helper_);
+		executor_->add_node(helper_);
+		for(const rclcpp::Node::SharedPtr & node : nodes_)
+		{
+			booting.remove_node(node);
+			executor_->add_node(node);
+		}
+		return result;
+	}
+
+	/// 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.
+	 *
+	 * Several nodes only publish when they have 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; });
+	}
+
+	/**
+	 * @brief Publishes a static transform on /tf_static.
+	 *
+	 * /tf_static is transient-local, so a listener that subscribes later still receives
+	 * it. That makes static frames far less timing-sensitive in tests than /tf.
+	 */
+	void publishStaticTf(
+			const std::string & parent, const std::string & child,
+			double x = 0.0, double y = 0.0, double z = 0.0)
+	{
+		if(!staticTfPublisher_)
+		{
+			staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
+					"/tf_static", rclcpp::QoS(100).transient_local());
+		}
+		geometry_msgs::msg::TransformStamped t;
+		t.header.stamp = helper_->now();
+		t.header.frame_id = parent;
+		t.child_frame_id = child;
+		t.transform.translation.x = x;
+		t.transform.translation.y = y;
+		t.transform.translation.z = z;
+		t.transform.rotation.w = 1.0;
+		tf2_msgs::msg::TFMessage msg;
+		msg.transforms.push_back(t);
+		staticTfPublisher_->publish(msg);
+		spinFor(std::chrono::milliseconds(100));
+	}
+
+	/// Publishes a static transform with a rotation, given as roll/pitch/yaw.
+	void publishStaticTfRPY(
+			const std::string & parent, const std::string & child,
+			double roll, double pitch, double yaw,
+			double x = 0.0, double y = 0.0, double z = 0.0)
+	{
+		if(!staticTfPublisher_)
+		{
+			staticTfPublisher_ = helper_->create_publisher<tf2_msgs::msg::TFMessage>(
+					"/tf_static", rclcpp::QoS(100).transient_local());
+		}
+		tf2::Quaternion q;
+		q.setRPY(roll, pitch, yaw);
+		geometry_msgs::msg::TransformStamped t;
+		t.header.stamp = helper_->now();
+		t.header.frame_id = parent;
+		t.child_frame_id = child;
+		t.transform.translation.x = x;
+		t.transform.translation.y = y;
+		t.transform.translation.z = z;
+		t.transform.rotation = tf2::toMsg(q);
+		tf2_msgs::msg::TFMessage msg;
+		msg.transforms.push_back(t);
+		staticTfPublisher_->publish(msg);
+		spinFor(std::chrono::milliseconds(100));
+	}
+
+	/// 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_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfPublisher_;
+};
+
+}  // namespace rtabmap_util_test
+
+#endif /* RTABMAP_UTIL_NODE_TEST_UTILS_HPP_ */
diff --git a/rtabmap_util/test/test_db_player.cpp b/rtabmap_util/test/test_db_player.cpp
new file mode 100644
index 00000000..f045de04
--- /dev/null
+++ b/rtabmap_util/test/test_db_player.cpp
@@ -0,0 +1,809 @@
+/*
+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 "db_builders.hpp"
+
+#include <rtabmap_util/db_player.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rosgraph_msgs/msg/clock.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include <cmath>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+/// Replay at 1000x the recorded stamps: db_player sleeps between frames otherwise.
+constexpr double kReplayRate = 1000.0;
+
+bool hasParameter(const std::vector<rclcpp::Parameter> & overrides, const std::string & name)
+{
+	for(size_t i=0; i<overrides.size(); ++i)
+	{
+		if(overrides[i].get_name() == name) { return true; }
+	}
+	return false;
+}
+}  // namespace
+
+/**
+ * db_player is driven by its own loop in DbPlayerNode, so the tests call
+ * publishNextFrame() directly instead of waiting on a timer.
+ *
+ * Two things shape every test below. The publishers do not exist until the first frame
+ * has been read -- db_player decides which topics to create from the payloads it finds in
+ * the database -- and everything except /tf is only published when someone is subscribed.
+ * So the sequence is always: replay one frame to create the publishers, subscribe, then
+ * replay again to get the data.
+ */
+class DbPlayerTest : public NodeTest
+{
+protected:
+	void start(const std::string & databasePath, std::vector<rclcpp::Parameter> overrides = {})
+	{
+		if(!hasParameter(overrides, "database"))
+		{
+			overrides.push_back(rclcpp::Parameter("database", databasePath));
+		}
+		if(!hasParameter(overrides, "rate"))
+		{
+			overrides.push_back(rclcpp::Parameter("rate", kReplayRate));
+		}
+		player_ = addNode(std::make_shared<rtabmap_util::DbPlayer>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+	}
+
+	/// Reads one frame, which is what creates the publishers.
+	void primePublishers()
+	{
+		ASSERT_TRUE(player_->publishNextFrame()) << "the database has no readable frame";
+	}
+
+	/// Replays frames until @p done, or the database runs out.
+	bool replayUntil(const std::function<bool()> & done)
+	{
+		for(int i=0; i<kDbFrames && !done(); ++i)
+		{
+			if(!player_->publishNextFrame()) { break; }
+			spinFor(std::chrono::milliseconds(30));
+		}
+		return done();
+	}
+
+	/**
+	 * @brief The database node a replayed message came from, recovered from its stamp.
+	 *
+	 * How many frames a test ends up replaying depends on discovery, so the expected
+	 * pose is derived from the stamp the message itself carries rather than assumed.
+	 * That also checks the stamp really comes from the database.
+	 */
+	static int nodeIdOf(const builtin_interfaces::msg::Time & stamp)
+	{
+		const double seconds = rtabmap_conversions::timestampFromROS(stamp);
+		return int(std::round((seconds - kFirstStamp) / kStampStep)) + 1;
+	}
+
+	/// The most recent transform published for @p parent -> @p child.
+	static bool findTransform(
+			const Collector<tf2_msgs::msg::TFMessage> & tf,
+			const std::string & parent, const std::string & child,
+			geometry_msgs::msg::TransformStamped & out)
+	{
+		bool found = false;
+		for(size_t i=0; i<tf.messages.size(); ++i)
+		{
+			for(size_t j=0; j<tf.messages[i]->transforms.size(); ++j)
+			{
+				const geometry_msgs::msg::TransformStamped & t = tf.messages[i]->transforms[j];
+				if(t.header.frame_id == parent && t.child_frame_id == child)
+				{
+					out = t;
+					found = true;
+				}
+			}
+		}
+		return found;
+	}
+
+	static rtabmap::Transform toRtabmap(const geometry_msgs::msg::TransformStamped & t)
+	{
+		return rtabmap_conversions::transformFromGeometryMsg(t.transform);
+	}
+
+	/// Subscribes to /tf and waits for db_player's broadcaster to be discovered.
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> collectTf()
+	{
+		std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+				collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+		EXPECT_TRUE(waitForPublisher(tf->subscription));
+		return tf;
+	}
+
+	std::shared_ptr<rtabmap_util::DbPlayer> player_;
+};
+
+//============================================================================
+// RGB-D
+//============================================================================
+
+TEST_F(DbPlayerTest, ReplaysRgbAndDepthImages)
+{
+	TempDatabase db("rgbd");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth/image");
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	ASSERT_TRUE(replayUntil([&]() { return !rgb->empty() && !depth->empty(); }))
+		<< "no image replayed";
+
+	EXPECT_EQ(rgb->back().encoding, sensor_msgs::image_encodings::BGR8);
+	EXPECT_EQ(rgb->back().width, uint32_t(kImageWidth));
+	EXPECT_EQ(rgb->back().height, uint32_t(kImageHeight));
+	EXPECT_EQ(rgb->back().header.frame_id, "camera_optical_link");
+
+	EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(depth->back().header.frame_id, "camera_optical_link")
+		<< "depth is registered with the color camera, so it shares its frame";
+	EXPECT_EQ(*reinterpret_cast<const uint16_t *>(depth->back().data.data()), kDepthMillimeters);
+}
+
+TEST_F(DbPlayerTest, StampsImagesWithTheDatabaseStamps)
+{
+	TempDatabase db("stamps");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgb/image");
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }));
+
+	const int id = nodeIdOf(rgb->front().header.stamp);
+	EXPECT_GE(id, 2) << "the first frame only creates the publishers";
+	EXPECT_LE(id, kDbFrames);
+	EXPECT_NEAR(rtabmap_conversions::timestampFromROS(rgb->front().header.stamp),
+			stampOfNode(id), 1e-6);
+}
+
+TEST_F(DbPlayerTest, ReplaysCameraCalibration)
+{
+	TempDatabase db("caminfo");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgb/camera_info");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
+			collect<sensor_msgs::msg::CameraInfo>("depth/camera_info");
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	ASSERT_TRUE(waitForPublisher(rgbInfo->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !rgbInfo->empty() && !depthInfo->empty(); }));
+
+	EXPECT_EQ(rgbInfo->back().width, uint32_t(kImageWidth));
+	EXPECT_EQ(rgbInfo->back().height, uint32_t(kImageHeight));
+	EXPECT_NEAR(rgbInfo->back().k[0], kFx, 1e-6);
+	EXPECT_NEAR(rgbInfo->back().k[2], kCx, 1e-6);
+	EXPECT_NEAR(rgbInfo->back().k[4], kFy, 1e-6);
+	EXPECT_NEAR(rgbInfo->back().k[5], kCy, 1e-6);
+	EXPECT_EQ(rgbInfo->back().header.frame_id, "camera_optical_link");
+
+	EXPECT_NEAR(depthInfo->back().k[0], kFx, 1e-6)
+		<< "the depth camera info repeats the color calibration";
+}
+
+TEST_F(DbPlayerTest, ReplaysImageWithoutCalibrationOnImageTopic)
+{
+	// A database with no calibration at all is still replayable, on "image".
+	TempDatabase db("imageonly");
+	writeImageOnlyDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> image =
+			collect<sensor_msgs::msg::Image>("image");
+	ASSERT_TRUE(waitForPublisher(image->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !image->empty(); }));
+
+	EXPECT_EQ(image->back().encoding, sensor_msgs::image_encodings::BGR8);
+	EXPECT_EQ(image->back().width, uint32_t(kImageWidth));
+}
+
+//============================================================================
+// Stereo
+//============================================================================
+
+TEST_F(DbPlayerTest, ReplaysStereoPairAndCalibration)
+{
+	TempDatabase db("stereo");
+	writeStereoDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
+			collect<sensor_msgs::msg::Image>("left/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
+			collect<sensor_msgs::msg::Image>("right/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
+			collect<sensor_msgs::msg::CameraInfo>("left/camera_info");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
+			collect<sensor_msgs::msg::CameraInfo>("right/camera_info");
+	ASSERT_TRUE(waitForPublisher(left->subscription));
+	ASSERT_TRUE(waitForPublisher(right->subscription));
+	ASSERT_TRUE(replayUntil([&]() {
+		return !left->empty() && !right->empty() &&
+			   !leftInfo->empty() && !rightInfo->empty(); }));
+
+	EXPECT_EQ(left->back().encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(left->back().header.frame_id, "left_camera_optical_link");
+	EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(right->back().header.frame_id, "right_camera_optical_link");
+
+	// Both cameras share the intrinsics of a rectified pair and are stamped with the
+	// frame of the image they belong to.
+	EXPECT_EQ(leftInfo->back().width, uint32_t(kImageWidth));
+	EXPECT_EQ(leftInfo->back().height, uint32_t(kImageHeight));
+	EXPECT_NEAR(leftInfo->back().k[0], kFx, 1e-6);
+	EXPECT_NEAR(leftInfo->back().k[2], kCx, 1e-6);
+	EXPECT_NEAR(leftInfo->back().k[4], kFy, 1e-6);
+	EXPECT_NEAR(leftInfo->back().k[5], kCy, 1e-6);
+	EXPECT_EQ(leftInfo->back().header.frame_id, "left_camera_optical_link");
+	EXPECT_NEAR(rightInfo->back().k[0], kFx, 1e-6);
+	EXPECT_EQ(rightInfo->back().header.frame_id, "right_camera_optical_link");
+
+	// Only the right camera carries the baseline: it is P(0,3) = -fx*baseline, and the
+	// left camera of a rectified pair sits at the origin of the stereo frame.
+	EXPECT_NEAR(leftInfo->back().p[3], 0.0, 1e-6);
+	EXPECT_NEAR(rightInfo->back().p[3], -kFx*kBaseline, 1e-6);
+}
+
+//============================================================================
+// Laser scans
+//============================================================================
+
+TEST_F(DbPlayerTest, Replays2dLaserScan)
+{
+	TempDatabase db("scan2d");
+	writeScan2dDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
+			collect<sensor_msgs::msg::LaserScan>("scan");
+	ASSERT_TRUE(waitForPublisher(scan->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); })) << "no scan replayed";
+
+	const sensor_msgs::msg::LaserScan & msg = scan->back();
+	EXPECT_EQ(msg.header.frame_id, "base_laser_link");
+
+	// The scan carries its own angles, so the scan_angle_* parameters are not used.
+	EXPECT_NEAR(msg.angle_min, kScanAngleMin, 1e-6);
+	EXPECT_NEAR(msg.angle_max, kScanAngleMax, 1e-6);
+	EXPECT_NEAR(msg.angle_increment, kScanAngleIncrement, 1e-6);
+	EXPECT_NEAR(msg.range_min, kScanRangeMin, 1e-6);
+	EXPECT_NEAR(msg.range_max, kScanRangeMax, 1e-6);
+
+	// db_player re-bins the cartesian points, so every bin must come back at its range.
+	ASSERT_EQ(msg.ranges.size(), size_t(kScanBins));
+	for(int bin=0; bin<kScanBins; ++bin)
+	{
+		EXPECT_NEAR(msg.ranges[bin], scanRangeOf(bin), 1e-3) << "bin " << bin;
+	}
+}
+
+TEST_F(DbPlayerTest, A2dScanNeverAdvertisesScanCloud)
+{
+	// initializePublishers() runs on every frame, so the publisher it creates has to
+	// match the scan being replayed. It used to create whichever one did not exist yet,
+	// which advertised an empty "scan_cloud" from the second frame of a 2D database on.
+	TempDatabase db("scan2donly");
+	writeScan2dDatabase(db.path());
+	start(db.path());
+
+	std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
+			collect<sensor_msgs::msg::LaserScan>("scan");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
+
+	while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
+
+	EXPECT_FALSE(scan->empty()) << "the 2D scan must still be replayed";
+	EXPECT_EQ(cloud->subscription->get_publisher_count(), 0u)
+		<< "a 2D database must not advertise scan_cloud";
+	EXPECT_TRUE(cloud->empty());
+}
+
+TEST_F(DbPlayerTest, A3dScanNeverAdvertisesScan)
+{
+	TempDatabase db("scan3donly");
+	writeScan3dDatabase(db.path());
+	start(db.path());
+
+	std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
+			collect<sensor_msgs::msg::LaserScan>("scan");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
+
+	while(player_->publishNextFrame()) { spinFor(std::chrono::milliseconds(30)); }
+
+	EXPECT_FALSE(cloud->empty()) << "the 3D scan must still be replayed";
+	EXPECT_EQ(scan->subscription->get_publisher_count(), 0u)
+		<< "a 3D database must not advertise scan";
+}
+
+TEST_F(DbPlayerTest, UsesScanParametersWhenTheScanHasNoAngles)
+{
+	// A scan saved without angle metadata falls back to the scan_angle_*/scan_range_*
+	// parameters, which is how a database recorded from a 3D lidar can be replayed as 2D.
+	const double angleMin = -0.5;
+	const double angleIncrement = 0.05;
+	const int targetBin = 10;
+	// The center of the target bin: db_player truncates (angle-angle_min)/increment, so a
+	// bearing on a bin boundary would land on either side depending on the rounding.
+	const float bearing = float(angleMin + (double(targetBin) + 0.5) * angleIncrement);
+	const float nearest = 1.0f;
+
+	TempDatabase db("scan2dnoangles");
+	writeDatabase(db.path(), kDbFrames, [bearing, nearest](int id, double stamp) {
+		cv::Mat points(1, kScanBins, CV_32FC2);
+		for(int bin=0; bin<kScanBins; ++bin)
+		{
+			// All at the same bearing, at increasing ranges: db_player keeps the nearest.
+			const float range = nearest + 0.1f * float(bin);
+			points.at<cv::Vec2f>(0, bin) =
+					cv::Vec2f(range * std::cos(bearing), range * std::sin(bearing));
+		}
+		rtabmap::SensorData data;
+		data.setId(id);
+		data.setStamp(stamp);
+		data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(points),
+				/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXY,
+				scanLocalTransform()));
+		return data;
+	});
+	start(db.path(), {rclcpp::Parameter("scan_angle_min", angleMin),
+					  rclcpp::Parameter("scan_angle_max", 0.5),
+					  rclcpp::Parameter("scan_angle_increment", angleIncrement),
+					  rclcpp::Parameter("scan_range_min", 0.2),
+					  rclcpp::Parameter("scan_range_max", 20.0)});
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::LaserScan>> scan =
+			collect<sensor_msgs::msg::LaserScan>("scan");
+	ASSERT_TRUE(waitForPublisher(scan->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !scan->empty(); }));
+
+	const sensor_msgs::msg::LaserScan & msg = scan->back();
+	EXPECT_NEAR(msg.angle_min, angleMin, 1e-6);
+	EXPECT_NEAR(msg.angle_max, 0.5, 1e-6);
+	EXPECT_NEAR(msg.angle_increment, angleIncrement, 1e-6);
+	EXPECT_NEAR(msg.range_min, 0.2, 1e-6);
+	EXPECT_NEAR(msg.range_max, 20.0, 1e-6);
+	ASSERT_EQ(msg.ranges.size(), 20u) << "ceil((0.5 - -0.5)/0.05)";
+
+	EXPECT_NEAR(msg.ranges[targetBin], nearest, 1e-3)
+		<< "every point shares a bearing, so only its bin is filled, at the nearest range";
+	for(size_t bin=0; bin<msg.ranges.size(); ++bin)
+	{
+		if(int(bin) != targetBin)
+		{
+			EXPECT_FLOAT_EQ(msg.ranges[bin], 0.0f) << "bin " << bin << " should be empty";
+		}
+	}
+}
+
+TEST_F(DbPlayerTest, Replays3dScanAsPointCloud)
+{
+	TempDatabase db("scan3d");
+	writeScan3dDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("scan_cloud");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !cloud->empty(); })) << "no cloud replayed";
+
+	EXPECT_EQ(cloud->back().header.frame_id, "base_laser_link");
+	EXPECT_EQ(cloud->back().width * cloud->back().height, uint32_t(kScanCloudPoints));
+}
+
+//============================================================================
+// Odometry
+//============================================================================
+
+TEST_F(DbPlayerTest, ReplaysOdometryWithItsCovariance)
+{
+	TempDatabase db("odom");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	ASSERT_TRUE(waitForPublisher(odom->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !odom->empty(); })) << "no odometry replayed";
+
+	const nav_msgs::msg::Odometry & msg = odom->back();
+	EXPECT_EQ(msg.header.frame_id, "odom");
+	EXPECT_EQ(msg.child_frame_id, "base_link");
+
+	const int id = nodeIdOf(msg.header.stamp);
+	ASSERT_GE(id, 1);
+	ASSERT_LE(id, kDbFrames);
+	EXPECT_NEAR(msg.pose.pose.position.x, poseOf(id).x(), 1e-5)
+		<< "the pose must be the one recorded for node " << id;
+	EXPECT_NEAR(msg.pose.pose.position.y, 0.0, 1e-5);
+
+	// The covariance is the inverse of the neighbor link's information matrix.
+	EXPECT_NEAR(msg.pose.covariance[0], kOdomVariance, 1e-6);
+	EXPECT_NEAR(msg.pose.covariance[35], kOdomVariance, 1e-6);
+}
+
+TEST_F(DbPlayerTest, IgnoreOdomDropsTheOdometry)
+{
+	TempDatabase db("ignoreodom");
+	writeRgbdDatabase(db.path());
+	start(db.path(), {rclcpp::Parameter("ignore_odom", true)});
+	primePublishers();
+
+	std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+			collect<nav_msgs::msg::Odometry>("odom");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgb/image");
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !rgb->empty(); }))
+		<< "the images must still be replayed";
+
+	EXPECT_EQ(odom->subscription->get_publisher_count(), 0u)
+		<< "with no odometry in the stream the topic is never even created";
+	EXPECT_TRUE(odom->empty());
+}
+
+//============================================================================
+// Transforms
+//============================================================================
+
+TEST_F(DbPlayerTest, BroadcastsOdometryAndCameraTransforms)
+{
+	TempDatabase db("tf");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+
+	// TF is not gated on subscribers, so the very first frame already broadcasts.
+	ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); })) << "nothing broadcast on /tf";
+
+	geometry_msgs::msg::TransformStamped odomToBase;
+	ASSERT_TRUE(findTransform(*tf, "odom", "base_link", odomToBase));
+	const int id = nodeIdOf(odomToBase.header.stamp);
+	ASSERT_GE(id, 1);
+	ASSERT_LE(id, kDbFrames);
+	EXPECT_NEAR(odomToBase.transform.translation.x, poseOf(id).x(), 1e-5);
+
+	geometry_msgs::msg::TransformStamped baseToCamera;
+	ASSERT_TRUE(findTransform(*tf, "base_link", "camera_optical_link", baseToCamera));
+	EXPECT_LT(toRtabmap(baseToCamera).getDistance(cameraLocalTransform()), 1e-4f)
+		<< "the camera transform is the model's local transform: "
+		<< toRtabmap(baseToCamera).prettyPrint();
+}
+
+TEST_F(DbPlayerTest, BroadcastsStereoTransformsShiftedByTheBaseline)
+{
+	TempDatabase db("stereotf");
+	writeStereoDatabase(db.path());
+	start(db.path());
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+	ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
+
+	geometry_msgs::msg::TransformStamped baseToLeft, baseToRight;
+	ASSERT_TRUE(findTransform(*tf, "base_link", "left_camera_optical_link", baseToLeft));
+	ASSERT_TRUE(findTransform(*tf, "base_link", "right_camera_optical_link", baseToRight));
+
+	EXPECT_LT(toRtabmap(baseToLeft).getDistance(cameraLocalTransform()), 1e-4f);
+
+	// The right camera carries the baseline in Tx, which db_player turns back into a
+	// translation along the optical x axis so the frame sits next to the left one.
+	const rtabmap::Transform expectedRight =
+			cameraLocalTransform() * rtabmap::Transform(kBaseline, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	EXPECT_LT(toRtabmap(baseToRight).getDistance(expectedRight), 1e-4f)
+		<< toRtabmap(baseToRight).prettyPrint();
+}
+
+TEST_F(DbPlayerTest, BroadcastsTheLaserTransform)
+{
+	TempDatabase db("scantf");
+	writeScan3dDatabase(db.path());
+	start(db.path());
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+	ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
+
+	geometry_msgs::msg::TransformStamped baseToLaser;
+	ASSERT_TRUE(findTransform(*tf, "base_link", "base_laser_link", baseToLaser));
+	EXPECT_LT(toRtabmap(baseToLaser).getDistance(scanLocalTransform()), 1e-4f)
+		<< toRtabmap(baseToLaser).prettyPrint();
+}
+
+TEST_F(DbPlayerTest, BroadcastsGroundTruthAndImuTransforms)
+{
+	TempDatabase db("richtf");
+	writeRichDatabase(db.path());
+	start(db.path());
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+	ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
+
+	geometry_msgs::msg::TransformStamped worldToGt;
+	ASSERT_TRUE(findTransform(*tf, "world", "base_link_gt", worldToGt));
+	const int id = nodeIdOf(worldToGt.header.stamp);
+	ASSERT_GE(id, 1);
+	ASSERT_LE(id, kDbFrames);
+	EXPECT_LT(toRtabmap(worldToGt).getDistance(groundTruthOf(id)), 1e-4f)
+		<< "the ground truth is published apart from the odometry";
+
+	geometry_msgs::msg::TransformStamped baseToImu;
+	ASSERT_TRUE(findTransform(*tf, "base_link", "imu_link", baseToImu));
+	EXPECT_TRUE(toRtabmap(baseToImu).isIdentity())
+		<< "a gravity link is already expressed in the base frame";
+}
+
+TEST_F(DbPlayerTest, RenamesFramesFromParameters)
+{
+	TempDatabase db("frames");
+	writeRgbdDatabase(db.path());
+	start(db.path(), {rclcpp::Parameter("frame_id", std::string("robot")),
+					  rclcpp::Parameter("odom_frame_id", std::string("world_odom")),
+					  rclcpp::Parameter("camera_frame_id", std::string("optical"))});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+	ASSERT_TRUE(replayUntil([&]() { return !tf->empty(); }));
+
+	geometry_msgs::msg::TransformStamped t;
+	EXPECT_TRUE(findTransform(*tf, "world_odom", "robot", t));
+	EXPECT_TRUE(findTransform(*tf, "robot", "optical", t));
+	EXPECT_FALSE(findTransform(*tf, "odom", "base_link", t)) << "the defaults must be gone";
+}
+
+TEST_F(DbPlayerTest, PublishTfFalseBroadcastsNothing)
+{
+	TempDatabase db("notf");
+	writeRgbdDatabase(db.path());
+	start(db.path(), {rclcpp::Parameter("publish_tf", false)});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+
+	ASSERT_TRUE(player_->publishNextFrame());
+	ASSERT_TRUE(player_->publishNextFrame());
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_TRUE(tf->empty()) << "publish_tf:=false must not create the broadcaster";
+}
+
+//============================================================================
+// The optional channels
+//============================================================================
+
+TEST_F(DbPlayerTest, ReplaysGlobalPose)
+{
+	TempDatabase db("globalpose");
+	writeRichDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<geometry_msgs::msg::PoseWithCovarianceStamped>> pose =
+			collect<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose");
+	ASSERT_TRUE(waitForPublisher(pose->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !pose->empty(); })) << "no global pose replayed";
+
+	const int id = nodeIdOf(pose->back().header.stamp);
+	ASSERT_GE(id, 1);
+	ASSERT_LE(id, kDbFrames);
+	EXPECT_EQ(pose->back().header.frame_id, "base_link");
+	EXPECT_NEAR(pose->back().pose.pose.position.y, globalPoseOf(id).y(), 1e-5)
+		<< "the prior pose is offset in y, unlike the odometry";
+	// The prior was saved with an information matrix of 100*I.
+	EXPECT_NEAR(pose->back().pose.covariance[0], 0.01, 1e-6);
+}
+
+TEST_F(DbPlayerTest, ReplaysGpsFix)
+{
+	TempDatabase db("gps");
+	writeRichDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::NavSatFix>> gps =
+			collect<sensor_msgs::msg::NavSatFix>("gps/fix");
+	ASSERT_TRUE(waitForPublisher(gps->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !gps->empty(); })) << "no GPS replayed";
+
+	const sensor_msgs::msg::NavSatFix & msg = gps->back();
+	EXPECT_NEAR(msg.longitude, kGpsLongitude, 1e-9);
+	EXPECT_NEAR(msg.latitude, kGpsLatitude, 1e-9);
+	EXPECT_NEAR(msg.altitude, kGpsAltitude, 1e-9);
+	EXPECT_EQ(msg.position_covariance_type,
+			uint8_t(sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN));
+	EXPECT_NEAR(msg.position_covariance[0], kGpsError*kGpsError, 1e-9)
+		<< "the reported error is squared into a variance";
+	EXPECT_NEAR(msg.position_covariance[4], kGpsError*kGpsError, 1e-9);
+	EXPECT_NEAR(msg.position_covariance[8], kGpsError*kGpsError, 1e-9);
+}
+
+TEST_F(DbPlayerTest, ReplaysImu)
+{
+	TempDatabase db("imu");
+	writeRichDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Imu>> imu =
+			collect<sensor_msgs::msg::Imu>("imu");
+	ASSERT_TRUE(waitForPublisher(imu->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !imu->empty(); })) << "no IMU replayed";
+
+	EXPECT_EQ(imu->back().header.frame_id, "imu_link");
+
+	// DBReader rebuilds the IMU from the gravity link, so only the orientation survives.
+	const Eigen::Quaterniond expected = gravityTransform().getQuaterniond();
+	EXPECT_NEAR(std::abs(imu->back().orientation.w), std::abs(expected.w()), 1e-5);
+	EXPECT_NEAR(std::abs(imu->back().orientation.x), std::abs(expected.x()), 1e-5);
+	EXPECT_NEAR(std::abs(imu->back().orientation.y), std::abs(expected.y()), 1e-5);
+	EXPECT_NEAR(std::abs(imu->back().orientation.z), std::abs(expected.z()), 1e-5);
+}
+
+TEST_F(DbPlayerTest, ReplaysEnvSensor)
+{
+	TempDatabase db("envsensor");
+	writeRichDatabase(db.path());
+	start(db.path());
+	primePublishers();
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::EnvSensor>> env =
+			collect<rtabmap_msgs::msg::EnvSensor>("env_sensor");
+	ASSERT_TRUE(waitForPublisher(env->subscription));
+	ASSERT_TRUE(replayUntil([&]() { return !env->empty(); })) << "no env sensor replayed";
+
+	EXPECT_EQ(env->back().type, int(rtabmap::EnvSensor::kAmbientTemperature));
+	EXPECT_NEAR(env->back().value, kEnvSensorValue, 1e-9);
+	EXPECT_EQ(env->back().header.frame_id, "base_link");
+}
+
+TEST_F(DbPlayerTest, PublishesClockWhenAsked)
+{
+	TempDatabase db("clock");
+	writeRgbdDatabase(db.path());
+	start(db.path(), {rclcpp::Parameter("publish_clock", true)});
+	std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
+			collect<rosgraph_msgs::msg::Clock>("/clock");
+	ASSERT_TRUE(waitForPublisher(clock->subscription));
+
+	// The clock is not gated on subscribers either.
+	ASSERT_TRUE(replayUntil([&]() { return !clock->empty(); })) << "no clock published";
+
+	const int id = nodeIdOf(clock->back().clock);
+	ASSERT_GE(id, 1);
+	ASSERT_LE(id, kDbFrames);
+	EXPECT_NEAR(rtabmap_conversions::timestampFromROS(clock->back().clock),
+			stampOfNode(id), 1e-6) << "the clock follows the database stamps";
+}
+
+TEST_F(DbPlayerTest, NoClockByDefault)
+{
+	TempDatabase db("noclock");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	std::shared_ptr<Collector<rosgraph_msgs::msg::Clock>> clock =
+			collect<rosgraph_msgs::msg::Clock>("/clock");
+
+	ASSERT_TRUE(player_->publishNextFrame());
+	ASSERT_TRUE(player_->publishNextFrame());
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_TRUE(clock->empty());
+}
+
+//============================================================================
+// Reading the database
+//============================================================================
+
+TEST_F(DbPlayerTest, StopsAtTheEndOfTheDatabase)
+{
+	TempDatabase db("end");
+	writeRgbdDatabase(db.path(), 4);
+	start(db.path());
+
+	int frames = 0;
+	while(player_->publishNextFrame())
+	{
+		++frames;
+		ASSERT_LE(frames, 10) << "publishNextFrame() never reported the end";
+	}
+	EXPECT_EQ(frames, 4) << "every node must be replayed exactly once";
+}
+
+TEST_F(DbPlayerTest, StartIdSkipsTheEarlierNodes)
+{
+	TempDatabase db("startid");
+	writeRgbdDatabase(db.path(), 4);
+	start(db.path(), {rclcpp::Parameter("start_id", 3)});
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf = collectTf();
+
+	int frames = 0;
+	while(player_->publishNextFrame()) { ++frames; }
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_EQ(frames, 2) << "nodes 3 and 4 only";
+
+	geometry_msgs::msg::TransformStamped t;
+	ASSERT_TRUE(findTransform(*tf, "odom", "base_link", t));
+	EXPECT_EQ(nodeIdOf(tf->front().transforms[0].header.stamp), 3)
+		<< "the replay must start at node 3";
+}
+
+//============================================================================
+// Pause / resume
+//============================================================================
+
+TEST_F(DbPlayerTest, StartsRunning)
+{
+	TempDatabase db("pause");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+	EXPECT_FALSE(player_->isPaused());
+}
+
+TEST_F(DbPlayerTest, PauseAndResumeServicesTogglePlayback)
+{
+	TempDatabase db("pausesrv");
+	writeRgbdDatabase(db.path());
+	start(db.path());
+
+	rclcpp::Client<std_srvs::srv::Empty>::SharedPtr pause =
+			helper()->create_client<std_srvs::srv::Empty>("db_player/pause");
+	rclcpp::Client<std_srvs::srv::Empty>::SharedPtr resume =
+			helper()->create_client<std_srvs::srv::Empty>("db_player/resume");
+	ASSERT_TRUE(spinUntil([&]() { return pause->service_is_ready() && resume->service_is_ready(); }))
+		<< "the pause/resume services were never advertised";
+
+	pause->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
+	ASSERT_TRUE(spinUntil([&]() { return player_->isPaused(); })) << "pause had no effect";
+
+	resume->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
+	ASSERT_TRUE(spinUntil([&]() { return !player_->isPaused(); })) << "resume had no effect";
+}
+
+//============================================================================
+// Opening the database
+//============================================================================
+
+TEST_F(DbPlayerTest, ThrowsWithoutADatabaseParameter)
+{
+	// The node used to exit(-1) here, which took down every other node sharing its
+	// component container. Throwing lets the caller decide.
+	EXPECT_THROW(
+		std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions()),
+		std::invalid_argument);
+}
+
+TEST_F(DbPlayerTest, ThrowsWhenTheDatabaseCannotBeOpened)
+{
+	TempDatabase db("missing");   // the path is never written
+	EXPECT_THROW(
+		std::make_shared<rtabmap_util::DbPlayer>(rclcpp::NodeOptions().parameter_overrides(
+				{rclcpp::Parameter("database", db.path())})),
+		std::runtime_error);
+}
diff --git a/rtabmap_util/test/test_disparity_to_depth.cpp b/rtabmap_util/test/test_disparity_to_depth.cpp
new file mode 100644
index 00000000..ae622e59
--- /dev/null
+++ b/rtabmap_util/test/test_disparity_to_depth.cpp
@@ -0,0 +1,248 @@
+/*
+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 <rtabmap_util/disparity_to_depth.hpp>
+
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <stereo_msgs/msg/disparity_image.hpp>
+#include <rtabmap/utilite/UException.h>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr float kBaseline = 0.1f;     // t, meters
+constexpr float kFocal = 500.0f;      // f, pixels
+constexpr int kWidth = 4;
+constexpr int kHeight = 4;
+
+/// A 4x4 32FC1 disparity image, every pixel set to @p disparity.
+stereo_msgs::msg::DisparityImage makeDisparity(
+		float disparity,
+		const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
+{
+	stereo_msgs::msg::DisparityImage msg;
+	msg.header.frame_id = "camera_link";
+	msg.header.stamp = rclcpp::Time(1000, 0, RCL_ROS_TIME);
+	msg.t = kBaseline;
+	msg.f = kFocal;
+	msg.min_disparity = 1.0f;
+	msg.max_disparity = 100.0f;
+
+	msg.image.header = msg.header;
+	msg.image.encoding = encoding;
+	msg.image.height = kHeight;
+	msg.image.width = kWidth;
+	msg.image.step = kWidth * sizeof(float);
+	msg.image.data.resize(msg.image.step * kHeight);
+	float * p = reinterpret_cast<float *>(msg.image.data.data());
+	for(int i=0; i<kWidth*kHeight; ++i)
+	{
+		p[i] = disparity;
+	}
+	return msg;
+}
+
+float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
+{
+	return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
+}
+
+uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
+{
+	return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
+}
+}  // namespace
+
+class DisparityToDepthTest : public NodeTest {};
+
+TEST_F(DisparityToDepthTest, ConvertsDisparityToMetricDepth)
+{
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription)) << "the node never advertised depth";
+
+	// depth = baseline * focal / disparity = 0.1 * 500 / 10 = 5 m
+	pub->publish(makeDisparity(10.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
+
+	const sensor_msgs::msg::Image & img = depth->back();
+	EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+	EXPECT_EQ(img.width, uint32_t(kWidth));
+	EXPECT_EQ(img.height, uint32_t(kHeight));
+	EXPECT_EQ(img.header.frame_id, "camera_link") << "the input header must be preserved";
+	EXPECT_NEAR(pixel32f(img, 0, 0), 5.0f, 1e-4);
+	EXPECT_NEAR(pixel32f(img, kHeight-1, kWidth-1), 5.0f, 1e-4);
+}
+
+TEST_F(DisparityToDepthTest, PublishesMillimetersOnDepthRaw)
+{
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> raw =
+			collect<sensor_msgs::msg::Image>("depth_raw");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(raw->subscription));
+
+	pub->publish(makeDisparity(10.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !raw->empty(); }));
+
+	const sensor_msgs::msg::Image & img = raw->back();
+	EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(pixel16u(img, 0, 0), 5000) << "5 m expressed in millimeters";
+}
+
+TEST_F(DisparityToDepthTest, PublishesBothUnitsConsistentlyFromOneInput)
+{
+	// With both topics subscribed the node fills the 32FC1 and 16UC1 images in the same
+	// pass. The two must describe the same depth, one in meters and one in millimeters.
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> meters =
+			collect<sensor_msgs::msg::Image>("depth");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> millimeters =
+			collect<sensor_msgs::msg::Image>("depth_raw");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(meters->subscription));
+	ASSERT_TRUE(waitForPublisher(millimeters->subscription));
+
+	// A disparity of 25 gives 0.1 * 500 / 25 = 2 m.
+	pub->publish(makeDisparity(25.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !meters->empty() && !millimeters->empty(); }))
+		<< "both outputs must be produced from a single input";
+
+	EXPECT_EQ(meters->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+	EXPECT_EQ(millimeters->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+
+	for(int row=0; row<kHeight; ++row)
+	{
+		for(int col=0; col<kWidth; ++col)
+		{
+			const float m = pixel32f(meters->back(), row, col);
+			const uint16_t mm = pixel16u(millimeters->back(), row, col);
+			EXPECT_NEAR(m, 2.0f, 1e-4) << "at " << row << "," << col;
+			EXPECT_EQ(mm, 2000) << "at " << row << "," << col;
+			EXPECT_EQ(mm, uint16_t(m * 1000.0f)) << "the two units must agree at " << row << "," << col;
+		}
+	}
+}
+
+TEST_F(DisparityToDepthTest, LeavesOutOfRangeDisparityAtZero)
+{
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	// Above max_disparity (100), so no depth can be computed.
+	pub->publish(makeDisparity(500.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
+
+	EXPECT_FLOAT_EQ(pixel32f(depth->back(), 0, 0), 0.0f);
+}
+
+TEST_F(DisparityToDepthTest, RejectsNon32FC1Input)
+{
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	pub->publish(makeDisparity(10.0f, sensor_msgs::image_encodings::TYPE_16UC1));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(depth->empty()) << "only 32FC1 disparity is supported";
+}
+
+TEST_F(DisparityToDepthTest, HonorsTheConfiguredQueueDepths)
+{
+	// Queue depth is not observable from outside, so this pins down that the parameters
+	// are accepted and the node still converts with them set.
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("queue_sub", 20),
+								  rclcpp::Parameter("queue_pub", 10)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth");
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	pub->publish(makeDisparity(1.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
+	EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+}
+
+TEST_F(DisparityToDepthTest, RejectsAZeroQueueDepth)
+{
+	EXPECT_THROW(
+		addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
+				.parameter_overrides({rclcpp::Parameter("queue_pub", 0)}))),
+		UException);
+}
+
+TEST_F(DisparityToDepthTest, BridgesABestEffortSourceToAReliableConsumer)
+{
+	// A reliable subscription refuses to match a best-effort publisher, so setting the
+	// two sides apart is what lets the conversion cross that gap.
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("qos_sub", 2),
+								  rclcpp::Parameter("qos_pub", 1)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
+					"disparity", rclcpp::QoS(10).best_effort());
+	ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the node";
+	ASSERT_TRUE(waitForPublisher(depth->subscription))
+		<< "a reliable consumer must be able to subscribe to the depth output";
+
+	pub->publish(makeDisparity(1.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
+	EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+}
+
+TEST_F(DisparityToDepthTest, TheTwoQosSidesFallBackToQos)
+{
+	// Only qos is given, so both sides must be best effort: a reliable consumer matches
+	// neither the publishers nor, from the other end, the subscription.
+	addNode(std::make_shared<rtabmap_util::DisparityToDepth>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("qos", 2)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("depth", rclcpp::QoS(10).reliable());
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr pub =
+			helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
+					"disparity", rclcpp::QoS(10).best_effort());
+	EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
+
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(depth->subscription->get_publisher_count(), 0u)
+		<< "the publishers must have followed qos too: best effort, so a reliable "
+		   "consumer cannot match them";
+}
diff --git a/rtabmap_util/test/test_imu_to_tf.cpp b/rtabmap_util/test/test_imu_to_tf.cpp
new file mode 100644
index 00000000..f35ca7a7
--- /dev/null
+++ b/rtabmap_util/test/test_imu_to_tf.cpp
@@ -0,0 +1,205 @@
+/*
+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 <rtabmap_util/imu_to_tf.hpp>
+
+#include <sensor_msgs/msg/imu.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2/utils.hpp>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+/// An Imu message whose orientation is a pure rotation of @p yaw about z.
+sensor_msgs::msg::Imu makeImu(const std::string & frameId, double stamp, double yaw = 0.0)
+{
+	tf2::Quaternion q;
+	q.setRPY(0.0, 0.0, yaw);
+
+	sensor_msgs::msg::Imu msg;
+	msg.header.frame_id = frameId;
+	msg.header.stamp = rclcpp::Time(int32_t(stamp), uint32_t((stamp - int32_t(stamp)) * 1e9), RCL_ROS_TIME);
+	msg.orientation = tf2::toMsg(q);
+	return msg;
+}
+}  // namespace
+
+class ImuToTFTest : public NodeTest
+{
+protected:
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticTfKeepAlive_;
+};
+
+TEST_F(ImuToTFTest, BroadcastsOrientationAsTf)
+{
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "odom")})));
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub)) << "the node never subscribed to imu/data";
+
+	pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); })) << "no transform was broadcast";
+
+	ASSERT_EQ(tf->back().transforms.size(), 1u);
+	const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
+	EXPECT_EQ(t.header.frame_id, "odom");
+	EXPECT_EQ(t.child_frame_id, "imu_link") << "with no base_frame_id the imu frame is used";
+
+	// The broadcast rotation must be the IMU's orientation.
+	tf2::Quaternion q;
+	tf2::fromMsg(t.transform.rotation, q);
+	EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
+
+	// It is an orientation only: no translation.
+	EXPECT_NEAR(t.transform.translation.x, 0.0, 1e-9);
+	EXPECT_NEAR(t.transform.translation.y, 0.0, 1e-9);
+	EXPECT_NEAR(t.transform.translation.z, 0.0, 1e-9);
+}
+
+TEST_F(ImuToTFTest, UsesTheConfiguredFixedFrame)
+{
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("fixed_frame_id", "my_odom")})));
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeImu("imu_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+
+	EXPECT_EQ(tf->back().transforms[0].header.frame_id, "my_odom");
+}
+
+TEST_F(ImuToTFTest, PreservesTheImuStamp)
+{
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const sensor_msgs::msg::Imu imu = makeImu("imu_link", 1234.5);
+	pub->publish(imu);
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+
+	EXPECT_EQ(tf->back().transforms[0].header.stamp.sec, imu.header.stamp.sec);
+	EXPECT_EQ(tf->back().transforms[0].header.stamp.nanosec, imu.header.stamp.nanosec);
+}
+
+TEST_F(ImuToTFTest, ReportsTheOrientationInTheBaseFrame)
+{
+	// With base_frame_id set and the mounting transform available, the node re-expresses
+	// the IMU orientation in the base frame and broadcasts that frame instead.
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("base_frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
+	publishStaticTf("base_link", "imu_link", 0.1, 0.0, 0.2);   // translation only
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/2.0));
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }))
+		<< "with the mounting transform available a transform must be broadcast";
+
+	const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
+	EXPECT_EQ(t.header.frame_id, "odom");
+	EXPECT_EQ(t.child_frame_id, "base_link")
+		<< "the base frame is broadcast, not the imu frame";
+
+	// The mounting has no rotation, so the orientation is unchanged.
+	tf2::Quaternion q;
+	tf2::fromMsg(t.transform.rotation, q);
+	EXPECT_NEAR(tf2::getYaw(q), M_PI/2.0, 1e-6);
+}
+
+TEST_F(ImuToTFTest, IgnoresAYawOnlyMountingOffset)
+{
+	// The node strips the yaw of the mounting transform (it uses only getYaw to build
+	// the correction), so a purely yaw-rotated mount leaves the reported orientation
+	// alone: the IMU's absolute yaw is what matters, not how it is bolted on.
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("base_frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform_duration", 0.5)})));
+
+	// base_link -> imu_link rotated 90 degrees about z.
+	{
+		tf2::Quaternion mount;
+		mount.setRPY(0.0, 0.0, M_PI/2.0);
+		geometry_msgs::msg::TransformStamped m;
+		m.header.stamp = helper()->now();
+		m.header.frame_id = "base_link";
+		m.child_frame_id = "imu_link";
+		m.transform.rotation = tf2::toMsg(mount);
+		tf2_msgs::msg::TFMessage msg;
+		msg.transforms.push_back(m);
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr staticPub =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>(
+						"/tf_static", rclcpp::QoS(100).transient_local());
+		staticPub->publish(msg);
+		spinFor(std::chrono::milliseconds(150));
+		staticTfKeepAlive_ = staticPub;
+	}
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeImu("imu_link", 1000.0, /*yaw=*/M_PI/4.0));
+	ASSERT_TRUE(spinUntil([&]() { return !tf->empty(); }));
+
+	const geometry_msgs::msg::TransformStamped & t = tf->back().transforms[0];
+	EXPECT_EQ(t.child_frame_id, "base_link");
+
+	tf2::Quaternion q;
+	tf2::fromMsg(t.transform.rotation, q);
+	EXPECT_NEAR(tf2::getYaw(q), M_PI/4.0, 1e-5)
+		<< "the mounting yaw must cancel out, leaving the imu's own yaw";
+}
+
+TEST_F(ImuToTFTest, DropsTheMessageWhenTheBaseTransformIsMissing)
+{
+	// base_frame_id differs from the imu frame, so the node needs imu_link -> base_link
+	// from TF. Nothing publishes it, so nothing may be broadcast.
+	addNode(std::make_shared<rtabmap_util::ImuToTF>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("base_frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform_duration", 0.0)})));
+
+	std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tf =
+			collect<tf2_msgs::msg::TFMessage>("/tf");
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeImu("imu_link", 1000.0, M_PI/2.0));
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(tf->empty()) << "without the base transform the node must not broadcast";
+}
diff --git a/rtabmap_util/test/test_lidar_deskewing.cpp b/rtabmap_util/test/test_lidar_deskewing.cpp
new file mode 100644
index 00000000..e28db65c
--- /dev/null
+++ b/rtabmap_util/test/test_lidar_deskewing.cpp
@@ -0,0 +1,204 @@
+/*
+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_util/lidar_deskewing.hpp>
+
+#include <cmath>
+
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+class LidarDeskewingTest : public NodeTest
+{
+protected:
+	static constexpr double kSweep = 0.099;    ///< first sample to last, seconds
+	static constexpr double kSpeed = 1.0;      ///< m/s, straight at the wall
+	static constexpr float kWall = 5.0f;       ///< distance to the wall, meters
+
+	/// Distance travelled since the first sample. Drives both the TF and the skew.
+	static double travelled(double elapsed) { return kSpeed * elapsed; }
+
+	/// Publishes odom -> lidar following exactly that trajectory.
+	void publishOdomMotion(double startStamp)
+	{
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+		spinFor(std::chrono::milliseconds(100));   // let the node's listener subscribe
+
+		// Covers exactly the sweep, from the first sample to the last. Nothing beyond:
+		// asking for more than laser_geometry needs would be a regression.
+		for(int i=0; i<=2; ++i)
+		{
+			const double elapsed = kSweep * double(i) / 2.0;
+			geometry_msgs::msg::TransformStamped t;
+			t.header.stamp = stampOf(startStamp + elapsed);
+			t.header.frame_id = "odom";
+			t.child_frame_id = "lidar";
+			t.transform.translation.x = travelled(elapsed);
+			t.transform.rotation.w = 1.0;
+			tf2_msgs::msg::TFMessage msg;
+			msg.transforms.push_back(t);
+			tfPub->publish(msg);
+		}
+		spinFor(std::chrono::milliseconds(200));   // let the buffer fill
+		tfPub_ = tfPub;                            // keep the publisher alive
+	}
+
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
+};
+
+TEST_F(LidarDeskewingTest, DeskewsACloudUsingTf)
+{
+	// The wall is recorded bent because the sensor closes in during the sweep, and TF
+	// carries that same motion. A correct deskew must flatten it back to kWall.
+	addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+
+	publishOdomMotion(1000.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const size_t sampleCount = 20;
+	const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(
+			"lidar", 1000.0, sampleCount, kSweep, kWall, &travelled);
+
+	// The input really is bent: the last sample is a full sweep of travel closer.
+	ASSERT_NEAR(readXYZ(in, 0).x, kWall, 1e-4);
+	ASSERT_NEAR(readXYZ(in, sampleCount-1).x, kWall - float(kSpeed*kSweep), 1e-4);
+
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out->back();
+	EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
+	ASSERT_EQ(cloud.width, sampleCount);
+
+	// Every sample must land back on the wall.
+	for(size_t i=0; i<sampleCount; ++i)
+	{
+		EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "sample " << i;
+	}
+}
+
+TEST_F(LidarDeskewingTest, DeskewsAScanUsingTf)
+{
+	// Same idea as the cloud case, for the 2D path. A ray at angle theta taken once the
+	// sensor has advanced d meters measures (wall - d)/cos(theta), so the raw scan bends.
+	// Deskewing must put every point back on the wall at x = kWall.
+	addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+
+	publishOdomMotion(1000.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = "lidar";
+	scan.header.stamp = stampOf(1000.0);
+	scan.angle_min = -0.4f;
+	scan.angle_max = 0.4f;
+	scan.angle_increment = 0.05f;
+	scan.range_min = 0.1f;
+	scan.range_max = 30.0f;
+	const size_t rayCount = size_t((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1;
+	scan.time_increment = float(kSweep / double(rayCount - 1));
+	scan.ranges.resize(rayCount);
+	for(size_t i=0; i<rayCount; ++i)
+	{
+		const double elapsed = double(i) * scan.time_increment;
+		const double angle = scan.angle_min + double(i) * scan.angle_increment;
+		// Distance to a wall at x=kWall, from a sensor that has already advanced.
+		scan.ranges[i] = float((kWall - travelled(elapsed)) / std::cos(angle));
+	}
+
+	pub->publish(scan);
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no deskewed scan published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out->back();
+	EXPECT_EQ(cloud.header.frame_id, "lidar") << "output stays in the sensor frame";
+	ASSERT_EQ(cloud.width, rayCount);
+
+	// Without deskewing the last ray would sit a full sweep of travel short of the wall.
+	for(size_t i=0; i<rayCount; ++i)
+	{
+		EXPECT_NEAR(readXYZ(cloud, i).x, kWall, 5e-3) << "ray " << i;
+	}
+}
+
+TEST_F(LidarDeskewingTest, RepublishesTheCloudUnchangedWhenDeskewingFails)
+{
+	// With no TF the cloud cannot be deskewed, but the node deliberately republishes it
+	// as-is rather than dropping it, so downstream nodes keep receiving data.
+	addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.0)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("input_cloud/deskewed");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("input_cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const sensor_msgs::msg::PointCloud2 in =
+			makeXYZCloud("lidar", 1000.0, {{5.0f, 0.0f, 0.0f}, {5.0f, 1.0f, 0.0f}}, true);
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
+		<< "the cloud must still be forwarded";
+
+	EXPECT_EQ(out->back().data, in.data) << "and forwarded byte for byte, still skewed";
+}
+
+TEST_F(LidarDeskewingTest, DropsAScanWhenTfIsMissing)
+{
+	// The 2D scan path does the opposite of the cloud path: it returns early and
+	// publishes nothing when the transform is unavailable.
+	addNode(std::make_shared<rtabmap_util::LidarDeskewing>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.0)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("input_scan/deskewed");
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::LaserScan>("input_scan", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	sensor_msgs::msg::LaserScan scan;
+	scan.header.frame_id = "lidar";
+	scan.header.stamp = stampOf(1000.0);
+	scan.angle_min = -1.0f;
+	scan.angle_max = 1.0f;
+	scan.angle_increment = 0.1f;
+	scan.time_increment = 0.001f;
+	scan.range_min = 0.1f;
+	scan.range_max = 30.0f;
+	scan.ranges.assign(21, 5.0f);
+	pub->publish(scan);
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out->empty()) << "the scan path drops the message instead of forwarding it";
+}
diff --git a/rtabmap_util/test/test_map_assembler.cpp b/rtabmap_util/test/test_map_assembler.cpp
new file mode 100644
index 00000000..e2b22312
--- /dev/null
+++ b/rtabmap_util/test/test_map_assembler.cpp
@@ -0,0 +1,549 @@
+/*
+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_util/map_assembler.hpp>
+
+#include <rtabmap_conversions/MsgConversion.h>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Signature.h>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_srvs/srv/empty.hpp>
+
+#include <thread>
+
+#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
+#include <octomap_msgs/srv/get_octomap.hpp>
+#endif
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr float kCellSize = 0.05f;
+/// Anything above this is an obstacle once a grid is regenerated from a scan.
+constexpr float kGroundHeight = 0.1f;
+constexpr float kObstacleHeight = 0.5f;
+
+cv::Mat toCellMat(const std::vector<cv::Point3f> & points)
+{
+	if(points.empty()) { return cv::Mat(); }
+	cv::Mat mat(1, int(points.size()), CV_32FC3);
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		mat.at<cv::Vec3f>(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z);
+	}
+	return mat;
+}
+
+/**
+ * @brief A graph node as it arrives on "mapData".
+ *
+ * map_assembler only caches a node that carries compressed images or a compressed scan,
+ * so the scan is what makes the node acceptable at all. The occupancy grid is what
+ * MapsManager normally uses; the two are deliberately given different geometry so a test
+ * can tell which one ended up in the map.
+ *
+ * @param scan     points of the raw scan, in the node's frame
+ * @param ground   ground cells of the ready-made grid
+ * @param obstacles obstacle cells of the ready-made grid
+ */
+rtabmap::Signature makeNode(
+		int id, const rtabmap::Transform & pose,
+		const std::vector<cv::Point3f> & scan,
+		const std::vector<cv::Point3f> & ground,
+		const std::vector<cv::Point3f> & obstacles)
+{
+	rtabmap::SensorData data;
+	data.setId(id);
+	data.setStamp(1000.0 + id);
+	data.setLaserScan(rtabmap::LaserScan(rtabmap::compressData2(toCellMat(scan)),
+			/*maxPoints=*/0, /*maxRange=*/0.0f, rtabmap::LaserScan::kXYZ));
+	if(!ground.empty() || !obstacles.empty())
+	{
+		data.setOccupancyGrid(toCellMat(ground), toCellMat(obstacles), cv::Mat(), kCellSize,
+				cv::Point3f(0, 0, 0));
+	}
+	return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"",
+			pose, rtabmap::Transform(), data);
+}
+
+cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t xo = 0, yo = 4, zo = 8;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "x") { xo = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "y") { yo = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "z") { zo = cloud.fields[i].offset; }
+	}
+	const unsigned char * base = &cloud.data[index * cloud.point_step];
+	return cv::Point3f(
+			*reinterpret_cast<const float *>(base + xo),
+			*reinterpret_cast<const float *>(base + yo),
+			*reinterpret_cast<const float *>(base + zo));
+}
+
+bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected,
+		float tolerance = 1e-3f)
+{
+	for(size_t i=0; i<size_t(cloud.width)*cloud.height; ++i)
+	{
+		if(cv::norm(pointAt(cloud, i) - expected) < tolerance) { return true; }
+	}
+	return false;
+}
+
+size_t pointCount(const sensor_msgs::msg::PointCloud2 & cloud)
+{
+	return size_t(cloud.width) * cloud.height;
+}
+}  // namespace
+
+/**
+ * map_assembler subscribes to "mapData", caches the nodes it carries and hands them to a
+ * MapsManager, which is what actually publishes the maps.
+ *
+ * By default it first asks rtabmap for the map it missed: a one second timer whose
+ * callback calls get_map_data, blocks on the reply, and only then subscribes to
+ * "mapData". That needs two callbacks of the same node to run at once, hence the fake
+ * service and spinMultiThreadedUntil() in startInitializingFromRtabmap(). Most tests
+ * below have nothing to catch up on, so start() sets the timeout to 0 and the node
+ * subscribes immediately.
+ */
+class MapAssemblerTest : public NodeTest
+{
+protected:
+	/// The name the fake get_map_data service is advertised under.
+	static constexpr const char * kRtabmapName = "fake_rtabmap";
+
+	void SetUp() override
+	{
+		NodeTest::SetUp();
+		getMapCalls_ = 0;
+	}
+
+	/**
+	 * @brief Advertises the get_map_data service map_assembler calls on start-up.
+	 *
+	 * @param initial the map handed back, i.e. what the node starts with in its cache.
+	 */
+	void advertiseGetMapData(const rtabmap_msgs::msg::MapData & initial =
+			rtabmap_msgs::msg::MapData())
+	{
+		initialMap_ = initial;
+		getMapService_ = helper()->create_service<rtabmap_msgs::srv::GetMap>(
+				std::string(kRtabmapName) + "/get_map_data",
+				[this](const std::shared_ptr<rtabmap_msgs::srv::GetMap::Request>,
+					   std::shared_ptr<rtabmap_msgs::srv::GetMap::Response> response) {
+					++getMapCalls_;
+					response->data = initialMap_;
+				});
+	}
+
+	/// Creates the node with the start-up call skipped, so it subscribes right away.
+	void start(std::vector<rclcpp::Parameter> overrides = {})
+	{
+		overrides.push_back(rclcpp::Parameter("initialize_from_rtabmap_timeout", 0.0));
+		createNode(overrides);
+		ASSERT_TRUE(waitForSubscriber(mapDataPub_))
+			<< "map_assembler never subscribed to mapData";
+	}
+
+	/**
+	 * @brief Creates the node with the start-up call enabled, as it is by default.
+	 *
+	 * It only subscribes to "mapData" once that call has returned, and the call blocks a
+	 * callback on another callback of the same node, so it needs a multi-threaded
+	 * executor to get through.
+	 */
+	void startInitializingFromRtabmap(double timeout = 5.0,
+			std::vector<rclcpp::Parameter> overrides = {})
+	{
+		overrides.push_back(
+				rclcpp::Parameter("initialize_from_rtabmap_timeout", timeout));
+		createNode(overrides);
+		ASSERT_TRUE(spinMultiThreadedUntil(
+				[&]() { return mapDataPub_->get_subscription_count() > 0; }))
+			<< "map_assembler never subscribed to mapData";
+	}
+
+	void createNode(std::vector<rclcpp::Parameter> overrides)
+	{
+		overrides.push_back(rclcpp::Parameter("rtabmap", std::string(kRtabmapName)));
+		assembler_ = addNode(std::make_shared<rtabmap_util::MapAssembler>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		mapDataPub_ = helper()->create_publisher<rtabmap_msgs::msg::MapData>("mapData",
+				rclcpp::QoS(1));
+	}
+
+	/// A MapData carrying @p signatures and a graph over their poses.
+	static rtabmap_msgs::msg::MapData makeMapData(
+			const std::vector<rtabmap::Signature> & signatures,
+			const std::vector<int> & graphIds = {})
+	{
+		std::map<int, rtabmap::Transform> poses;
+		rtabmap_msgs::msg::MapData msg;
+		msg.header.frame_id = "map";
+		msg.header.stamp = stampOf(2000.0);
+
+		for(size_t i=0; i<signatures.size(); ++i)
+		{
+			rtabmap_msgs::msg::Node node;
+			rtabmap_conversions::nodeToROS(signatures[i], node);
+			msg.nodes.push_back(node);
+			poses.insert(std::make_pair(signatures[i].id(), signatures[i].getPose()));
+		}
+		// The graph may name nodes whose data is not resent, which is the normal case
+		// once map_assembler has them cached.
+		for(size_t i=0; i<graphIds.size(); ++i)
+		{
+			poses.insert(std::make_pair(graphIds[i], poseOf(graphIds[i])));
+		}
+		rtabmap_conversions::mapGraphToROS(poses, std::multimap<int, rtabmap::Link>(),
+				rtabmap::Transform::getIdentity(), msg.graph);
+		return msg;
+	}
+
+	static rtabmap::Transform poseOf(int id)
+	{
+		return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	}
+
+	/// Two nodes whose ready-made grids hold one ground and one obstacle cell each.
+	static std::vector<rtabmap::Signature> twoNodes()
+	{
+		return {
+			makeNode(1, poseOf(1),
+					/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
+							  cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
+					/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
+					/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)}),
+			makeNode(2, poseOf(2),
+					/*scan=*/{cv::Point3f(0.5f, -0.1f, 0.0f),
+							  cv::Point3f(0.5f, 0.1f, kObstacleHeight)},
+					/*ground=*/{cv::Point3f(0.5f, -0.1f, 0.0f)},
+					/*obstacles=*/{cv::Point3f(1.0f, 0.1f, 0.0f)})};
+	}
+
+	void publishMapData(const rtabmap_msgs::msg::MapData & msg)
+	{
+		mapDataPub_->publish(msg);
+	}
+
+	std::shared_ptr<rtabmap_util::MapAssembler> assembler_;
+	rclcpp::Publisher<rtabmap_msgs::msg::MapData>::SharedPtr mapDataPub_;
+	rclcpp::Service<rtabmap_msgs::srv::GetMap>::SharedPtr getMapService_;
+	rtabmap_msgs::msg::MapData initialMap_;
+	std::atomic_int getMapCalls_{0};
+};
+
+constexpr const char * MapAssemblerTest::kRtabmapName;
+
+//============================================================================
+// Start-up
+//============================================================================
+
+TEST_F(MapAssemblerTest, AsksRtabmapForTheMapByDefault)
+{
+	advertiseGetMapData(makeMapData(twoNodes()));
+	createNode({});   // no overrides at all, so the default timeout applies
+	ASSERT_TRUE(spinMultiThreadedUntil(
+			[&]() { return mapDataPub_->get_subscription_count() > 0; }));
+
+	EXPECT_EQ(getMapCalls_.load(), 1) << "the start-up service call is made exactly once";
+}
+
+TEST_F(MapAssemblerTest, SkipsTheStartUpCallWhenTheTimeoutIsZero)
+{
+	// Nothing to catch up on, so the node should not spend its start-up waiting on a
+	// service: it subscribes immediately instead.
+	advertiseGetMapData(makeMapData(twoNodes()));
+	start();
+	EXPECT_EQ(getMapCalls_.load(), 0) << "rtabmap must not be called with a zero timeout";
+}
+
+TEST_F(MapAssemblerTest, SubscribesAnywayWhenRtabmapNeverAnswers)
+{
+	// Nothing advertises get_map_data, so the start-up call times out. The node must
+	// still come up and subscribe, since rtabmap may be started afterwards.
+	// Short, because unlike every other test here this one waits the timeout out.
+	startInitializingFromRtabmap(/*timeout=*/0.5);
+	EXPECT_EQ(getMapCalls_.load(), 0);
+}
+
+TEST_F(MapAssemblerTest, WaitsForRtabmapToShowUpDuringTheTimeout)
+{
+	// get_map_data is not advertised when the node starts asking for it: it appears part
+	// way through the wait. The call must still go through, which is what makes the
+	// timeout a real wait rather than a check of what happens to be up already.
+	//
+	// The node's timer fires one second after construction and then waits 750 ms, so
+	// advertising at 1250 ms lands inside that window with room on both sides.
+	std::thread rtabmapStartsLate([this]() {
+		std::this_thread::sleep_for(std::chrono::milliseconds(1250));
+		advertiseGetMapData(makeMapData(twoNodes()));
+	});
+
+	startInitializingFromRtabmap(/*timeout=*/0.75);
+	rtabmapStartsLate.join();
+
+	EXPECT_EQ(getMapCalls_.load(), 1) << "rtabmap showed up before the wait expired";
+}
+
+TEST_F(MapAssemblerTest, StartsFromTheMapRtabmapHandsBack)
+{
+	// The nodes come from the start-up call, and the graph that arrives later names them
+	// without resending their data. The map must still be assembled from the cache.
+	advertiseGetMapData(makeMapData(twoNodes()));
+	startInitializingFromRtabmap();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+
+	publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "no cloud assembled";
+
+	EXPECT_EQ(pointCount(cloud->back()), 4u) << "one ground and one obstacle cell per node";
+}
+
+//============================================================================
+// Assembling
+//============================================================================
+
+TEST_F(MapAssemblerTest, AssemblesTheCloudFromMapData)
+{
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+
+	publishMapData(makeMapData(twoNodes()));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty() && !obstacles->empty(); }))
+		<< "no cloud assembled";
+
+	EXPECT_EQ(cloud->back().header.frame_id, "map") << "the frame comes from the message";
+	EXPECT_EQ(pointCount(cloud->back()), 4u);
+	EXPECT_EQ(pointCount(obstacles->back()), 2u);
+	// Node 2 sits 2 m along x, so its obstacle cell lands at 3 m.
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)));
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, 0.1f, 0.0f)));
+}
+
+TEST_F(MapAssemblerTest, PublishesTheOccupancyGrid)
+{
+	start();
+
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collect<nav_msgs::msg::OccupancyGrid>("map");
+	ASSERT_TRUE(waitForPublisher(grid->subscription));
+
+	publishMapData(makeMapData(twoNodes()));
+	ASSERT_TRUE(spinUntil([&]() { return !grid->empty(); })) << "no grid assembled";
+
+	EXPECT_EQ(grid->back().header.frame_id, "map");
+	EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6);
+	EXPECT_GT(grid->back().info.width, 0u);
+}
+
+TEST_F(MapAssemblerTest, IgnoresAnEmptyMapData)
+{
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+
+	rtabmap_msgs::msg::MapData empty;
+	empty.header.frame_id = "map";
+	empty.header.stamp = stampOf(2000.0);
+	publishMapData(empty);
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(cloud->empty()) << "a message with no graph and no nodes is nothing to do";
+}
+
+TEST_F(MapAssemblerTest, PublishesAnEmptyMapForAGraphWithNoCachedNodes)
+{
+	// A graph can name nodes whose data map_assembler has never seen -- it has no cache
+	// at all here. It still publishes, using the poses as they are.
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+
+	publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); })) << "nothing published";
+
+	EXPECT_EQ(pointCount(cloud->back()), 0u) << "no data cached, so nothing to assemble";
+	EXPECT_EQ(cloud->back().header.frame_id, "map");
+}
+
+//============================================================================
+// regenerate_local_grids
+//============================================================================
+
+TEST_F(MapAssemblerTest, UsesTheGridsThatCameWithTheNodes)
+{
+	// By default the ready-made grid wins: its obstacle is at y=+0.1, the scan's is at
+	// y=-0.1 with the ground point, so the two are told apart by where the cells land.
+	start({rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
+		   rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
+				   std::string("false")),
+		   rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
+				   std::string("0.1"))});
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+	ASSERT_TRUE(waitForPublisher(obstacles->subscription));
+
+	publishMapData(makeMapData({twoNodes()[0]}));
+	ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
+
+	EXPECT_EQ(pointCount(obstacles->back()), 1u);
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
+		<< "the obstacle cell of the grid that came with the node";
+}
+
+TEST_F(MapAssemblerTest, RegenerateLocalGridsRebuildsThemFromTheScan)
+{
+	// With regenerate_local_grids the grid that came with the node is thrown away, so
+	// MapsManager segments the scan instead: the raised scan point becomes the obstacle.
+	start({rclcpp::Parameter("regenerate_local_grids", true),
+		   rclcpp::Parameter(rtabmap::Parameters::kGridSensor(), std::string("0")),
+		   rclcpp::Parameter(rtabmap::Parameters::kGridNormalsSegmentation(),
+				   std::string("false")),
+		   rclcpp::Parameter(rtabmap::Parameters::kGridMaxGroundHeight(),
+				   std::string("0.1"))});
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+	ASSERT_TRUE(waitForPublisher(obstacles->subscription));
+
+	publishMapData(makeMapData({twoNodes()[0]}));
+	ASSERT_TRUE(spinUntil([&]() { return !obstacles->empty(); }));
+
+	EXPECT_EQ(pointCount(obstacles->back()), 1u);
+	EXPECT_TRUE(containsPoint(obstacles->back(),
+			cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize))
+		<< "the raised scan point, not the cell the node arrived with";
+	EXPECT_FALSE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.1f, 0.0f)))
+		<< "the grid that came with the node must have been discarded";
+}
+
+//============================================================================
+// Services
+//============================================================================
+
+TEST_F(MapAssemblerTest, ResetEmptiesTheMap)
+{
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+
+	publishMapData(makeMapData(twoNodes()));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
+	ASSERT_EQ(pointCount(cloud->back()), 4u);
+
+	rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset =
+			helper()->create_client<std_srvs::srv::Empty>("map_assembler/reset");
+	ASSERT_TRUE(spinUntil([&]() { return reset->service_is_ready(); }))
+		<< "the reset service was never advertised";
+	reset->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
+	ASSERT_TRUE(spinUntil([&]() { return cloud->size() >= 1u; }));
+	spinFor(std::chrono::milliseconds(200));
+
+	// The cache is gone, so the same graph now assembles nothing.
+	const size_t before = cloud->size();
+	publishMapData(makeMapData({}, /*graphIds=*/{1, 2}));
+	ASSERT_TRUE(spinUntil([&]() { return cloud->size() > before; }));
+	EXPECT_EQ(pointCount(cloud->back()), 0u) << "reset must drop the cached nodes";
+}
+
+#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
+TEST_F(MapAssemblerTest, ServesTheBinaryOctomap)
+{
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+	publishMapData(makeMapData(twoNodes()));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
+
+	rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
+			helper()->create_client<octomap_msgs::srv::GetOctomap>(
+					"map_assembler/octomap_binary");
+	ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
+
+	auto future = client->async_send_request(
+			std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
+	ASSERT_TRUE(spinUntil([&]() {
+		return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }))
+		<< "octomap_binary never answered";
+
+	// Keep the response alive: future.get() hands back a temporary shared_ptr, so binding
+	// a reference into it would dangle.
+	const std::shared_ptr<octomap_msgs::srv::GetOctomap::Response> response = future.get();
+	EXPECT_EQ(response->map.header.frame_id, "map")
+		<< "the frame of the last map data received";
+	EXPECT_TRUE(response->map.binary);
+	EXPECT_FALSE(response->map.data.empty())
+		<< "the octomap is built on demand from the cache";
+}
+
+TEST_F(MapAssemblerTest, ServesTheFullOctomap)
+{
+	start();
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>("cloud_map");
+	ASSERT_TRUE(waitForPublisher(cloud->subscription));
+	publishMapData(makeMapData(twoNodes()));
+	ASSERT_TRUE(spinUntil([&]() { return !cloud->empty(); }));
+
+	rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
+			helper()->create_client<octomap_msgs::srv::GetOctomap>(
+					"map_assembler/octomap_full");
+	ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
+
+	auto future = client->async_send_request(
+			std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
+	ASSERT_TRUE(spinUntil([&]() {
+		return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
+
+	EXPECT_FALSE(future.get()->map.binary);
+}
+
+TEST_F(MapAssemblerTest, ServesAnEmptyOctomapWithoutData)
+{
+	start();
+
+	rclcpp::Client<octomap_msgs::srv::GetOctomap>::SharedPtr client =
+			helper()->create_client<octomap_msgs::srv::GetOctomap>(
+					"map_assembler/octomap_binary");
+	ASSERT_TRUE(spinUntil([&]() { return client->service_is_ready(); }));
+
+	auto future = client->async_send_request(
+			std::make_shared<octomap_msgs::srv::GetOctomap::Request>());
+	ASSERT_TRUE(spinUntil([&]() {
+		return future.wait_for(std::chrono::seconds(0)) == std::future_status::ready; }));
+
+	EXPECT_TRUE(future.get()->map.data.empty()) << "nothing cached, nothing to serve";
+}
+#endif
diff --git a/rtabmap_util/test/test_maps_manager.cpp b/rtabmap_util/test/test_maps_manager.cpp
new file mode 100644
index 00000000..aa49d7f1
--- /dev/null
+++ b/rtabmap_util/test/test_maps_manager.cpp
@@ -0,0 +1,1060 @@
+/*
+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 <rtabmap_util/MapsManager.h>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <algorithm>
+
+#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
+#include <octomap_msgs/msg/octomap.hpp>
+#endif
+#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
+#include <grid_map_msgs/msg/grid_map.hpp>
+#endif
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr float kCellSize = 0.05f;
+
+/// Anything at or below this height is ground, anything above it an obstacle.
+constexpr float kGroundHeight = 0.1f;
+/// The height of node 2's obstacle, which is what puts it on the obstacle side.
+constexpr float kObstacleHeight = 0.5f;
+
+/**
+ * @brief A node carrying a ready-made local occupancy grid.
+ *
+ * MapsManager only regenerates a local grid when the sensor data has none
+ * (`gridCellSize() == 0`). Handing it the cells directly keeps the assembled map exactly
+ * predictable, instead of depending on how a depth image or scan would be segmented.
+ *
+ * @param cells coordinates in the node's own frame; the pose is applied when assembling.
+ */
+rtabmap::Signature makeGridSignature(
+		int id, const rtabmap::Transform & pose,
+		const std::vector<cv::Point3f> & ground,
+		const std::vector<cv::Point3f> & obstacles,
+		const std::vector<cv::Point3f> & empty = {})
+{
+	auto toMat = [](const std::vector<cv::Point3f> & points) {
+		if(points.empty()) { return cv::Mat(); }
+		cv::Mat mat(1, int(points.size()), CV_32FC3);
+		for(size_t i=0; i<points.size(); ++i)
+		{
+			mat.at<cv::Vec3f>(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z);
+		}
+		return mat;
+	};
+
+	rtabmap::SensorData data;
+	data.setId(id);
+	data.setStamp(1000.0 + id);
+	data.setOccupancyGrid(toMat(ground), toMat(obstacles), toMat(empty), kCellSize,
+			cv::Point3f(0, 0, 0));
+
+	rtabmap::Signature s(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"", pose,
+			rtabmap::Transform(), data);
+	return s;
+}
+
+/**
+ * @brief A node carrying a raw laser scan, which MapsManager has to segment itself.
+ *
+ * This is the other half of updateMapCaches(): when the sensor data has no local grid it
+ * builds one with LocalGridMaker instead of just caching the cells. With the parameters
+ * in MapsManagerTest::sceneParameters() the segmentation is a plain height passthrough,
+ * so which points come back as ground and which as obstacles is decided by their z alone.
+ */
+rtabmap::Signature makeScanSignature(
+		int id, const rtabmap::Transform & pose, const std::vector<cv::Point3f> & points)
+{
+	cv::Mat scan(1, int(points.size()), CV_32FC3);
+	for(size_t i=0; i<points.size(); ++i)
+	{
+		scan.at<cv::Vec3f>(0, int(i)) = cv::Vec3f(points[i].x, points[i].y, points[i].z);
+	}
+
+	rtabmap::SensorData data;
+	data.setId(id);
+	data.setStamp(1000.0 + id);
+	data.setLaserScan(rtabmap::LaserScan(scan, /*maxPoints=*/0, /*maxRange=*/0.0f,
+			rtabmap::LaserScan::kXYZ));
+
+	return rtabmap::Signature(id, /*mapId=*/0, /*weight=*/1, data.stamp(), /*label=*/"",
+			pose, rtabmap::Transform(), data);
+}
+
+/// Reads point @p index of an XYZRGB cloud.
+cv::Point3f pointAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t xo = 0, yo = 4, zo = 8;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "x") { xo = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "y") { yo = cloud.fields[i].offset; }
+		else if(cloud.fields[i].name == "z") { zo = cloud.fields[i].offset; }
+	}
+	const unsigned char * base = &cloud.data[index * cloud.point_step];
+	return cv::Point3f(
+			*reinterpret_cast<const float *>(base + xo),
+			*reinterpret_cast<const float *>(base + yo),
+			*reinterpret_cast<const float *>(base + zo));
+}
+
+/// Reads the packed rgb field of point @p index as (r,g,b).
+cv::Vec3b colorAt(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t offset = 16;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "rgb") { offset = cloud.fields[i].offset; }
+	}
+	uint32_t packed = 0;
+	memcpy(&packed, &cloud.data[index * cloud.point_step + offset], 4);
+	return cv::Vec3b(uint8_t(packed >> 16), uint8_t(packed >> 8), uint8_t(packed));
+}
+
+/**
+ * @brief The center of octomap voxel (@p i, @p j, @p k) at kCellSize resolution.
+ *
+ * Cells handed to the octomap have to sit on voxel centers when their neighbors matter:
+ * a coordinate on a voxel boundary (a multiple of the cell size) falls on either side
+ * depending on rounding, so a cell meant to touch its neighbor may not.
+ */
+cv::Point3f voxelCenter(int i, int j, int k)
+{
+	return cv::Point3f((float(i)+0.5f)*kCellSize, (float(j)+0.5f)*kCellSize,
+			(float(k)+0.5f)*kCellSize);
+}
+
+/// Where OctoMap::createCloud() reports the voxel centerd at @p center: x and y at the
+/// cell corner, z at the center.
+cv::Point3f asReported(const cv::Point3f & center)
+{
+	return cv::Point3f(center.x - 0.5f*kCellSize, center.y - 0.5f*kCellSize, center.z);
+}
+
+/// True if @p cloud holds a point within @p tolerance of @p expected.
+bool containsPoint(const sensor_msgs::msg::PointCloud2 & cloud, const cv::Point3f & expected,
+		float tolerance = 1e-3f)
+{
+	for(size_t i=0; i<size_t(cloud.width)*cloud.height; ++i)
+	{
+		if(cv::norm(pointAt(cloud, i) - expected) < tolerance) { return true; }
+	}
+	return false;
+}
+
+/// The occupancy value at world position (@p x, @p y), or -2 if it falls outside the map.
+int8_t cellAt(const nav_msgs::msg::OccupancyGrid & map, double x, double y)
+{
+	const int col = int((x - map.info.origin.position.x) / map.info.resolution);
+	const int row = int((y - map.info.origin.position.y) / map.info.resolution);
+	if(col < 0 || row < 0 || col >= int(map.info.width) || row >= int(map.info.height))
+	{
+		return -2;
+	}
+	return map.data[size_t(row) * map.info.width + col];
+}
+/**
+ * @brief True if any cell within @p radius cells of (@p x, @p y) holds @p value.
+ *
+ * Used for the octomap grid, which is discretized on OctoMap's own voxel lattice: the
+ * cell containing a given point can sit a column away from where the same point lands in
+ * the occupancy grid, and pinning that offset would be testing octomap's internals.
+ */
+bool hasValueNear(const nav_msgs::msg::OccupancyGrid & map, double x, double y,
+		int8_t value, int radius = 1)
+{
+	const int col = int((x - map.info.origin.position.x) / map.info.resolution);
+	const int row = int((y - map.info.origin.position.y) / map.info.resolution);
+	for(int r=row-radius; r<=row+radius; ++r)
+	{
+		for(int c=col-radius; c<=col+radius; ++c)
+		{
+			if(r >= 0 && c >= 0 && r < int(map.info.height) && c < int(map.info.width) &&
+			   map.data[size_t(r) * map.info.width + c] == value)
+			{
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+/// How many cells of @p map hold @p value.
+int countCells(const nav_msgs::msg::OccupancyGrid & map, int8_t value)
+{
+	int count = 0;
+	for(size_t i=0; i<map.data.size(); ++i)
+	{
+		if(map.data[i] == value) { ++count; }
+	}
+	return count;
+}
+}  // namespace
+
+/**
+ * MapsManager is a helper object rather than a node: it is handed an rclcpp::Node to
+ * advertise on, then fed poses and signatures. The tests drive it the way map_assembler
+ * does -- init(), backwardCompatibilityParameters(), setParameters(), then
+ * updateMapCaches() and publishMaps() -- and read the results off its topics.
+ *
+ * Everything it publishes is gated on subscribers, so a test subscribes before asking it
+ * to publish.
+ */
+class MapsManagerTest : public NodeTest
+{
+protected:
+	/**
+	 * @brief The rtabmap parameters scene() is written against.
+	 *
+	 * Node 2's grid is regenerated from its scan, so the segmentation has to be
+	 * deterministic: no normals, just a height passthrough splitting ground from
+	 * obstacles at kGroundHeight.
+	 */
+	static rtabmap::ParametersMap sceneParameters()
+	{
+		rtabmap::ParametersMap parameters;
+		parameters.insert(rtabmap::ParametersPair(
+				rtabmap::Parameters::kGridSensor(), "0"));   // build the grid from the scan
+		parameters.insert(rtabmap::ParametersPair(
+				rtabmap::Parameters::kGridNormalsSegmentation(), "false"));
+		parameters.insert(rtabmap::ParametersPair(
+				rtabmap::Parameters::kGridMaxGroundHeight(), uNumber2Str(kGroundHeight)));
+		parameters.insert(rtabmap::ParametersPair(
+				rtabmap::Parameters::kGridMaxObstacleHeight(), "2.0"));
+		return parameters;
+	}
+
+	void start(const std::vector<rclcpp::Parameter> & overrides = {},
+			const rtabmap::ParametersMap & rtabmapParameters = rtabmap::ParametersMap())
+	{
+		// Each test gets its own namespace: MapsManager reports whether anyone is
+		// listening, and a subscription from a previous test in this process can still
+		// be winding down on the shared topic names.
+		static int counter = 0;
+		namespace_ = uFormat("/maps_manager_test_%d", ++counter);
+		node_ = addNode(std::make_shared<rclcpp::Node>("maps_manager_test", namespace_,
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		maps_ = std::make_shared<rtabmap_util::MapsManager>();
+		maps_->init(*node_, "test", true);
+
+		rtabmap::ParametersMap parameters = sceneParameters();
+		for(rtabmap::ParametersMap::const_iterator iter=rtabmapParameters.begin();
+			iter!=rtabmapParameters.end(); ++iter)
+		{
+			parameters[iter->first] = iter->second;
+		}
+		maps_->setParameters(parameters);
+	}
+
+	/// The fully qualified name of one of MapsManager's topics.
+	std::string topic(const std::string & name) const { return namespace_ + "/" + name; }
+
+	/**
+	 * @brief The two-node scene every geometric assertion below is written against.
+	 *
+	 * @note The cells span both axes on purpose. An occupancy grid is a 2D map, and
+	 *       OccupancyGrid::assemble() deliberately builds nothing from a scene that is
+	 *       only a line of cells, so a fixture laid out along a single axis would give
+	 *       an empty grid with working clouds.
+	 * @note Node 1 also carries empty cells, which is what the octomap reports as free
+	 *       space; they do not reach the ground/obstacle clouds.
+	 * @note The two nodes deliberately arrive differently: node 1 with a ready-made local
+	 *       grid, node 2 with a raw scan MapsManager has to segment itself. Both branches
+	 *       of updateMapCaches() are therefore exercised by every test below.
+	 */
+	std::map<int, rtabmap::Signature> scene()
+	{
+		std::map<int, rtabmap::Signature> signatures;
+		signatures.insert(std::make_pair(1, makeGridSignature(1, poseOf(1),
+				{cv::Point3f(0.5f, -0.1f, 0.0f), cv::Point3f(0.5f, 0.1f, 0.0f)},
+				{cv::Point3f(1.0f, 0.0f, 0.0f)},
+				{cv::Point3f(0.2f, -0.1f, 0.0f), cv::Point3f(0.2f, 0.1f, 0.0f)})));
+		// Node 2 hands over the raw scan instead, so MapsManager has to segment it: the
+		// point at ground height becomes a ground cell, the raised one an obstacle.
+		signatures.insert(std::make_pair(2, makeScanSignature(2, poseOf(2),
+				{cv::Point3f(0.5f, 0.1f, 0.0f),
+				 cv::Point3f(1.0f, -0.1f, kObstacleHeight)})));
+		return signatures;
+	}
+
+	static rtabmap::Transform poseOf(int id)
+	{
+		return rtabmap::Transform(2.0f * float(id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	}
+
+	static std::map<int, rtabmap::Transform> posesOfScene()
+	{
+		std::map<int, rtabmap::Transform> poses;
+		poses.insert(std::make_pair(1, poseOf(1)));
+		poses.insert(std::make_pair(2, poseOf(2)));
+		return poses;
+	}
+
+	/// Feeds the scene in and publishes it, then spins so the messages arrive.
+	void updateAndPublish(bool updateGrid = true, bool updateOctomap = false)
+	{
+		const std::map<int, rtabmap::Signature> signatures = scene();
+		const std::map<int, rtabmap::Transform> poses = posesOfScene();
+		maps_->updateMapCaches(poses, /*memory=*/0, updateGrid, updateOctomap, signatures);
+		maps_->publishMaps(poses, node_->now(), "map");
+		spinFor(std::chrono::milliseconds(100));
+	}
+
+	/// Feeds the scene in with the octomap updated, then publishes.
+	void updateAndPublishOctomap()
+	{
+		const std::map<int, rtabmap::Transform> poses = posesOfScene();
+		maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false,
+				/*updateOctomap=*/true, scene());
+		maps_->publishMaps(poses, node_->now(), "map");
+		spinFor(std::chrono::milliseconds(150));
+	}
+
+	/// Subscribes and waits until MapsManager has seen the subscription.
+	template <typename MsgT>
+	std::shared_ptr<Collector<MsgT>> collectFromMaps(const std::string & name)
+	{
+		std::shared_ptr<Collector<MsgT>> collector = collect<MsgT>(topic(name));
+		EXPECT_TRUE(waitForPublisher(collector->subscription))
+			<< "no publisher on " << topic(name);
+		EXPECT_TRUE(spinUntil([&]() { return maps_->hasSubscribers(); }))
+			<< "MapsManager never saw the subscription on " << topic(name);
+		return collector;
+	}
+
+	std::string namespace_;
+	rclcpp::Node::SharedPtr node_;
+	std::shared_ptr<rtabmap_util::MapsManager> maps_;
+};
+
+//============================================================================
+// Assembled clouds
+//============================================================================
+
+TEST_F(MapsManagerTest, AssemblesGroundAndObstacleClouds)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+
+	updateAndPublish();
+
+	ASSERT_FALSE(ground->empty()) << "no ground cloud published";
+	ASSERT_FALSE(obstacles->empty()) << "no obstacle cloud published";
+
+	EXPECT_EQ(ground->back().header.frame_id, "map");
+	EXPECT_EQ(ground->back().width * ground->back().height, 3u)
+		<< "two ground cells from node 1 and one from node 2";
+	EXPECT_EQ(obstacles->back().width * obstacles->back().height, 2u);
+
+	// The cells are stored in each node's own frame and placed by its pose.
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.0f, 0.0f)))
+		<< "node 1 sits at the origin";
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(3.0f, -0.1f, kObstacleHeight)))
+		<< "node 2 sits 2 m along x, so its obstacle lands at 3 m";
+}
+
+TEST_F(MapsManagerTest, ColorsGroundGreenAndObstaclesRed)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+
+	updateAndPublish();
+	ASSERT_FALSE(ground->empty());
+	ASSERT_FALSE(obstacles->empty());
+
+	EXPECT_EQ(colorAt(ground->back(), 0), cv::Vec3b(0, 255, 0));
+	EXPECT_EQ(colorAt(obstacles->back(), 0), cv::Vec3b(255, 0, 0));
+}
+
+TEST_F(MapsManagerTest, CloudMapCombinesGroundAndObstacles)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloudMap =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_map");
+
+	updateAndPublish();
+
+	ASSERT_FALSE(cloudMap->empty()) << "no cloud map published";
+	EXPECT_EQ(cloudMap->back().width * cloudMap->back().height, 5u)
+		<< "three ground cells plus two obstacles";
+	EXPECT_TRUE(containsPoint(cloudMap->back(), cv::Point3f(1.0f, 0.0f, 0.0f)));
+	EXPECT_TRUE(containsPoint(cloudMap->back(), cv::Point3f(2.5f, 0.1f, 0.0f)))
+		<< "node 2's ground cell";
+}
+
+TEST_F(MapsManagerTest, RegeneratesLocalGridsFromARawScan)
+{
+	// The branch of updateMapCaches() where the sensor data has no local grid, so
+	// LocalGridMaker builds one. Two scan points, split by height alone.
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+
+	std::map<int, rtabmap::Signature> signatures;
+	signatures.insert(std::make_pair(1, makeScanSignature(1, rtabmap::Transform::getIdentity(),
+			{cv::Point3f(0.5f, -0.1f, 0.0f),
+			 cv::Point3f(0.5f, 0.1f, kObstacleHeight)})));
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity()));
+
+	maps_->updateMapCaches(poses, /*memory=*/0, true, false, signatures);
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+
+	ASSERT_FALSE(ground->empty()) << "no ground cloud published";
+	ASSERT_FALSE(obstacles->empty()) << "no obstacle cloud published";
+	EXPECT_EQ(ground->back().width * ground->back().height, 1u)
+		<< "the point at ground height";
+	EXPECT_EQ(obstacles->back().width * obstacles->back().height, 1u)
+		<< "the raised point";
+	// The cells are snapped to the grid, so they land within a cell of the scan points.
+	EXPECT_TRUE(containsPoint(ground->back(), cv::Point3f(0.5f, -0.1f, 0.0f), kCellSize));
+	EXPECT_TRUE(containsPoint(obstacles->back(),
+			cv::Point3f(0.5f, 0.1f, kObstacleHeight), kCellSize));
+}
+
+TEST_F(MapsManagerTest, TheGroundHeightDecidesWhatIsAnObstacle)
+{
+	// Same scan, but with the threshold lifted above the raised point: it is ground now,
+	// which is what shows the height passthrough is doing the segmenting.
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(
+			rtabmap::Parameters::kGridMaxGroundHeight(), "1.0"));
+	start({}, parameters);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_obstacles");
+
+	std::map<int, rtabmap::Signature> signatures;
+	signatures.insert(std::make_pair(1, makeScanSignature(1, rtabmap::Transform::getIdentity(),
+			{cv::Point3f(0.5f, -0.1f, 0.0f),
+			 cv::Point3f(0.5f, 0.1f, kObstacleHeight)})));
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity()));
+
+	maps_->updateMapCaches(poses, /*memory=*/0, true, false, signatures);
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+
+	ASSERT_FALSE(ground->empty());
+	ASSERT_FALSE(obstacles->empty());
+	EXPECT_EQ(ground->back().width * ground->back().height, 2u)
+		<< "both points are below the raised threshold";
+	EXPECT_EQ(obstacles->back().width * obstacles->back().height, 0u);
+}
+
+//============================================================================
+// Occupancy grid
+//============================================================================
+
+TEST_F(MapsManagerTest, PublishesTheOccupancyGrid)
+{
+	start();
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collectFromMaps<nav_msgs::msg::OccupancyGrid>("map");
+
+	updateAndPublish();
+
+	ASSERT_FALSE(grid->empty()) << "no occupancy grid published";
+	const nav_msgs::msg::OccupancyGrid & map = grid->back();
+	EXPECT_EQ(map.header.frame_id, "map");
+	EXPECT_NEAR(map.info.resolution, kCellSize, 1e-6);
+	EXPECT_GT(map.info.width, 0u);
+
+	// The map must agree with what getGridMap() hands out.
+	float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f;
+	const cv::Mat pixels = maps_->getGridMap(xMin, yMin, cellSize);
+	EXPECT_NEAR(map.info.origin.position.x, xMin, 1e-6);
+	EXPECT_NEAR(map.info.origin.position.y, yMin, 1e-6);
+	EXPECT_NEAR(cellSize, kCellSize, 1e-6);
+	EXPECT_EQ(map.info.width, uint32_t(pixels.cols));
+	EXPECT_EQ(map.info.height, uint32_t(pixels.rows));
+
+	// Obstacles are occupied, ground is free.
+	EXPECT_EQ(cellAt(map, 1.0, 0.0), 100) << "node 1's obstacle";
+	EXPECT_EQ(cellAt(map, 3.0, -0.1), 100) << "node 2's obstacle";
+	EXPECT_EQ(cellAt(map, 0.5, -0.1), 0) << "node 1's ground";
+}
+
+TEST_F(MapsManagerTest, CellSizeParameterChangesTheResolution)
+{
+	rtabmap::ParametersMap parameters;
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGridCellSize(), "0.1"));
+	start({}, parameters);
+
+	float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f;
+	maps_->getGridMap(xMin, yMin, cellSize);
+	EXPECT_NEAR(cellSize, 0.1f, 1e-6) << "setParameters must reach the occupancy grid";
+}
+
+TEST_F(MapsManagerTest, GridProbMapUsesProbabilities)
+{
+	start();
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collectFromMaps<nav_msgs::msg::OccupancyGrid>("grid_prob_map");
+
+	updateAndPublish();
+
+	ASSERT_FALSE(grid->empty()) << "no probability grid published";
+	EXPECT_NEAR(grid->back().info.resolution, kCellSize, 1e-6);
+	// The probability map reports 0..100 instead of the ternary free/occupied/unknown.
+	EXPECT_GT(cellAt(grid->back(), 1.0, 0.0), 50) << "the obstacle cell is likely occupied";
+}
+
+//============================================================================
+// Poses
+//============================================================================
+
+TEST_F(MapsManagerTest, KeepsEveryPoseWithoutAFilterRadius)
+{
+	start();
+	std::map<int, rtabmap::Transform> poses;
+	for(int id=1; id<=4; ++id)
+	{
+		poses.insert(std::make_pair(id, rtabmap::Transform(0.1f*float(id), 0, 0, 0, 0, 0)));
+	}
+	EXPECT_EQ(maps_->getFilteredPoses(poses).size(), poses.size())
+		<< "map_filter_radius defaults to 0, which disables the filter";
+}
+
+TEST_F(MapsManagerTest, FilterRadiusThinsNearbyPoses)
+{
+	start({rclcpp::Parameter("map_filter_radius", 1.0)});
+	std::map<int, rtabmap::Transform> poses;
+	for(int id=1; id<=4; ++id)
+	{
+		// All within a meter of each other, and all facing the same way.
+		poses.insert(std::make_pair(id, rtabmap::Transform(0.1f*float(id), 0, 0, 0, 0, 0)));
+	}
+	EXPECT_LT(maps_->getFilteredPoses(poses).size(), poses.size())
+		<< "poses closer than the radius must be dropped";
+	EXPECT_GE(maps_->getFilteredPoses(poses).size(), 1u);
+}
+
+TEST_F(MapsManagerTest, DropsTheLatestPoseUnlessAlwaysUpdating)
+{
+	// Pose 0 is the "current" node, not yet in the graph. It is only mapped when
+	// map_always_update is set, otherwise the map only shows committed nodes.
+	start({rclcpp::Parameter("map_empty_ray_tracing", false)});
+
+	std::map<int, rtabmap::Signature> signatures = scene();
+	signatures.insert(std::make_pair(0, makeGridSignature(0, rtabmap::Transform(),
+			{}, {cv::Point3f(9.0f, 0.0f, 0.0f)})));
+	std::map<int, rtabmap::Transform> poses = posesOfScene();
+	poses.insert(std::make_pair(0, rtabmap::Transform::getIdentity()));
+
+	const std::map<int, rtabmap::Transform> filtered =
+			maps_->updateMapCaches(poses, 0, true, false, signatures);
+	EXPECT_EQ(filtered.find(0), filtered.end()) << "node 0 must be dropped by default";
+	EXPECT_EQ(filtered.size(), 2u);
+}
+
+TEST_F(MapsManagerTest, KeepsTheLatestPoseWhenAlwaysUpdating)
+{
+	start({rclcpp::Parameter("map_always_update", true),
+		   rclcpp::Parameter("map_empty_ray_tracing", false)});
+
+	std::map<int, rtabmap::Signature> signatures = scene();
+	signatures.insert(std::make_pair(0, makeGridSignature(0, rtabmap::Transform(),
+			{}, {cv::Point3f(9.0f, 0.0f, 0.0f)})));
+	std::map<int, rtabmap::Transform> poses = posesOfScene();
+	poses.insert(std::make_pair(0, rtabmap::Transform::getIdentity()));
+
+	const std::map<int, rtabmap::Transform> filtered =
+			maps_->updateMapCaches(poses, 0, true, false, signatures);
+	EXPECT_NE(filtered.find(0), filtered.end()) << "node 0 must be kept";
+	EXPECT_EQ(filtered.size(), 3u);
+}
+
+TEST_F(MapsManagerTest, IgnoresLandmarkPoses)
+{
+	// Landmarks use negative ids and have no grid to contribute.
+	start();
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(-5, rtabmap::Transform::getIdentity()));
+	poses.insert(std::make_pair(1, poseOf(1)));
+	poses.insert(std::make_pair(2, poseOf(2)));
+
+	const std::map<int, rtabmap::Transform> filtered =
+			maps_->updateMapCaches(poses, 0, true, false, scene());
+	EXPECT_EQ(filtered.find(-5), filtered.end());
+	EXPECT_EQ(filtered.size(), 2u);
+}
+
+TEST_F(MapsManagerTest, RefusesEmptyPoses)
+{
+	start();
+	EXPECT_TRUE(maps_->updateMapCaches(std::map<int, rtabmap::Transform>(), 0, true, false,
+			scene()).empty());
+}
+
+TEST_F(MapsManagerTest, RefusesWithoutMemoryOrSignatures)
+{
+	start();
+	EXPECT_TRUE(maps_->updateMapCaches(posesOfScene(), 0, true, false,
+			std::map<int, rtabmap::Signature>()).empty());
+}
+
+//============================================================================
+// Subscriber bookkeeping
+//============================================================================
+
+TEST_F(MapsManagerTest, HasNoSubscribersOnItsOwn)
+{
+	start();
+	spinFor(std::chrono::milliseconds(100));
+	EXPECT_FALSE(maps_->hasSubscribers());
+}
+
+TEST_F(MapsManagerTest, HasSubscribersOnceSomeoneListens)
+{
+	start();
+	collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_map");
+	EXPECT_TRUE(maps_->hasSubscribers());
+}
+
+TEST_F(MapsManagerTest, AssumesTheMapChangedWithoutGridSubscribers)
+{
+	// Whether the map changed is only known from OccupancyGrid::update(), which is only
+	// run when someone wants a grid. With nobody listening the answer is assumed true.
+	start();
+	spinFor(std::chrono::milliseconds(100));
+	EXPECT_TRUE(maps_->isMapUpdated());
+}
+
+TEST_F(MapsManagerTest, ReportsTheMapUnchangedOnASecondIdenticalUpdate)
+{
+	start();
+	collectFromMaps<nav_msgs::msg::OccupancyGrid>("map");
+
+	maps_->updateMapCaches(posesOfScene(), 0, true, false, scene());
+	EXPECT_TRUE(maps_->isMapUpdated()) << "the first update adds both nodes";
+
+	maps_->updateMapCaches(posesOfScene(), 0, true, false, scene());
+	EXPECT_FALSE(maps_->isMapUpdated()) << "nothing moved and nothing was added";
+}
+
+TEST_F(MapsManagerTest, PublishesNothingWithoutSubscribers)
+{
+	start();
+	maps_->updateMapCaches(posesOfScene(), 0, true, false, scene());
+	maps_->publishMaps(posesOfScene(), node_->now(), "map");
+
+	// Subscribing afterwards with a volatile subscription sees nothing.
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collect<sensor_msgs::msg::PointCloud2>(topic("cloud_map"));
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(cloud->empty());
+}
+
+//============================================================================
+// Latching
+//============================================================================
+
+TEST_F(MapsManagerTest, LatchesTheMapForLateSubscribers)
+{
+	start();   // latch defaults to true
+	EXPECT_TRUE(maps_->isLatching());
+	collectFromMaps<nav_msgs::msg::OccupancyGrid>("map");
+	updateAndPublish();
+
+	// A subscriber joining after the fact still gets the last map, because the publisher
+	// is transient local.
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> late =
+			collect<nav_msgs::msg::OccupancyGrid>(topic("map"),
+					rclcpp::QoS(1).reliable().transient_local());
+	EXPECT_TRUE(spinUntil([&]() { return !late->empty(); }))
+		<< "the latched map was not delivered";
+}
+
+TEST_F(MapsManagerTest, DoesNotLatchWhenLatchIsFalse)
+{
+	start({rclcpp::Parameter("latch", false)});
+	EXPECT_FALSE(maps_->isLatching());
+	collectFromMaps<nav_msgs::msg::OccupancyGrid>("map");
+	updateAndPublish();
+
+	// With a volatile publisher there is no history to hand out (and a transient local
+	// subscription is not even compatible), so a late subscriber gets nothing.
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> late =
+			collect<nav_msgs::msg::OccupancyGrid>(topic("map"),
+					rclcpp::QoS(1).reliable().transient_local());
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(late->empty());
+}
+
+//============================================================================
+// Caches
+//============================================================================
+
+TEST_F(MapsManagerTest, ClearEmptiesTheAssembledClouds)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> cloud =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("cloud_map");
+	updateAndPublish();
+	ASSERT_FALSE(cloud->empty());
+	ASSERT_GT(cloud->back().width * cloud->back().height, 0u);
+
+	maps_->clear();
+	maps_->publishMaps(posesOfScene(), node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+
+	EXPECT_EQ(cloud->back().width * cloud->back().height, 0u)
+		<< "clear() must drop the cached grids, leaving nothing to assemble";
+}
+
+TEST_F(MapsManagerTest, Set2DMapInstallsAGridDirectly)
+{
+	// Used when a map comes back from the database rather than from local grids.
+	start();
+	cv::Mat map(4, 6, CV_8SC1, cv::Scalar(-1));
+	map.at<int8_t>(2, 3) = 100;
+	map.at<int8_t>(1, 1) = 0;
+
+	// The poses are not optional: set2DMap() keeps the map only when it is told which
+	// nodes it was assembled from.
+	maps_->set2DMap(map, /*xMin=*/-1.0f, /*yMin=*/-0.5f, kCellSize, posesOfScene());
+
+	float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f;
+	const cv::Mat out = maps_->getGridMap(xMin, yMin, cellSize);
+	ASSERT_FALSE(out.empty());
+	EXPECT_EQ(out.cols, 6);
+	EXPECT_EQ(out.rows, 4);
+	EXPECT_NEAR(xMin, -1.0f, 1e-6);
+	EXPECT_NEAR(yMin, -0.5f, 1e-6);
+	EXPECT_NEAR(cellSize, kCellSize, 1e-6);
+	EXPECT_EQ(out.at<int8_t>(2, 3), 100);
+	EXPECT_EQ(out.at<int8_t>(1, 1), 0);
+}
+
+//============================================================================
+// Parameters that moved to the rtabmap library
+//============================================================================
+
+TEST_F(MapsManagerTest, Set2DMapNeedsThePosesTheMapCameFrom)
+{
+	// The grid is kept only together with the poses it was assembled from, so that it
+	// knows which nodes are already in it. Without them the map is dropped, and
+	// MapsManager warns rather than leaving the caller to wonder.
+	start();
+	cv::Mat map(4, 6, CV_8SC1, cv::Scalar(-1));
+	map.at<int8_t>(2, 3) = 100;
+
+	maps_->set2DMap(map, -1.0f, -0.5f, kCellSize, std::map<int, rtabmap::Transform>());
+
+	float xMin = 0.0f, yMin = 0.0f, cellSize = 0.0f;
+	EXPECT_TRUE(maps_->getGridMap(xMin, yMin, cellSize).empty());
+}
+
+TEST_F(MapsManagerTest, CopiesMovedParametersToTheirNewNames)
+{
+	start();
+	node_->declare_parameter("grid_cell_size", 0.1);
+	node_->declare_parameter("proj_max_ground_height", 0.3);
+
+	rtabmap::ParametersMap parameters;
+	maps_->backwardCompatibilityParameters(*node_, parameters);
+
+	ASSERT_TRUE(parameters.find(rtabmap::Parameters::kGridCellSize()) != parameters.end())
+		<< "grid_cell_size must be copied to " << rtabmap::Parameters::kGridCellSize();
+	EXPECT_NEAR(uStr2Float(parameters.at(rtabmap::Parameters::kGridCellSize())), 0.1f, 1e-6);
+
+	ASSERT_TRUE(parameters.find(rtabmap::Parameters::kGridMaxGroundHeight()) != parameters.end());
+	EXPECT_NEAR(uStr2Float(parameters.at(rtabmap::Parameters::kGridMaxGroundHeight())), 0.3f, 1e-6);
+}
+
+TEST_F(MapsManagerTest, LeavesUnsetLegacyParametersAlone)
+{
+	start();
+	rtabmap::ParametersMap parameters;
+	maps_->backwardCompatibilityParameters(*node_, parameters);
+	EXPECT_TRUE(parameters.empty()) << "nothing was declared, so nothing should be copied";
+}
+
+//============================================================================
+// Octomap
+//============================================================================
+
+#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
+
+TEST_F(MapsManagerTest, PublishesTheBinaryOctomap)
+{
+	start();
+	std::shared_ptr<Collector<octomap_msgs::msg::Octomap>> binary =
+			collectFromMaps<octomap_msgs::msg::Octomap>("octomap_binary");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(binary->empty()) << "no binary octomap published";
+	EXPECT_EQ(binary->back().header.frame_id, "map");
+	EXPECT_TRUE(binary->back().binary);
+	EXPECT_EQ(binary->back().id, "ColorOcTree")
+		<< "rtabmap keeps a color per voxel, so the tree type is not a plain OcTree";
+	EXPECT_NEAR(binary->back().resolution, kCellSize, 1e-6);
+	EXPECT_FALSE(binary->back().data.empty()) << "the serialized tree must not be empty";
+}
+
+TEST_F(MapsManagerTest, PublishesTheFullOctomap)
+{
+	start();
+	std::shared_ptr<Collector<octomap_msgs::msg::Octomap>> full =
+			collectFromMaps<octomap_msgs::msg::Octomap>("octomap_full");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(full->empty()) << "no full octomap published";
+	EXPECT_FALSE(full->back().binary) << "the full tree carries occupancy probabilities";
+	EXPECT_EQ(full->back().id, "ColorOcTree")
+		<< "consumers deserialize on this id, so both messages must report the same type";
+	EXPECT_NEAR(full->back().resolution, kCellSize, 1e-6);
+	EXPECT_FALSE(full->back().data.empty());
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapOccupiedSpace)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> occupied =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_occupied_space");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(occupied->empty()) << "no octomap cloud published";
+	EXPECT_EQ(occupied->back().header.frame_id, "map");
+	EXPECT_EQ(occupied->back().width * occupied->back().height, 5u)
+		<< "occupied space is the obstacles plus the ground: 2 + 3 cells";
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapObstacles)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_obstacles");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(obstacles->empty()) << "no octomap obstacles published";
+	EXPECT_EQ(obstacles->back().width * obstacles->back().height, 2u);
+	// Points come back at voxel centers, up to half a cell from where they went in.
+	EXPECT_TRUE(containsPoint(obstacles->back(), cv::Point3f(1.0f, 0.0f, 0.0f), kCellSize));
+	EXPECT_TRUE(containsPoint(obstacles->back(),
+			cv::Point3f(3.0f, -0.1f, kObstacleHeight), kCellSize));
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapGround)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_ground");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(ground->empty()) << "no octomap ground published";
+	EXPECT_EQ(ground->back().width * ground->back().height, 3u)
+		<< "the ground cells only, not the empty ones";
+	EXPECT_TRUE(containsPoint(ground->back(), cv::Point3f(0.5f, -0.1f, 0.0f), kCellSize));
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapEmptySpace)
+{
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> empty =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_empty_space");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(empty->empty()) << "no octomap empty space published";
+	EXPECT_EQ(empty->back().header.frame_id, "map");
+	EXPECT_EQ(empty->back().width * empty->back().height, 2u)
+		<< "node 1's two empty cells, and nothing else: ground cells are stored as "
+		   "occupied nodes flagged as ground, so they are not free space";
+	// createCloud() reports x and y at the cell corner but z at the cell center.
+	EXPECT_TRUE(containsPoint(empty->back(),
+			cv::Point3f(0.2f, -0.1f, 0.5f*kCellSize), 1e-3f));
+	EXPECT_TRUE(containsPoint(empty->back(),
+			cv::Point3f(0.2f, 0.1f, 0.5f*kCellSize), 1e-3f));
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapFrontier)
+{
+	// A frontier cell is a free cell with at least one unknown face neighbor. Nothing
+	// encloses this scene, so the frontier is exactly the free space: node 1's two empty
+	// cells. The ground and obstacle cells are occupied nodes and never qualify.
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> frontier =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(frontier->empty()) << "no octomap frontier published";
+	EXPECT_EQ(frontier->back().header.frame_id, "map");
+	EXPECT_EQ(frontier->back().width * frontier->back().height, 2u);
+	EXPECT_TRUE(containsPoint(frontier->back(),
+			cv::Point3f(0.2f, -0.1f, 0.5f*kCellSize), 1e-3f));
+	EXPECT_TRUE(containsPoint(frontier->back(),
+			cv::Point3f(0.2f, 0.1f, 0.5f*kCellSize), 1e-3f));
+}
+
+TEST_F(MapsManagerTest, AnEnclosedEmptyCellIsNotAFrontier)
+{
+	// The frontier rule in one scene: two identical empty cells, one walled in on all six
+	// faces by obstacles and one out in the open. Both are free space, but only the open
+	// one has an unknown neighbor, so only it is a frontier.
+	start();
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> frontier =
+			collectFromMaps<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> empty =
+			collect<sensor_msgs::msg::PointCloud2>(topic("octomap_empty_space"));
+	ASSERT_TRUE(waitForPublisher(empty->subscription));
+
+	const cv::Point3f enclosed = voxelCenter(19, 0, 9);
+	const cv::Point3f open = voxelCenter(39, 0, 9);
+
+	std::map<int, rtabmap::Signature> signatures;
+	signatures.insert(std::make_pair(1, makeGridSignature(1, rtabmap::Transform::getIdentity(),
+			/*ground=*/{},
+			/*obstacles=*/{voxelCenter(18, 0, 9), voxelCenter(20, 0, 9),    // -x, +x
+						   voxelCenter(19, -1, 9), voxelCenter(19, 1, 9),   // -y, +y
+						   voxelCenter(19, 0, 8), voxelCenter(19, 0, 10)},  // -z, +z
+			/*empty=*/{enclosed, open})));
+	std::map<int, rtabmap::Transform> poses;
+	poses.insert(std::make_pair(1, rtabmap::Transform::getIdentity()));
+
+	maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false, /*updateOctomap=*/true,
+			signatures);
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(200));
+
+	ASSERT_FALSE(empty->empty()) << "no octomap empty space published";
+	ASSERT_FALSE(frontier->empty()) << "no octomap frontier published";
+
+	// Both cells are free space...
+	EXPECT_EQ(empty->back().width * empty->back().height, 2u);
+	EXPECT_TRUE(containsPoint(empty->back(), asReported(enclosed), 1e-3f));
+	EXPECT_TRUE(containsPoint(empty->back(), asReported(open), 1e-3f));
+
+	// ...but the walled-in one is not on the frontier.
+	EXPECT_EQ(frontier->back().width * frontier->back().height, 1u);
+	EXPECT_TRUE(containsPoint(frontier->back(), asReported(open), 1e-3f))
+		<< "the open cell borders unknown space";
+	EXPECT_FALSE(containsPoint(frontier->back(), asReported(enclosed), 1e-3f))
+		<< "all six face neighbors of the enclosed cell are known, so it is not a frontier";
+}
+
+TEST_F(MapsManagerTest, PublishesTheOctomapGrid)
+{
+	start();
+	std::shared_ptr<Collector<nav_msgs::msg::OccupancyGrid>> grid =
+			collectFromMaps<nav_msgs::msg::OccupancyGrid>("octomap_grid");
+
+	updateAndPublishOctomap();
+
+	ASSERT_FALSE(grid->empty()) << "no octomap grid published";
+	const nav_msgs::msg::OccupancyGrid & map = grid->back();
+	EXPECT_EQ(map.header.frame_id, "map");
+	EXPECT_EQ(countCells(map, 100), 2) << "one occupied cell per obstacle";
+	EXPECT_TRUE(hasValueNear(map, 1.0, 0.0, 100)) << "node 1's obstacle";
+	EXPECT_TRUE(hasValueNear(map, 3.0, -0.1, 100)) << "node 2's obstacle";
+	EXPECT_TRUE(hasValueNear(map, 0.5, -0.1, 0)) << "node 1's ground is free space";
+}
+
+TEST_F(MapsManagerTest, ExposesTheOctomap)
+{
+	start();
+	ASSERT_NE(maps_->getOctomap(), nullptr);
+}
+#endif
+
+//============================================================================
+// Elevation map (grid_map)
+//============================================================================
+
+#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
+TEST_F(MapsManagerTest, PublishesTheElevationMap)
+{
+	// updateMapCaches() has no explicit flag for the elevation map: it is only built
+	// through the "nothing requested, so follow the subscribers" path.
+	start();
+	std::shared_ptr<Collector<grid_map_msgs::msg::GridMap>> elevation =
+			collectFromMaps<grid_map_msgs::msg::GridMap>("elevation_map");
+
+	const std::map<int, rtabmap::Transform> poses = posesOfScene();
+	maps_->updateMapCaches(poses, /*memory=*/0, /*updateGrid=*/false, /*updateOctomap=*/false,
+			scene());
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+
+	ASSERT_FALSE(elevation->empty()) << "no elevation map published";
+	const grid_map_msgs::msg::GridMap & msg = elevation->back();
+	EXPECT_EQ(msg.header.frame_id, "map");
+	EXPECT_NEAR(msg.info.resolution, kCellSize, 1e-6);
+	EXPECT_GT(msg.info.length_x, 0.0);
+	EXPECT_GT(msg.info.length_y, 0.0);
+
+	ASSERT_FALSE(msg.layers.empty()) << "the grid map must carry its layers";
+	EXPECT_NE(std::find(msg.layers.begin(), msg.layers.end(), "elevation"), msg.layers.end())
+		<< "the elevation layer is what makes this an elevation map";
+	EXPECT_EQ(msg.data.size(), msg.layers.size()) << "one data matrix per layer";
+}
+
+TEST_F(MapsManagerTest, DoesNotRepublishAnUnchangedElevationMap)
+{
+	// Like every other map, once latched it should stay put until something changes.
+	start();
+	std::shared_ptr<Collector<grid_map_msgs::msg::GridMap>> elevation =
+			collectFromMaps<grid_map_msgs::msg::GridMap>("elevation_map");
+
+	const std::map<int, rtabmap::Transform> poses = posesOfScene();
+	maps_->updateMapCaches(poses, 0, false, false, scene());
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+	ASSERT_FALSE(elevation->empty());
+	const size_t afterFirst = elevation->size();
+
+	// Nothing new to assemble, so nothing to send.
+	maps_->updateMapCaches(poses, 0, false, false, scene());
+	maps_->publishMaps(poses, node_->now(), "map");
+	spinFor(std::chrono::milliseconds(150));
+
+	EXPECT_EQ(elevation->size(), afterFirst)
+		<< "the latched elevation map was republished unchanged";
+}
+#endif
+
+TEST_F(MapsManagerTest, ExposesTheOccupancyGridAndLocalMapMaker)
+{
+	start();
+	ASSERT_NE(maps_->getOccupancyGrid(), nullptr);
+	ASSERT_NE(maps_->getLocalMapMaker(), nullptr);
+	EXPECT_NEAR(maps_->getOccupancyGrid()->getCellSize(), kCellSize, 1e-6);
+}
+
diff --git a/rtabmap_util/test/test_obstacles_detection.cpp b/rtabmap_util/test/test_obstacles_detection.cpp
new file mode 100644
index 00000000..50c9e499
--- /dev/null
+++ b/rtabmap_util/test/test_obstacles_detection.cpp
@@ -0,0 +1,423 @@
+/*
+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_util/obstacles_detection.hpp>
+
+#include <cmath>
+#include <limits>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+/**
+ * @brief A dense ground plane at z=0 plus a vertical wall in front of it.
+ *
+ * The 0.05 m spacing matters: the segmentation clusters points with
+ * Grid/ClusterRadius (0.1 m by default) and drops clusters below
+ * Grid/MinClusterSize (10), so a sparser cloud is discarded entirely.
+ */
+std::vector<cv::Point3f> groundAndWall()
+{
+	std::vector<cv::Point3f> points;
+	for(int i=0; i<=20; ++i)          // ground: 1 m x 1 m at z=0
+	{
+		for(int j=0; j<=20; ++j)
+		{
+			points.push_back(cv::Point3f(0.3f + 0.05f*i, -0.5f + 0.05f*j, 0.0f));
+		}
+	}
+	for(int j=0; j<=20; ++j)          // wall: vertical, 1 m wide, 0.75 m tall
+	{
+		for(int k=1; k<=15; ++k)
+		{
+			points.push_back(cv::Point3f(1.4f, -0.5f + 0.05f*j, 0.05f*k));
+		}
+	}
+	return points;
+}
+/**
+ * @brief A plane that is horizontal in the map frame, given a base frame pitched by
+ *        @p pitch. In the base frame it therefore rises with x: z = x * tan(pitch).
+ */
+std::vector<cv::Point3f> planeLevelInMapFrame(double pitch)
+{
+	std::vector<cv::Point3f> points;
+	for(int i=0; i<=20; ++i)
+	{
+		const float x = 0.8f + 0.05f*i;
+		for(int j=0; j<=20; ++j)
+		{
+			points.push_back(cv::Point3f(x, -0.5f + 0.05f*j, x * float(std::tan(pitch))));
+		}
+	}
+	return points;
+}
+/// Two flat 25-point patches: one about 0.5 m from the sensor, one about 3 m away.
+std::vector<cv::Point3f> nearAndFarPatches()
+{
+	std::vector<cv::Point3f> points;
+	for(int i=0; i<5; ++i)
+	{
+		for(int j=0; j<5; ++j)
+		{
+			points.push_back(cv::Point3f(0.4f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
+			points.push_back(cv::Point3f(2.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
+		}
+	}
+	return points;
+}
+
+/// Smallest and largest x in a cloud, to tell the near patch from the far one.
+std::pair<float, float> xExtent(const sensor_msgs::msg::PointCloud2 & cloud)
+{
+	float lo = std::numeric_limits<float>::max();
+	float hi = -std::numeric_limits<float>::max();
+	for(size_t i=0; i<cloud.width*cloud.height; ++i)
+	{
+		const float x = readXYZ(cloud, i).x;
+		lo = std::min(lo, x);
+		hi = std::max(hi, x);
+	}
+	return std::make_pair(lo, hi);
+}
+}  // namespace
+
+class ObstaclesDetectionTest : public NodeTest {};
+
+TEST_F(ObstaclesDetectionTest, SeparatesGroundFromObstacles)
+{
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform", 0.1)})));
+	publishStaticTf("base_link", "lidar");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
+		<< "both ground and obstacles must be published";
+
+	EXPECT_GT(ground->back().width, 0u) << "the flat points must be classified as ground";
+	EXPECT_GT(obstacles->back().width, 0u) << "the wall must be classified as obstacles";
+	// The clouds are transformed back into the frame of the input topic, not frame_id.
+	EXPECT_EQ(ground->back().header.frame_id, "lidar");
+	EXPECT_EQ(obstacles->back().header.frame_id, "lidar");
+}
+
+TEST_F(ObstaclesDetectionTest, ProjectsObstaclesOntoTheGroundPlane)
+{
+	// proj_obstacles is the obstacles cloud flattened to z=0, with flat surfaces removed.
+	// Note it is published in frame_id, unlike ground/obstacles which keep the input frame.
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform", 0.1)})));
+	publishStaticTf("base_link", "lidar");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> proj =
+			collect<sensor_msgs::msg::PointCloud2>("proj_obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(proj->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
+	ASSERT_TRUE(spinUntil([&]() { return !proj->empty(); }));
+
+	const sensor_msgs::msg::PointCloud2 & cloud = proj->back();
+	ASSERT_GT(cloud.width, 0u) << "the wall must survive as a projected obstacle";
+	EXPECT_EQ(cloud.header.frame_id, "base_link")
+		<< "proj_obstacles uses frame_id, not the input frame";
+
+	for(size_t i=0; i<cloud.width; ++i)
+	{
+		EXPECT_NEAR(readXYZ(cloud, i).z, 0.0f, 1e-6)
+			<< "every projected point is flattened to z=0, point " << i;
+	}
+}
+
+TEST_F(ObstaclesDetectionTest, HeightSegmentationIsRelativeToTheBaseFrameByDefault)
+{
+	// With normals segmentation off the split is a plain height threshold. Without a
+	// map frame the heights are those of the cloud in the base frame, so the flat points
+	// at z=0 fall below Grid/MaxGroundHeight and are ground.
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("wait_for_transform", 0.1),
+				rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+				rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
+	publishStaticTf("base_link", "lidar");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+
+	EXPECT_GT(ground->back().width, 0u) << "the z=0 plane is below the 0.2 m threshold";
+	EXPECT_GT(obstacles->back().width, 0u) << "the wall rises above it";
+}
+
+TEST_F(ObstaclesDetectionTest, MapFrameIdAloneDoesNotMoveTheHeightReference)
+{
+	// The robot sits 1 m above the map origin, but Grid/MapFrameProjection is false by
+	// default, so pose.z() is ignored and the heights stay relative to the base frame.
+	// Setting map_frame_id on its own therefore changes nothing here.
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("map_frame_id", "map"),
+				rclcpp::Parameter("wait_for_transform", 0.1),
+				rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+				rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
+	publishStaticTf("base_link", "lidar");
+	publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+
+	EXPECT_GT(ground->back().width, 0u)
+		<< "without Grid/MapFrameProjection the map height is not applied";
+}
+
+TEST_F(ObstaclesDetectionTest, MapFrameIdLevelsTheGroundUsingRollAndPitch)
+{
+	// Only pose.z() is gated by Grid/MapFrameProjection: roll and pitch are always
+	// applied. So map_frame_id on its own still levels the segmentation to the map's
+	// horizontal, which is what matters when the robot is on a slope.
+	const double pitch = 10.0 * M_PI / 180.0;
+
+	// A plane that is level in the map frame, seen from a base frame pitched by 10 deg:
+	// in the base frame it rises to well above the 0.1 m ground threshold.
+	const std::vector<cv::Point3f> plane = planeLevelInMapFrame(pitch);
+	ASSERT_GT(plane.back().z, 0.1f) << "precondition: tilted beyond the threshold";
+
+	// Without a map frame the tilt is taken at face value: not ground.
+	{
+		addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+				.parameter_overrides({
+					rclcpp::Parameter("frame_id", "base_link"),
+					rclcpp::Parameter("wait_for_transform", 0.1),
+					rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+					rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
+		publishStaticTf("base_link", "lidar");
+
+		std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+				collect<sensor_msgs::msg::PointCloud2>("ground");
+		std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+				collect<sensor_msgs::msg::PointCloud2>("obstacles");
+		rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+				helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		ASSERT_TRUE(waitForSubscriber(pub));
+		ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+		pub->publish(makeXYZCloud("lidar", 1000.0, plane));
+		ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+		EXPECT_EQ(ground->back().width, 0u)
+			<< "a slope read in the base frame is not ground";
+	}
+}
+
+TEST_F(ObstaclesDetectionTest, MapFrameIdRecoversTheGroundOnASlope)
+{
+	// Same tilted plane, but now the node knows the robot is pitched in the map frame,
+	// so it levels the cloud and the slope becomes ground again.
+	const double pitch = 10.0 * M_PI / 180.0;
+
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("map_frame_id", "map"),
+				rclcpp::Parameter("wait_for_transform", 0.1),
+				rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+				rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.1"))})));
+	publishStaticTf("base_link", "lidar");
+	publishStaticTfRPY("map", "base_link", 0.0, pitch, 0.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, planeLevelInMapFrame(pitch)));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+
+	EXPECT_GT(ground->back().width, 0u)
+		<< "leveled by the map pitch, the slope is ground -- roll/pitch apply even "
+		   "though Grid/MapFrameProjection is false";
+}
+
+TEST_F(ObstaclesDetectionTest, MapFrameProjectionSegmentsRelativeToTheMap)
+{
+	// Same setup plus Grid/MapFrameProjection=true. Now pose.z() participates, the whole
+	// cloud sits 1 m up in the map frame, and nothing is below the 0.2 m ground
+	// threshold any more.
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("map_frame_id", "map"),
+				rclcpp::Parameter("wait_for_transform", 0.1),
+				rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+				rclcpp::Parameter("Grid/MapFrameProjection", std::string("true")),
+				rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2"))})));
+	publishStaticTf("base_link", "lidar");
+	publishStaticTf("map", "base_link", 0.0, 0.0, 1.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, groundAndWall()));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+
+	EXPECT_EQ(ground->back().width, 0u)
+		<< "lifted 1 m in the map frame, nothing is below the ground threshold";
+	EXPECT_GT(obstacles->back().width, 0u) << "everything becomes an obstacle instead";
+}
+
+/// Runs the node with the given Grid range settings and returns the ground cloud.
+class ObstaclesDetectionRangeTest : public NodeTest
+{
+protected:
+	sensor_msgs::msg::PointCloud2 groundWithRange(
+			const std::string & rangeMin, const std::string & rangeMax,
+			const std::vector<cv::Point3f> & points)
+	{
+		addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+				.parameter_overrides({
+					rclcpp::Parameter("frame_id", "base_link"),
+					rclcpp::Parameter("wait_for_transform", 0.1),
+					rclcpp::Parameter("Grid/NormalsSegmentation", std::string("false")),
+					rclcpp::Parameter("Grid/MaxGroundHeight", std::string("0.2")),
+					rclcpp::Parameter("Grid/RangeMin", rangeMin),
+					rclcpp::Parameter("Grid/RangeMax", rangeMax)})));
+		publishStaticTf("base_link", "lidar");
+
+		std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+				collect<sensor_msgs::msg::PointCloud2>("ground");
+		std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+				collect<sensor_msgs::msg::PointCloud2>("obstacles");
+		rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+				helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+		EXPECT_TRUE(waitForPublisher(ground->subscription));
+
+		pub->publish(makeXYZCloud("lidar", 1000.0, points));
+		EXPECT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }));
+		return ground->empty() ? sensor_msgs::msg::PointCloud2() : ground->back();
+	}
+};
+
+TEST_F(ObstaclesDetectionRangeTest, RangeFilteringDisabledKeepsEverything)
+{
+	// Grid/RangeMax=0 means no upper limit, so both patches survive.
+	const sensor_msgs::msg::PointCloud2 ground =
+			groundWithRange("0.0", "0.0", nearAndFarPatches());
+	EXPECT_EQ(ground.width, 50u);
+}
+
+TEST_F(ObstaclesDetectionRangeTest, GridRangeMaxDropsDistantPoints)
+{
+	// Only the patch inside 1 m survives.
+	const sensor_msgs::msg::PointCloud2 ground =
+			groundWithRange("0.0", "1.0", nearAndFarPatches());
+	ASSERT_EQ(ground.width, 25u);
+
+	const std::pair<float, float> extent = xExtent(ground);
+	EXPECT_NEAR(extent.first, 0.4f, 1e-3);
+	EXPECT_LT(extent.second, 1.0f) << "nothing beyond the 1 m limit may remain";
+}
+
+TEST_F(ObstaclesDetectionRangeTest, GridRangeMinDropsNearbyPoints)
+{
+	// The mirror image: everything closer than 1 m is discarded instead.
+	const sensor_msgs::msg::PointCloud2 ground =
+			groundWithRange("1.0", "0.0", nearAndFarPatches());
+	ASSERT_EQ(ground.width, 25u);
+
+	const std::pair<float, float> extent = xExtent(ground);
+	EXPECT_GT(extent.first, 1.0f) << "nothing closer than the 1 m limit may remain";
+	EXPECT_NEAR(extent.second, 3.1f, 1e-3);
+}
+
+TEST_F(ObstaclesDetectionRangeTest, DefaultRangeMaxIsFiveMeters)
+{
+	// Grid/RangeMax defaults to 5.0, not infinity: a patch at 6 m is silently dropped
+	// even though no range parameter was set.
+	std::vector<cv::Point3f> points = nearAndFarPatches();
+	for(int i=0; i<5; ++i)
+	{
+		for(int j=0; j<5; ++j)
+		{
+			points.push_back(cv::Point3f(5.9f + 0.05f*i, -0.1f + 0.05f*j, 0.0f));
+		}
+	}
+
+	const sensor_msgs::msg::PointCloud2 ground =
+			groundWithRange("0.0", "5.0", points);   // the defaults, stated explicitly
+	EXPECT_EQ(ground.width, 50u) << "the 6 m patch is beyond the default range";
+	EXPECT_LT(xExtent(ground).second, 5.0f);
+}
+
+TEST_F(ObstaclesDetectionTest, PublishesEmptyCloudsForAnEmptyInput)
+{
+	addNode(std::make_shared<rtabmap_util::ObstaclesDetection>(rclcpp::NodeOptions()
+			.parameter_overrides({rclcpp::Parameter("frame_id", "base_link")})));
+	publishStaticTf("base_link", "lidar");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> ground =
+			collect<sensor_msgs::msg::PointCloud2>("ground");
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> obstacles =
+			collect<sensor_msgs::msg::PointCloud2>("obstacles");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(ground->subscription));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, {}));
+	ASSERT_TRUE(spinUntil([&]() { return !ground->empty() && !obstacles->empty(); }))
+		<< "an empty input must still produce output, not a dropped message";
+
+	EXPECT_EQ(ground->back().width, 0u);
+	EXPECT_EQ(obstacles->back().width, 0u);
+}
diff --git a/rtabmap_util/test/test_point_cloud_aggregator.cpp b/rtabmap_util/test/test_point_cloud_aggregator.cpp
new file mode 100644
index 00000000..45469223
--- /dev/null
+++ b/rtabmap_util/test/test_point_cloud_aggregator.cpp
@@ -0,0 +1,194 @@
+/*
+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_util/point_cloud_aggregator.hpp>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+class PointCloudAggregatorTest : public NodeTest
+{
+protected:
+	/// odom -> base_link advancing along x at 1 m/s across the two cloud stamps.
+	void publishOdomMotion(double startStamp, double duration)
+	{
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+		spinFor(std::chrono::milliseconds(100));
+		for(int i=0; i<=6; ++i)
+		{
+			const double elapsed = duration * double(i) / 6.0;
+			geometry_msgs::msg::TransformStamped t;
+			t.header.stamp = stampOf(startStamp + elapsed);
+			t.header.frame_id = "odom";
+			t.child_frame_id = "base_link";
+			t.transform.translation.x = elapsed;      // 1 m/s
+			t.transform.rotation.w = 1.0;
+			tf2_msgs::msg::TFMessage msg;
+			msg.transforms.push_back(t);
+			tfPub->publish(msg);
+		}
+		spinFor(std::chrono::milliseconds(200));
+		tfPub_ = tfPub;
+	}
+
+	/**
+	 * @brief Publishes three pairs of clouds observing one landmark 5 m ahead in odom.
+	 *
+	 * Pair k is stamped at 1000.0+0.2k and 0.1 s later. The robot drives at 1 m/s, so
+	 * each sensor measures the landmark at 5 m minus the distance travelled by then.
+	 */
+	void publishPairs(
+			const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub1,
+			const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & pub2)
+	{
+		for(int k=0; k<3; ++k)
+		{
+			const double t1 = 1000.0 + 0.2*double(k);
+			const double t2 = t1 + 0.1;
+			pub1->publish(makeXYZCloud("lidar_a", t1, {{float(5.0-(t1-1000.0)), 0.0f, 0.0f}}));
+			pub2->publish(makeXYZCloud("lidar_b", t2, {{float(5.0-(t2-1000.0)), 0.0f, 0.0f}}));
+			spinFor(std::chrono::milliseconds(50));
+		}
+	}
+
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
+};
+
+TEST_F(PointCloudAggregatorTest, CombinesTwoSynchronizedClouds)
+{
+	addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("count", 2),
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("approx_sync", true),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+	publishStaticTf("base_link", "lidar_a", 0.0, 0.2, 0.0);
+	publishStaticTf("base_link", "lidar_b", 0.0, -0.2, 0.0);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
+	ASSERT_TRUE(waitForSubscriber(pub1));
+	ASSERT_TRUE(waitForSubscriber(pub2));
+
+	const std::vector<cv::Point3f> a = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
+	const std::vector<cv::Point3f> b = {{3.0f, 0.0f, 0.0f}};
+	pub1->publish(makeXYZCloud("lidar_a", 1000.0, a));
+	pub2->publish(makeXYZCloud("lidar_b", 1000.0, b));
+
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud published";
+	EXPECT_EQ(out->back().width, a.size() + b.size()) << "every input point must survive";
+	EXPECT_EQ(out->back().header.frame_id, "base_link")
+		<< "the combined cloud is expressed in frame_id";
+}
+
+TEST_F(PointCloudAggregatorTest, AlignsCloudsCapturedAtDifferentTimesWhileMoving)
+{
+	// The two sensors fire 0.1 s apart while the robot drives forward at 1 m/s, so they
+	// see the same world point at different ranges. With fixed_frame_id set, the second
+	// cloud is motion-compensated back to the first one's stamp and the two coincide.
+	addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("count", 2),
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("approx_sync", true),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+	publishStaticTf("base_link", "lidar_a");
+	publishStaticTf("base_link", "lidar_b");
+	publishOdomMotion(1000.0, 0.6);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
+	ASSERT_TRUE(waitForSubscriber(pub1));
+	ASSERT_TRUE(waitForSubscriber(pub2));
+
+	// A landmark 5 m ahead in odom, the robot driving at 1 m/s. Several pairs are sent
+	// because the ApproximateTime policy needs a following message before it can commit
+	// to a match when the stamps differ; the first emitted pair is the one asserted on.
+	publishPairs(pub1, pub2);
+
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); })) << "no combined cloud";
+	const sensor_msgs::msg::PointCloud2 & cloud = out->front();
+	ASSERT_EQ(cloud.width, 2u);
+
+	// Both observations of the same landmark must land on the same point.
+	EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
+	EXPECT_NEAR(readXYZ(cloud, 1).x, 5.0f, 5e-3)
+		<< "the later cloud must be compensated for the 0.1 m of motion";
+}
+
+TEST_F(PointCloudAggregatorTest, WithoutAFixedFrameCloudsAreNotMotionCompensated)
+{
+	// Same inputs, no fixed_frame_id: the second cloud is taken at face value and the
+	// two observations stay 0.1 m apart. This is what fixed_frame_id exists to fix.
+	addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("count", 2),
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("approx_sync", true),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+	publishStaticTf("base_link", "lidar_a");
+	publishStaticTf("base_link", "lidar_b");
+	publishOdomMotion(1000.0, 0.6);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub2 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud2", 10);
+	ASSERT_TRUE(waitForSubscriber(pub1));
+	ASSERT_TRUE(waitForSubscriber(pub2));
+
+	publishPairs(pub1, pub2);
+
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
+	const sensor_msgs::msg::PointCloud2 & cloud = out->front();
+	ASSERT_EQ(cloud.width, 2u);
+
+	EXPECT_NEAR(readXYZ(cloud, 0).x, 5.0f, 5e-3);
+	EXPECT_NEAR(readXYZ(cloud, 1).x, 4.9f, 5e-3)
+		<< "uncompensated, the second observation stays where it was measured";
+}
+
+TEST_F(PointCloudAggregatorTest, WaitsForEveryInput)
+{
+	addNode(std::make_shared<rtabmap_util::PointCloudAggregator>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("count", 2),
+				rclcpp::Parameter("frame_id", "base_link"),
+				rclcpp::Parameter("approx_sync", true)})));
+	publishStaticTf("base_link", "lidar_a");
+	publishStaticTf("base_link", "lidar_b");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("combined_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub1 =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud1", 10);
+	ASSERT_TRUE(waitForSubscriber(pub1));
+
+	// Only one of the two inputs arrives: the synchronizer must not fire.
+	pub1->publish(makeXYZCloud("lidar_a", 1000.0, {{1.0f, 0.0f, 0.0f}}));
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(out->empty()) << "a single input must not produce a combined cloud";
+}
diff --git a/rtabmap_util/test/test_point_cloud_assembler.cpp b/rtabmap_util/test/test_point_cloud_assembler.cpp
new file mode 100644
index 00000000..9e7cc7f7
--- /dev/null
+++ b/rtabmap_util/test/test_point_cloud_assembler.cpp
@@ -0,0 +1,399 @@
+/*
+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_util/point_cloud_assembler.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rtabmap_msgs/msg/odom_info.hpp>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+class PointCloudAssemblerTest : public NodeTest
+{
+protected:
+	/// Starts the assembler with @p overrides, plus a static odom -> lidar transform.
+	void start(const std::vector<rclcpp::Parameter> & overrides)
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		publishStaticTf("odom", "lidar");
+		publishStaticTf("lidar", "base_link");
+		out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
+		pub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		ASSERT_TRUE(waitForSubscriber(pub_));
+	}
+
+	static bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
+	{
+		for(size_t i=0; i<cloud.fields.size(); ++i)
+		{
+			if(cloud.fields[i].name == name) { return true; }
+		}
+		return false;
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
+};
+
+TEST_F(PointCloudAssemblerTest, PublishesAfterMaxCloudsAreAccumulated)
+{
+	addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("max_clouds", 3),
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.2)})));
+	publishStaticTf("odom", "lidar");
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}};
+
+	// The first two clouds are only accumulated.
+	pub->publish(makeXYZCloud("lidar", 1000.0, points));
+	pub->publish(makeXYZCloud("lidar", 1000.1, points));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(out->empty()) << "nothing is published before max_clouds is reached";
+
+	// The third completes the batch.
+	pub->publish(makeXYZCloud("lidar", 1000.2, points));
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }))
+		<< "the assembled cloud must be published on the third input";
+
+	EXPECT_EQ(out->back().width, 3 * points.size()) << "all three clouds must be included";
+	EXPECT_EQ(out->back().header.frame_id, "lidar")
+		<< "the assembled cloud comes back in the sensor frame";
+}
+
+TEST_F(PointCloudAssemblerTest, AssemblingTimePublishesAfterTheConfiguredSpan)
+{
+	// An alternative trigger to max_clouds: publish once the newest cloud is at least
+	// assembling_time newer than the oldest one held.
+	start({rclcpp::Parameter("max_clouds", 0),
+		   rclcpp::Parameter("assembling_time", 0.25),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	for(int i=0; i<3; ++i)          // 1000.0, 1000.1, 1000.2 -- span 0.2 s, below 0.25
+	{
+		pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
+	}
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(out_->empty()) << "0.2 s of clouds is short of assembling_time";
+
+	pub_->publish(makeXYZCloud("lidar", 1000.3, points));   // span now 0.3 s
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
+		<< "crossing assembling_time must publish";
+	EXPECT_EQ(out_->back().width, 4u) << "all four clouds are included";
+}
+
+TEST_F(PointCloudAssemblerTest, CircularBufferPublishesOnEveryCloud)
+{
+	// With a circular buffer the node emits a sliding window instead of filling up,
+	// clearing and starting again: every input produces an output.
+	start({rclcpp::Parameter("max_clouds", 3),
+		   rclcpp::Parameter("circular_buffer", true),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	for(int i=0; i<4; ++i)
+	{
+		pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
+		ASSERT_TRUE(spinUntil([&]() { return out_->size() >= size_t(i+1); }))
+			<< "cloud " << i << " did not produce an output";
+	}
+	EXPECT_EQ(out_->size(), 4u) << "one output per input, not one per full batch";
+	// The window is capped at max_clouds.
+	EXPECT_LE(out_->back().width, 3u);
+}
+
+TEST_F(PointCloudAssemblerTest, RangeMaxDropsDistantPoints)
+{
+	start({rclcpp::Parameter("max_clouds", 1),
+		   rclcpp::Parameter("range_max", 3.0),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	// Two points inside 3 m, one well beyond it.
+	pub_->publish(makeXYZCloud("lidar", 1000.0,
+			{{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {9.0f, 0.0f, 0.0f}}));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width, 2u) << "the 9 m point must be filtered out";
+}
+
+TEST_F(PointCloudAssemblerTest, RemoveZDropsTheZField)
+{
+	start({rclcpp::Parameter("max_clouds", 1),
+		   rclcpp::Parameter("remove_z", true),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.5f}}));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	// The field is removed entirely, not zeroed: the output is a 2D cloud.
+	EXPECT_TRUE(hasField(out_->back(), "x"));
+	EXPECT_TRUE(hasField(out_->back(), "y"));
+	EXPECT_FALSE(hasField(out_->back(), "z")) << "remove_z drops the field itself";
+}
+
+TEST_F(PointCloudAssemblerTest, FrameIdSetsTheOutputFrame)
+{
+	start({rclcpp::Parameter("max_clouds", 1),
+		   rclcpp::Parameter("frame_id", "base_link"),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	pub_->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().header.frame_id, "base_link")
+		<< "the assembled cloud is returned in frame_id when it is set";
+}
+
+TEST_F(PointCloudAssemblerTest, SkipCloudsIgnoresIntermediateClouds)
+{
+	// skip_clouds=1 keeps every other cloud, so reaching max_clouds=2 takes four inputs.
+	start({rclcpp::Parameter("max_clouds", 2),
+		   rclcpp::Parameter("skip_clouds", 1),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	pub_->publish(makeXYZCloud("lidar", 1000.0, points));
+	pub_->publish(makeXYZCloud("lidar", 1000.1, points));
+	spinFor(std::chrono::milliseconds(300));
+	EXPECT_TRUE(out_->empty()) << "one of those two was skipped, so the batch is short";
+
+	pub_->publish(makeXYZCloud("lidar", 1000.2, points));
+	pub_->publish(makeXYZCloud("lidar", 1000.3, points));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().width, 2u) << "two kept clouds, two skipped";
+}
+
+TEST_F(PointCloudAssemblerTest, LinearUpdateSkipsCloudsWhileStationary)
+{
+	// With linear_update set, a cloud captured without the robot having moved far enough
+	// is discarded rather than accumulated, so a parked robot never fills a batch.
+	start({rclcpp::Parameter("max_clouds", 3),
+		   rclcpp::Parameter("linear_update", 0.5),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	for(int i=0; i<5; ++i)          // the TF is static, so the robot never moves
+	{
+		pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
+	}
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(out_->empty())
+		<< "a stationary robot must not accumulate a batch when linear_update is set";
+}
+
+TEST_F(PointCloudAssemblerTest, WithoutLinearUpdateEveryCloudCounts)
+{
+	// The same stationary robot, with the motion filter disabled: the batch fills.
+	start({rclcpp::Parameter("max_clouds", 3),
+		   rclcpp::Parameter("linear_update", 0.0),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	for(int i=0; i<3; ++i)
+	{
+		pub_->publish(makeXYZCloud("lidar", 1000.0 + 0.1*i, points));
+	}
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().width, 3u);
+}
+
+TEST_F(PointCloudAssemblerTest, VoxelSizeDownsamplesTheCloud)
+{
+	start({rclcpp::Parameter("max_clouds", 1),
+		   rclcpp::Parameter("voxel_size", 0.5),
+		   rclcpp::Parameter("fixed_frame_id", "odom"),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	// 100 points packed into a 0.2 m cube: a 0.5 m voxel grid collapses them.
+	std::vector<cv::Point3f> dense;
+	for(int i=0; i<10; ++i)
+	{
+		for(int j=0; j<10; ++j)
+		{
+			dense.push_back(cv::Point3f(1.0f + 0.02f*i, 0.02f*j, 0.0f));
+		}
+	}
+	pub_->publish(makeXYZCloud("lidar", 1000.0, dense));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_LT(out_->back().width, dense.size())
+		<< "voxel_size must reduce the point count";
+	EXPECT_GT(out_->back().width, 0u);
+}
+
+/// Fixture for the odometry-synchronized modes, which need fixed_frame_id to be empty.
+class PointCloudAssemblerOdomTest : public NodeTest
+{
+protected:
+	void start(std::vector<rclcpp::Parameter> overrides)
+	{
+		// fixed_frame_id defaults to "odom"; it has to be cleared for the node to
+		// subscribe to the odometry topic instead of reading TF directly.
+		overrides.push_back(rclcpp::Parameter("fixed_frame_id", ""));
+		addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		publishStaticTf("odom", "lidar");
+		out_ = collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
+		cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		odomPub_ = helper()->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+		odomInfoPub_ = helper()->create_publisher<rtabmap_msgs::msg::OdomInfo>("odom_info", 10);
+		ASSERT_TRUE(waitForSubscriber(cloudPub_));
+		ASSERT_TRUE(waitForSubscriber(odomPub_));
+	}
+
+	/// An odometry message at the origin; a null one has an all-zero orientation.
+	nav_msgs::msg::Odometry makeOdom(double stamp, bool null = false)
+	{
+		nav_msgs::msg::Odometry odom;
+		odom.header.stamp = stampOf(stamp);
+		odom.header.frame_id = "odom";
+		odom.child_frame_id = "lidar";
+		odom.pose.pose.orientation.w = null ? 0.0 : 1.0;
+		return odom;
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPub_;
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfoPub_;
+};
+
+TEST_F(PointCloudAssemblerOdomTest, TakesTheFixedFrameFromTheOdometryMessage)
+{
+	// With fixed_frame_id empty the node syncs cloud with odom and uses the odometry
+	// header's frame as the fixed frame.
+	start({rclcpp::Parameter("max_clouds", 2),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	for(int i=0; i<2; ++i)
+	{
+		const double t = 1000.0 + 0.1*i;
+		cloudPub_->publish(makeXYZCloud("lidar", t, points));
+		odomPub_->publish(makeOdom(t));          // exact sync: identical stamps
+		spinFor(std::chrono::milliseconds(50));
+	}
+
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }))
+		<< "cloud+odom synchronization must drive the assembly";
+	EXPECT_EQ(out_->back().width, 2u);
+}
+
+TEST_F(PointCloudAssemblerOdomTest, NullOdometryResetsTheBuffer)
+{
+	// A null odometry means tracking was lost, so the accumulated clouds are dropped
+	// rather than being stitched across the discontinuity.
+	start({rclcpp::Parameter("max_clouds", 3),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+
+	// Two good clouds, then a lost-tracking frame, then two more.
+	for(int i=0; i<2; ++i)
+	{
+		const double t = 1000.0 + 0.1*i;
+		cloudPub_->publish(makeXYZCloud("lidar", t, points));
+		odomPub_->publish(makeOdom(t));
+		spinFor(std::chrono::milliseconds(50));
+	}
+	cloudPub_->publish(makeXYZCloud("lidar", 1000.2, points));
+	odomPub_->publish(makeOdom(1000.2, /*null=*/true));
+	spinFor(std::chrono::milliseconds(150));
+	EXPECT_TRUE(out_->empty()) << "the null odometry must not complete the batch";
+
+	// After the reset it takes three fresh clouds again, not one.
+	for(int i=0; i<2; ++i)
+	{
+		const double t = 1000.3 + 0.1*i;
+		cloudPub_->publish(makeXYZCloud("lidar", t, points));
+		odomPub_->publish(makeOdom(t));
+		spinFor(std::chrono::milliseconds(50));
+	}
+	EXPECT_TRUE(out_->empty()) << "the buffer restarted, so two clouds are not enough";
+
+	cloudPub_->publish(makeXYZCloud("lidar", 1000.5, points));
+	odomPub_->publish(makeOdom(1000.5));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().width, 3u) << "only the post-reset clouds are assembled";
+}
+
+TEST_F(PointCloudAssemblerOdomTest, SubscribeOdomInfoKeepsOnlyKeyFrames)
+{
+	// With subscribe_odom_info the node also takes OdomInfo and accumulates a cloud only
+	// when that frame became a key frame.
+	start({rclcpp::Parameter("max_clouds", 2),
+		   rclcpp::Parameter("subscribe_odom_info", true),
+		   rclcpp::Parameter("wait_for_transform", 0.2)});
+	ASSERT_TRUE(waitForSubscriber(odomInfoPub_));
+
+	const std::vector<cv::Point3f> points = {{1.0f, 0.0f, 0.0f}};
+	auto publishFrame = [&](double t, bool keyFrame) {
+		rtabmap_msgs::msg::OdomInfo info;
+		info.header.stamp = stampOf(t);
+		info.header.frame_id = "odom";
+		info.key_frame_added = keyFrame;
+		cloudPub_->publish(makeXYZCloud("lidar", t, points));
+		odomPub_->publish(makeOdom(t));
+		odomInfoPub_->publish(info);
+		spinFor(std::chrono::milliseconds(60));
+	};
+
+	publishFrame(1000.0, false);
+	publishFrame(1000.1, false);
+	publishFrame(1000.2, false);
+	EXPECT_TRUE(out_->empty()) << "non key frames must be ignored";
+
+	publishFrame(1000.3, true);
+	publishFrame(1000.4, true);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+	EXPECT_EQ(out_->back().width, 2u) << "only the two key frames are assembled";
+}
+
+TEST_F(PointCloudAssemblerTest, DropsCloudsWithoutTheFixedFrame)
+{
+	// No TF at all, so the assembler cannot place the clouds relative to each other.
+	addNode(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()
+			.parameter_overrides({
+				rclcpp::Parameter("max_clouds", 2),
+				rclcpp::Parameter("fixed_frame_id", "odom"),
+				rclcpp::Parameter("wait_for_transform", 0.0)})));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out =
+			collect<sensor_msgs::msg::PointCloud2>("assembled_cloud");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeXYZCloud("lidar", 1000.0, {{1.0f, 0.0f, 0.0f}}));
+	pub->publish(makeXYZCloud("lidar", 1000.1, {{1.0f, 0.0f, 0.0f}}));
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_TRUE(out->empty());
+}
diff --git a/rtabmap_util/test/test_point_cloud_xyz.cpp b/rtabmap_util/test/test_point_cloud_xyz.cpp
new file mode 100644
index 00000000..700ea6cf
--- /dev/null
+++ b/rtabmap_util/test/test_point_cloud_xyz.cpp
@@ -0,0 +1,344 @@
+/*
+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_util/point_cloud_xyz.hpp>
+
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <cmath>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr int kWidth = 16;
+constexpr int kHeight = 16;
+constexpr double kFx = 100.0;
+
+/// A depth image where every pixel is at @p meters.
+sensor_msgs::msg::Image makeDepth(
+		double stamp, float meters,
+		const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
+{
+	cv::Mat image;
+	if(encoding == sensor_msgs::image_encodings::TYPE_32FC1)
+	{
+		image = cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters));
+	}
+	else
+	{
+		image = cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
+	}
+	return makeImage("camera_link", stamp, image, encoding);
+}
+
+/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
+stereo_msgs::msg::DisparityImage makeDisparity(
+		double stamp, float disparity, float focal = float(kFx), float baseline = 0.1f)
+{
+	stereo_msgs::msg::DisparityImage msg;
+	msg.header.frame_id = "camera_link";
+	msg.header.stamp = stampOf(stamp);
+	msg.f = focal;
+	msg.t = baseline;
+	msg.min_disparity = 1.0f;
+	msg.max_disparity = 100.0f;
+	msg.image = makeImage("camera_link", stamp,
+			cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
+			sensor_msgs::image_encodings::TYPE_32FC1);
+	return msg;
+}
+
+/// The same, in the 16SC1 fixed-point form where the stored value is 16*disparity.
+stereo_msgs::msg::DisparityImage makeDisparity16SC1(double stamp, float disparity)
+{
+	stereo_msgs::msg::DisparityImage msg = makeDisparity(stamp, disparity);
+	msg.image = makeImage("camera_link", stamp,
+			cv::Mat(kHeight, kWidth, CV_16SC1, cv::Scalar(short(disparity*16.0f))),
+			sensor_msgs::image_encodings::TYPE_16SC1);
+	return msg;
+}
+
+bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
+{
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == name) { return true; }
+	}
+	return false;
+}
+}  // namespace
+
+class PointCloudXYZTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		depthPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(depthPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	/// Publishes a synchronized depth + camera_info pair.
+	void publishFrame(double stamp, float meters,
+			const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
+	{
+		depthPub_->publish(makeDepth(stamp, meters, encoding));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(PointCloudXYZTest, ProjectsDepthIntoACloud)
+{
+	start();
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
+		<< "one point per pixel at decimation 1";
+	EXPECT_EQ(cloud.header.frame_id, "camera_link")
+		<< "the cloud takes the depth image's frame";
+
+	// The principal-point pixel projects straight ahead at the measured depth.
+	const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
+	EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
+}
+
+TEST_F(PointCloudXYZTest, Accepts16UC1Millimeters)
+{
+	start();
+	publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
+	EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3)
+		<< "millimeter depth must be converted to meters";
+}
+
+TEST_F(PointCloudXYZTest, RejectsUnsupportedEncoding)
+{
+	start();
+	depthPub_->publish(makeImage("camera_link", 1000.0,
+			cv::Mat(kHeight, kWidth, CV_8UC3, cv::Scalar(1,2,3)), "bgr8"));
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
+}
+
+TEST_F(PointCloudXYZTest, DecimationReducesThePointCount)
+{
+	start({rclcpp::Parameter("decimation", 2)});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
+		<< "decimation 2 keeps one pixel in four";
+}
+
+TEST_F(PointCloudXYZTest, MaxDepthMarksFarPointsInvalid)
+{
+	// cloudFromDepth keeps the cloud organized: points outside the depth range become
+	// NaN rather than disappearing, so the point count is unchanged.
+	start({rclcpp::Parameter("max_depth", 1.0)});
+	publishFrame(1000.0, 5.0f);          // beyond the limit
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
+		<< "the cloud stays organized";
+	EXPECT_TRUE(std::isnan(readXYZ(cloud, 0).z)) << "every point is past max_depth";
+	EXPECT_TRUE(std::isnan(readXYZ(cloud, kWidth*kHeight-1).z));
+}
+
+TEST_F(PointCloudXYZTest, WithinMaxDepthPointsStayValid)
+{
+	start({rclcpp::Parameter("max_depth", 10.0)});
+	publishFrame(1000.0, 5.0f);          // inside the limit
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
+	EXPECT_FALSE(std::isnan(readXYZ(out_->back(), center).z));
+	EXPECT_NEAR(readXYZ(out_->back(), center).z, 5.0f, 1e-3);
+}
+
+TEST_F(PointCloudXYZTest, MinDepthMarksNearPointsInvalid)
+{
+	start({rclcpp::Parameter("min_depth", 3.0)});
+	publishFrame(1000.0, 1.0f);          // closer than the limit
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_TRUE(std::isnan(readXYZ(out_->back(), 0).z))
+		<< "every point is nearer than min_depth";
+}
+
+TEST_F(PointCloudXYZTest, FilterNaNsRemovesInvalidPoints)
+{
+	// With filter_nans the invalid points are dropped instead, giving an unorganized
+	// cloud that is empty when nothing is in range.
+	start({rclcpp::Parameter("max_depth", 1.0),
+		   rclcpp::Parameter("filter_nans", true)});
+	publishFrame(1000.0, 5.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, 0u)
+		<< "filter_nans must remove the out-of-range points";
+}
+
+TEST_F(PointCloudXYZTest, NormalKAddsNormalFields)
+{
+	start({rclcpp::Parameter("normal_k", 10)});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_TRUE(hasField(out_->back(), "normal_x"))
+		<< "asking for normals must change the point type";
+	EXPECT_TRUE(hasField(out_->back(), "normal_z"));
+}
+
+TEST_F(PointCloudXYZTest, NoNormalFieldsByDefault)
+{
+	start();
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_FALSE(hasField(out_->back(), "normal_x"));
+}
+
+TEST_F(PointCloudXYZTest, StaysSilentWithoutASubscriber)
+{
+	// The projection is skipped entirely when nobody wants the cloud.
+	addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(rclcpp::NodeOptions()));
+	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>("depth/camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(depthPub));
+
+	depthPub->publish(makeDepth(1000.0, 2.0f));
+	infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(300));
+
+    std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
+			collect<sensor_msgs::msg::PointCloud2>("cloud");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+//============================================================================
+// disparity/image + disparity/camera_info
+//============================================================================
+
+class PointCloudXYZDisparityTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZ>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>(
+				"disparity/image", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>(
+				"disparity/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(dispPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	void publishFrame(double stamp, const stereo_msgs::msg::DisparityImage & disparity)
+	{
+		dispPub_->publish(disparity);
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(PointCloudXYZDisparityTest, ProjectsDisparityIntoACloud)
+{
+	start();
+	publishFrame(1000.0, makeDisparity(1000.0, 5.0f));   // depth = f*t/d = 100*0.1/5 = 2 m
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
+	EXPECT_EQ(cloud.header.frame_id, "camera_link")
+		<< "the cloud takes the disparity image's frame";
+
+	const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
+	EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 1e-3);
+}
+
+TEST_F(PointCloudXYZDisparityTest, Accepts16SC1FixedPointDisparity)
+{
+	// The 16-bit form stores 16*disparity, so the same 5 px must still give 2 m.
+	start();
+	publishFrame(1000.0, makeDisparity16SC1(1000.0, 5.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const size_t center = size_t(kHeight/2) * kWidth + kWidth/2;
+	EXPECT_NEAR(readXYZ(out_->back(), center).z, 2.0f, 1e-3);
+}
+
+TEST_F(PointCloudXYZDisparityTest, RejectsUnsupportedDisparityEncoding)
+{
+	start();
+	stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
+	msg.image = makeImage("camera_link", 1000.0,
+			cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
+	publishFrame(1000.0, msg);
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
+}
+
+TEST_F(PointCloudXYZDisparityTest, MaxDepthMarksFarPointsInvalid)
+{
+	// Like the depth path, cloudFromDisparity keeps the cloud organized and turns the
+	// out-of-range points into NaN instead of removing them.
+	start({rclcpp::Parameter("max_depth", 1.0)});
+	publishFrame(1000.0, makeDisparity(1000.0, 5.0f));   // 2 m, beyond the limit
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
+	EXPECT_TRUE(std::isnan(readXYZ(cloud, size_t(kHeight/2)*kWidth + kWidth/2).z));
+}
+
+TEST_F(PointCloudXYZDisparityTest, FilterNaNsRemovesInvalidPoints)
+{
+	start({rclcpp::Parameter("max_depth", 1.0),
+		   rclcpp::Parameter("filter_nans", true)});
+	publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, 0u);
+}
+
+TEST_F(PointCloudXYZDisparityTest, DecimationReducesThePointCount)
+{
+	start({rclcpp::Parameter("decimation", 2)});
+	publishFrame(1000.0, makeDisparity(1000.0, 5.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4);
+}
diff --git a/rtabmap_util/test/test_point_cloud_xyzrgb.cpp b/rtabmap_util/test/test_point_cloud_xyzrgb.cpp
new file mode 100644
index 00000000..a17221a0
--- /dev/null
+++ b/rtabmap_util/test/test_point_cloud_xyzrgb.cpp
@@ -0,0 +1,531 @@
+/*
+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_util/point_cloud_xyzrgb.hpp>
+
+#include <stereo_msgs/msg/disparity_image.hpp>
+
+#include <cmath>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr int kWidth = 16;
+constexpr int kHeight = 16;
+constexpr double kFx = 100.0;
+constexpr int kCenter = (kHeight/2) * kWidth + kWidth/2;
+
+/// The color every synthetic RGB image is painted with, in OpenCV's BGR order.
+const cv::Scalar kColor(10, 20, 30);
+
+sensor_msgs::msg::Image makeRgb(double stamp, const std::string & encoding = "bgr8")
+{
+	if(encoding == "mono8")
+	{
+		return makeImage("camera_link", stamp,
+				cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(128)), encoding);
+	}
+	return makeImage("camera_link", stamp,
+			cv::Mat(kHeight, kWidth, CV_8UC3, kColor), encoding);
+}
+
+sensor_msgs::msg::Image makeDepth(double stamp, float meters,
+		const std::string & encoding = sensor_msgs::image_encodings::TYPE_32FC1)
+{
+	cv::Mat image = encoding == sensor_msgs::image_encodings::TYPE_32FC1
+			? cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(meters))
+			: cv::Mat(kHeight, kWidth, CV_16UC1, cv::Scalar(uint16_t(meters*1000.0f)));
+	return makeImage("camera_link", stamp, image, encoding);
+}
+
+/// A disparity image where every pixel carries @p disparity, so depth = f*t/disparity.
+stereo_msgs::msg::DisparityImage makeDisparity(double stamp, float disparity,
+		float focal = float(kFx), float baseline = 0.1f)
+{
+	stereo_msgs::msg::DisparityImage msg;
+	msg.header.frame_id = "camera_link";
+	msg.header.stamp = stampOf(stamp);
+	msg.f = focal;
+	msg.t = baseline;
+	msg.min_disparity = 1.0f;
+	msg.max_disparity = 100.0f;
+	msg.image = makeImage("camera_link", stamp,
+			cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(disparity)),
+			sensor_msgs::image_encodings::TYPE_32FC1);
+	return msg;
+}
+
+bool hasField(const sensor_msgs::msg::PointCloud2 & cloud, const std::string & name)
+{
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == name) { return true; }
+	}
+	return false;
+}
+
+/// Reads the packed "rgb" float field of a point as (r,g,b).
+cv::Vec3b readRGB(const sensor_msgs::msg::PointCloud2 & cloud, size_t index)
+{
+	uint32_t offset = 16;
+	for(size_t i=0; i<cloud.fields.size(); ++i)
+	{
+		if(cloud.fields[i].name == "rgb") { offset = cloud.fields[i].offset; }
+	}
+	uint32_t packed = 0;
+	memcpy(&packed, &cloud.data[index * cloud.point_step + offset], 4);
+	return cv::Vec3b(
+			uint8_t((packed >> 16) & 0xFF),
+			uint8_t((packed >> 8) & 0xFF),
+			uint8_t(packed & 0xFF));
+}
+}  // namespace
+
+class PointCloudXYZRGBTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		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(waitForPublisher(out_->subscription));
+	}
+
+	/// Publishes a synchronized rgb + depth + camera_info triple.
+	void publishFrame(double stamp, float meters,
+			const std::string & depthEncoding = sensor_msgs::image_encodings::TYPE_32FC1,
+			const std::string & rgbEncoding = "bgr8")
+	{
+		rgbPub_->publish(makeRgb(stamp, rgbEncoding));
+		depthPub_->publish(makeDepth(stamp, meters, depthEncoding));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rgbPub_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+//============================================================================
+// rgb + depth + camera_info
+//============================================================================
+
+TEST_F(PointCloudXYZRGBTest, ProjectsRgbAndDepthIntoAColoredCloud)
+{
+	start();
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight))
+		<< "one point per pixel at decimation 1";
+	EXPECT_EQ(cloud.header.frame_id, "camera_link")
+		<< "the cloud takes the RGB image's frame";
+	EXPECT_TRUE(hasField(cloud, "rgb")) << "the whole point of this node";
+	EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
+
+	// The RGB image is uniform, so every point carries the same color. cv_bridge hands
+	// the node a bgr8 image, which reaches the cloud as r=30, g=20, b=10.
+	const cv::Vec3b rgb = readRGB(cloud, kCenter);
+	EXPECT_EQ(int(rgb[0]), 30);
+	EXPECT_EQ(int(rgb[1]), 20);
+	EXPECT_EQ(int(rgb[2]), 10);
+}
+
+TEST_F(PointCloudXYZRGBTest, Accepts16UC1Millimeters)
+{
+	start();
+	publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_16UC1);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_NEAR(readXYZ(out_->back(), kCenter).z, 2.0f, 1e-3)
+		<< "millimeter depth must be converted to meters";
+}
+
+TEST_F(PointCloudXYZRGBTest, AcceptsMono8Color)
+{
+	start();
+	publishFrame(1000.0, 2.0f, sensor_msgs::image_encodings::TYPE_32FC1, "mono8");
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const cv::Vec3b rgb = readRGB(out_->back(), kCenter);
+	EXPECT_EQ(int(rgb[0]), 128) << "a grey image gives grey points";
+	EXPECT_EQ(int(rgb[1]), 128);
+	EXPECT_EQ(int(rgb[2]), 128);
+}
+
+TEST_F(PointCloudXYZRGBTest, RejectsUnsupportedDepthEncoding)
+{
+	start();
+	rgbPub_->publish(makeRgb(1000.0));
+	depthPub_->publish(makeImage("camera_link", 1000.0,
+			cv::Mat(kHeight, kWidth, CV_8UC3, kColor), "bgr8"));
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty()) << "only 32FC1, 16UC1 and mono16 depth are supported";
+}
+
+TEST_F(PointCloudXYZRGBTest, DecimationReducesThePointCount)
+{
+	start({rclcpp::Parameter("decimation", 2)});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight)/4)
+		<< "decimation 2 keeps one pixel in four";
+}
+
+TEST_F(PointCloudXYZRGBTest, RoiRatiosCropTheCloud)
+{
+	// A quarter off each side of a 16x16 image leaves an 8x8 window.
+	start({rclcpp::Parameter("roi_ratios", std::string("0.25 0.25 0.25 0.25"))});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, 64u);
+}
+
+TEST_F(PointCloudXYZRGBTest, MaxDepthMarksFarPointsInvalid)
+{
+	// cloudFromDepthRGB keeps the cloud organized: out-of-range points become NaN
+	// rather than disappearing, so the point count is unchanged.
+	start({rclcpp::Parameter("max_depth", 1.0)});
+	publishFrame(1000.0, 5.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(kWidth*kHeight));
+	EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
+}
+
+TEST_F(PointCloudXYZRGBTest, MinDepthMarksNearPointsInvalid)
+{
+	start({rclcpp::Parameter("min_depth", 3.0)});
+	publishFrame(1000.0, 1.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
+}
+
+TEST_F(PointCloudXYZRGBTest, FilterNaNsRemovesInvalidPoints)
+{
+	start({rclcpp::Parameter("max_depth", 1.0),
+		   rclcpp::Parameter("filter_nans", true)});
+	publishFrame(1000.0, 5.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_EQ(out_->back().width * out_->back().height, 0u)
+		<< "filter_nans must remove the out-of-range points";
+}
+
+TEST_F(PointCloudXYZRGBTest, NormalKAddsNormalFields)
+{
+	start({rclcpp::Parameter("normal_k", 10)});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_TRUE(hasField(out_->back(), "normal_x"))
+		<< "asking for normals must change the point type";
+	EXPECT_TRUE(hasField(out_->back(), "rgb")) << "and must keep the color";
+}
+
+TEST_F(PointCloudXYZRGBTest, NoNormalFieldsByDefault)
+{
+	start();
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_FALSE(hasField(out_->back(), "normal_x"));
+}
+
+TEST_F(PointCloudXYZRGBTest, VoxelSizeThinsTheCloud)
+{
+	// A frontal plane at 2 m spans about 0.32 m across a 16-pixel image at fx=100, so a
+	// 0.1 m voxel grid collapses the 256 points into far fewer.
+	start({rclcpp::Parameter("voxel_size", 0.1)});
+	publishFrame(1000.0, 2.0f);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const uint32_t points = out_->back().width * out_->back().height;
+	EXPECT_GT(points, 0u);
+	EXPECT_LT(points, uint32_t(kWidth*kHeight));
+}
+
+TEST_F(PointCloudXYZRGBTest, StaysSilentWithoutASubscriber)
+{
+	// The projection is skipped entirely when nobody wants the cloud.
+	addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(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));
+
+	rgbPub->publish(makeRgb(1000.0));
+	depthPub->publish(makeDepth(1000.0, 2.0f));
+	infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(300));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> late =
+			collect<sensor_msgs::msg::PointCloud2>("cloud");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+//============================================================================
+// rgbd_image
+//============================================================================
+
+class PointCloudXYZRGBRgbdTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		rgbdPub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		ASSERT_TRUE(waitForSubscriber(rgbdPub_));
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdPub_;
+};
+
+TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAnRgbdImage)
+{
+	start();
+	rgbdPub_->publish(makeRGBDImage("camera_link", 1000.0, kWidth, kHeight));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
+	EXPECT_EQ(cloud.header.frame_id, "camera_link");
+	EXPECT_TRUE(hasField(cloud, "rgb"));
+	EXPECT_NEAR(readXYZ(cloud, kCenter).z, 1.5f, 1e-3)
+		<< "makeRGBDImage() fills the depth image with 1500 mm";
+}
+
+TEST_F(PointCloudXYZRGBRgbdTest, IgnoresAnInvalidRgbdImage)
+{
+	// isValid() is false without any image data, and nothing must be published.
+	start();
+	rtabmap_msgs::msg::RGBDImage msg;
+	msg.header.frame_id = "camera_link";
+	msg.header.stamp = stampOf(1000.0);
+	rgbdPub_->publish(msg);
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty());
+}
+
+TEST_F(PointCloudXYZRGBRgbdTest, PublishesAnEmptyCloudForAColorOnlyRgbdImage)
+{
+	// Depth is optional in an RGBDImage, so color alone must not be treated as a broken
+	// message: there is simply nothing to project.
+	start();
+	rtabmap_msgs::msg::RGBDImage msg = makeRGBDImage("camera_link", 1000.0, kWidth, kHeight);
+	msg.depth = sensor_msgs::msg::Image();
+	rgbdPub_->publish(msg);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	EXPECT_EQ(out_->back().width * out_->back().height, 0u);
+}
+
+TEST_F(PointCloudXYZRGBRgbdTest, ProjectsAStereoRgbdImage)
+{
+	// A stereo pair in an RGBDImage is dense-matched instead of read as depth.
+	start();
+	rgbdPub_->publish(makeStereoRGBDImage("camera_link", 1000.0, 160, 120));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	EXPECT_EQ(out_->back().width * out_->back().height, uint32_t(160*120));
+	EXPECT_TRUE(hasField(out_->back(), "rgb"));
+}
+
+//============================================================================
+// left/image + disparity + left/camera_info
+//============================================================================
+
+class PointCloudXYZRGBDisparityTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
+		dispPub_ = helper()->create_publisher<stereo_msgs::msg::DisparityImage>("disparity", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(leftPub_));
+		ASSERT_TRUE(waitForSubscriber(dispPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	void publishFrame(double stamp, float disparity)
+	{
+		leftPub_->publish(makeRgb(stamp));
+		dispPub_->publish(makeDisparity(stamp, disparity));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr leftPub_;
+	rclcpp::Publisher<stereo_msgs::msg::DisparityImage>::SharedPtr dispPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(PointCloudXYZRGBDisparityTest, ProjectsDisparityIntoAColoredCloud)
+{
+	start();
+	publishFrame(1000.0, 5.0f);   // depth = f*t/d = 100*0.1/5 = 2 m
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kWidth*kHeight));
+	EXPECT_EQ(cloud.header.frame_id, "camera_link")
+		<< "the cloud takes the disparity image's frame";
+	EXPECT_TRUE(hasField(cloud, "rgb"));
+	EXPECT_NEAR(readXYZ(cloud, kCenter).z, 2.0f, 1e-3);
+
+	const cv::Vec3b rgb = readRGB(cloud, kCenter);
+	EXPECT_EQ(int(rgb[0]), 30);
+	EXPECT_EQ(int(rgb[2]), 10);
+}
+
+TEST_F(PointCloudXYZRGBDisparityTest, RejectsUnsupportedDisparityEncoding)
+{
+	start();
+	stereo_msgs::msg::DisparityImage msg = makeDisparity(1000.0, 5.0f);
+	msg.image = makeImage("camera_link", 1000.0,
+			cv::Mat(kHeight, kWidth, CV_8UC1, cv::Scalar(5)), "mono8");
+	leftPub_->publish(makeRgb(1000.0));
+	dispPub_->publish(msg);
+	infoPub_->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty()) << "only 32FC1 and 16SC1 disparity are supported";
+}
+
+TEST_F(PointCloudXYZRGBDisparityTest, MaxDepthMarksFarPointsInvalid)
+{
+	start({rclcpp::Parameter("max_depth", 1.0)});
+	publishFrame(1000.0, 5.0f);   // 2 m, beyond the limit
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	EXPECT_TRUE(std::isnan(readXYZ(out_->back(), kCenter).z));
+}
+
+//============================================================================
+// left/image + right/image + both camera_infos
+//============================================================================
+
+class PointCloudXYZRGBStereoTest : public NodeTest
+{
+protected:
+	static constexpr int kStereoWidth = 160;
+	static constexpr int kStereoHeight = 120;
+	static constexpr float kBaseline = 0.12f;
+	static constexpr int kDisparity = 6;   // depth = fx*baseline/d = 100*0.12/6 = 2 m
+
+	void start(const std::vector<rclcpp::Parameter> & overrides = {})
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudXYZRGB>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		out_ = collect<sensor_msgs::msg::PointCloud2>("cloud");
+		leftPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("left/image", 10);
+		rightPub_ = helper()->create_publisher<sensor_msgs::msg::Image>("right/image", 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(waitForPublisher(out_->subscription));
+	}
+
+	/**
+	 * Publishes a textured pair whose true disparity is kDisparity everywhere: the right
+	 * image is the left one shifted, which is what a plane at a constant depth looks like.
+	 */
+	void publishFrame(double stamp)
+	{
+		cv::Mat left(kStereoHeight, kStereoWidth + kDisparity, CV_8UC1);
+		cv::RNG rng(42);
+		rng.fill(left, cv::RNG::UNIFORM, 0, 256);
+
+		cv::Mat leftBgr;
+		cv::cvtColor(cv::Mat(left, cv::Rect(0, 0, kStereoWidth, kStereoHeight)),
+				leftBgr, cv::COLOR_GRAY2BGR);
+		cv::Mat right(left, cv::Rect(kDisparity, 0, kStereoWidth, kStereoHeight));
+
+		leftPub_->publish(makeImage("camera_link", stamp, leftBgr, "bgr8"));
+		rightPub_->publish(makeImage("camera_link", stamp, right.clone(), "mono8"));
+		leftInfoPub_->publish(makeCameraInfo(
+				"camera_link", stamp, kStereoWidth, kStereoHeight, 0.0, kFx));
+		rightInfoPub_->publish(makeCameraInfo(
+				"camera_link", stamp, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::PointCloud2>> out_;
+	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_;
+};
+
+TEST_F(PointCloudXYZRGBStereoTest, MatchesAStereoPairIntoAColoredCloud)
+{
+	start({rclcpp::Parameter("StereoBM/NumDisparities", std::string("16")),
+		   rclcpp::Parameter("StereoBM/BlockSize", std::string("9"))});
+	publishFrame(1000.0);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })) << "no cloud published";
+
+	const sensor_msgs::msg::PointCloud2 & cloud = out_->back();
+	EXPECT_EQ(cloud.width * cloud.height, uint32_t(kStereoWidth*kStereoHeight))
+		<< "the cloud stays organized, one point per pixel";
+	EXPECT_EQ(cloud.header.frame_id, "camera_link")
+		<< "the cloud takes the left image's frame";
+	EXPECT_TRUE(hasField(cloud, "rgb"));
+
+	// The pair is a shifted copy of itself, so the whole matched area sits at one depth.
+	const size_t center = size_t(kStereoHeight/2) * kStereoWidth + kStereoWidth/2;
+	EXPECT_NEAR(readXYZ(cloud, center).z, 2.0f, 0.2f);
+}
+
+TEST_F(PointCloudXYZRGBStereoTest, RejectsUnsupportedStereoEncoding)
+{
+	start();
+	leftPub_->publish(makeImage("camera_link", 1000.0,
+			cv::Mat(kStereoHeight, kStereoWidth, CV_32FC1, cv::Scalar(1.0f)), "32FC1"));
+	rightPub_->publish(makeImage("camera_link", 1000.0,
+			cv::Mat(kStereoHeight, kStereoWidth, CV_8UC1, cv::Scalar(0)), "mono8"));
+	leftInfoPub_->publish(makeCameraInfo(
+			"camera_link", 1000.0, kStereoWidth, kStereoHeight, 0.0, kFx));
+	rightInfoPub_->publish(makeCameraInfo(
+			"camera_link", 1000.0, kStereoWidth, kStereoHeight, -kFx*kBaseline, kFx));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(out_->empty()) << "only 8-bit and mono16 stereo images are supported";
+}
diff --git a/rtabmap_util/test/test_pointcloud_to_depthimage.cpp b/rtabmap_util/test/test_pointcloud_to_depthimage.cpp
new file mode 100644
index 00000000..252797ed
--- /dev/null
+++ b/rtabmap_util/test/test_pointcloud_to_depthimage.cpp
@@ -0,0 +1,370 @@
+/*
+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_util/pointcloud_to_depthimage.hpp>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+
+constexpr int kWidth = 16;
+constexpr int kHeight = 16;
+constexpr double kFx = 100.0;
+
+float pixel32f(const sensor_msgs::msg::Image & img, int row, int col)
+{
+	return *reinterpret_cast<const float *>(&img.data[row * img.step + col * sizeof(float)]);
+}
+
+uint16_t pixel16u(const sensor_msgs::msg::Image & img, int row, int col)
+{
+	return *reinterpret_cast<const uint16_t *>(&img.data[row * img.step + col * sizeof(uint16_t)]);
+}
+}  // namespace
+
+class PointCloudToDepthImageTest : public NodeTest
+{
+protected:
+	void start(const std::vector<rclcpp::Parameter> & overrides = {}, bool withTf = true)
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		if(withTf)
+		{
+			publishStaticTf("camera_link", "lidar");
+		}
+		image32_ = collect<sensor_msgs::msg::Image>("image");
+		image16_ = collect<sensor_msgs::msg::Image>("image_raw");
+		cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(cloudPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForPublisher(image32_->subscription));
+	}
+
+	/// Publishes a cloud and its camera info with identical stamps.
+	void publishFrame(double stamp, const std::vector<cv::Point3f> & points)
+	{
+		cloudPub_->publish(makeXYZCloud("lidar", stamp, points));
+		infoPub_->publish(makeCameraInfo("camera_link", stamp, kWidth, kHeight, 0.0, kFx));
+	}
+
+	/// A block of points straight ahead of the optical axis at @p depth meters.
+	static std::vector<cv::Point3f> blockAt(float depth)
+	{
+		std::vector<cv::Point3f> points;
+		for(int i=-2; i<=2; ++i)
+		{
+			for(int j=-2; j<=2; ++j)
+			{
+				points.push_back(cv::Point3f(0.01f*i, 0.01f*j, depth));
+			}
+		}
+		return points;
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> image16_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+};
+
+TEST_F(PointCloudToDepthImageTest, ProjectsACloudIntoADepthImage)
+{
+	start();
+	publishFrame(1000.0, blockAt(2.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
+
+	const sensor_msgs::msg::Image & img = image32_->back();
+	EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_32FC1);
+	EXPECT_EQ(img.width, uint32_t(kWidth));
+	EXPECT_EQ(img.height, uint32_t(kHeight));
+	EXPECT_EQ(img.header.frame_id, "camera_link")
+		<< "the depth image belongs to the camera, not the cloud";
+
+	// The points sit on the optical axis, so they land on the principal point.
+	EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), 2.0f, 1e-3);
+	// A corner sees nothing.
+	EXPECT_FLOAT_EQ(pixel32f(img, 0, 0), 0.0f);
+}
+
+TEST_F(PointCloudToDepthImageTest, PublishesMillimetersOnImageRaw)
+{
+	start();
+	publishFrame(1000.0, blockAt(2.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !image16_->empty(); }));
+
+	const sensor_msgs::msg::Image & img = image16_->back();
+	EXPECT_EQ(img.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(pixel16u(img, kHeight/2, kWidth/2), 2000) << "2 m expressed in millimeters";
+}
+
+TEST_F(PointCloudToDepthImageTest, EmptyCloudGivesAnAllZeroImage)
+{
+	start();
+	publishFrame(1000.0, {});
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); }))
+		<< "an empty cloud must still produce an image, not a dropped frame";
+
+	const sensor_msgs::msg::Image & img = image32_->back();
+	EXPECT_EQ(img.width, uint32_t(kWidth));
+	for(int row=0; row<kHeight; ++row)
+	{
+		for(int col=0; col<kWidth; ++col)
+		{
+			ASSERT_FLOAT_EQ(pixel32f(img, row, col), 0.0f) << "at " << row << "," << col;
+		}
+	}
+}
+
+TEST_F(PointCloudToDepthImageTest, DecimationShrinksTheImageAndScalesTheModel)
+{
+	start({rclcpp::Parameter("decimation", 2)});
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> infoOut =
+			collect<sensor_msgs::msg::CameraInfo>("image/camera_info");
+	ASSERT_TRUE(waitForPublisher(infoOut->subscription));
+
+	publishFrame(1000.0, blockAt(2.0f));
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty() && !infoOut->empty(); }));
+
+	EXPECT_EQ(image32_->back().width, uint32_t(kWidth)/2);
+	EXPECT_EQ(image32_->back().height, uint32_t(kHeight)/2);
+	EXPECT_NEAR(infoOut->back().p[0], kFx/2.0, 1e-6)
+		<< "the published camera info must match the decimated image";
+	EXPECT_EQ(infoOut->back().width, uint32_t(kWidth)/2);
+}
+
+TEST_F(PointCloudToDepthImageTest, FailsWithoutTheCloudToCameraTransform)
+{
+	start({rclcpp::Parameter("wait_for_transform", 0.0)}, /*withTf=*/false);
+	publishFrame(1000.0, blockAt(2.0f));
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(image32_->empty())
+		<< "without TF the cloud cannot be placed in the camera frame";
+}
+
+TEST_F(PointCloudToDepthImageTest, StaysSilentWithoutASubscriber)
+{
+	// The projection is skipped entirely when neither image topic is subscribed.
+	addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
+	publishStaticTf("camera_link", "lidar");
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub =
+			helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
+			helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
+	ASSERT_TRUE(waitForSubscriber(cloudPub));
+
+	cloudPub->publish(makeXYZCloud("lidar", 1000.0, blockAt(2.0f)));
+	infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, 0.0, kFx));
+	spinFor(std::chrono::milliseconds(300));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> late =
+			collect<sensor_msgs::msg::Image>("image");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+//============================================================================
+// Motion compensation between the cloud stamp and the camera_info stamp
+//============================================================================
+
+/**
+ * With approximate synchronization the cloud and the camera info rarely share a stamp.
+ * When @c fixed_frame_id is set, the node asks TF how the lidar moved over that interval
+ * and folds the displacement into the camera's local transform, so the cloud is projected
+ * from where the camera was at its own stamp instead of where the lidar was.
+ *
+ * The frames follow the usual convention, as in rtabmap's own projectCloudToCamera tests:
+ * the cloud is expressed in a lidar frame with x forward, and the camera is attached to it
+ * through the optical rotation, so a point straight ahead lands on the principal point.
+ */
+class PointCloudToDepthImageMotionTest : public NodeTest
+{
+protected:
+	static constexpr double kSpeed = 1.0;        ///< m/s
+	static constexpr double kCloudStamp = 1000.0;
+	static constexpr double kInfoDelay = 0.04;   ///< the camera info lags the cloud by this
+	static constexpr float kRange = 2.0f;        ///< distance to the point, meters
+	static constexpr int kFrames = 3;            ///< see publishFrames()
+	static constexpr double kPeriod = 0.2;       ///< seconds between frames
+
+	/// Distance travelled between the two stamps: what the node has to compensate for.
+	static double travelled() { return kSpeed * kInfoDelay; }
+
+	/// The same distance seen sideways by the camera, in pixels.
+	static int shiftInPixels() { return int(kFx * travelled() / double(kRange)); }
+
+	/**
+	 * @param axis  'x' to drive straight at the point, 'y' to drive sideways past it,
+	 *              '0' to stand still
+	 */
+	void start(const std::vector<rclcpp::Parameter> & overrides, char axis = 'x')
+	{
+		addNode(std::make_shared<rtabmap_util::PointCloudToDepthImage>(
+				rclcpp::NodeOptions().parameter_overrides(overrides)));
+		// The camera is bolted to the lidar, looking the same way: x right, y down,
+		// z forward against the lidar's x forward, y left, z up.
+		publishStaticTfRPY("lidar", "camera_link", -M_PI/2.0, 0.0, -M_PI/2.0);
+		if(axis != '0')
+		{
+			publishOdomMotion(axis);
+		}
+		image32_ = collect<sensor_msgs::msg::Image>("image");
+		cloudPub_ = helper()->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
+		infoPub_ = helper()->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 10);
+		ASSERT_TRUE(waitForSubscriber(cloudPub_));
+		ASSERT_TRUE(waitForSubscriber(infoPub_));
+		ASSERT_TRUE(waitForPublisher(image32_->subscription));
+	}
+
+	/// Publishes odom -> lidar moving at kSpeed, covering every stamp used below.
+	void publishOdomMotion(char axis)
+	{
+		rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub =
+				helper()->create_publisher<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
+		spinFor(std::chrono::milliseconds(100));   // let the node's listener subscribe
+
+		// One sample per cloud stamp and per camera info stamp, plus a margin on each side
+		// so that nothing has to be extrapolated.
+		std::vector<double> elapsedSamples;
+		elapsedSamples.push_back(-kPeriod);
+		for(int k=0; k<kFrames; ++k)
+		{
+			elapsedSamples.push_back(kPeriod * double(k));
+			elapsedSamples.push_back(kPeriod * double(k) + kInfoDelay);
+		}
+		elapsedSamples.push_back(kPeriod * double(kFrames));
+
+		for(size_t i=0; i<elapsedSamples.size(); ++i)
+		{
+			const double elapsed = elapsedSamples[i];
+			geometry_msgs::msg::TransformStamped t;
+			t.header.stamp = stampOf(kCloudStamp + elapsed);
+			t.header.frame_id = "odom";
+			t.child_frame_id = "lidar";
+			(axis == 'x' ? t.transform.translation.x : t.transform.translation.y) =
+					kSpeed * elapsed;
+			t.transform.rotation.w = 1.0;
+			tf2_msgs::msg::TFMessage msg;
+			msg.transforms.push_back(t);
+			tfPub->publish(msg);
+		}
+		spinFor(std::chrono::milliseconds(200));   // let the buffer fill
+		tfPub_ = tfPub;                            // keep the publisher alive
+	}
+
+	/**
+	 * @brief Publishes kFrames cloud/camera_info pairs, the info @p delay seconds late.
+	 *
+	 * Each cloud holds a single point straight ahead of the lidar. The speed is constant,
+	 * so every pair needs the same correction and the first output is enough to assert on.
+	 * A burst is needed because ApproximateTime emits nothing for a lone pair whose stamps
+	 * differ: it cannot rule out a better match still to come.
+	 */
+	void publishFrames(double delay)
+	{
+		for(int k=0; k<kFrames; ++k)
+		{
+			const double elapsed = kPeriod * double(k);
+			cloudPub_->publish(makeXYZCloud("lidar", kCloudStamp + elapsed,
+					{cv::Point3f(kRange, 0.0f, 0.0f)}));
+			infoPub_->publish(makeCameraInfo(
+					"camera_link", kCloudStamp + elapsed + delay, kWidth, kHeight, 0.0, kFx));
+		}
+	}
+
+	/// The column the single point landed in on @p row, or -1 if that row is empty.
+	static int hitColumn(const sensor_msgs::msg::Image & img, int row)
+	{
+		for(int col=0; col<int(img.width); ++col)
+		{
+			if(pixel32f(img, row, col) != 0.0f) { return col; }
+		}
+		return -1;
+	}
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> image32_;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloudPub_;
+	rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub_;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tfPub_;
+};
+
+TEST_F(PointCloudToDepthImageMotionTest, NoShiftWhenTheStampsMatch)
+{
+	// The control case: same frames, same scene, nothing to compensate. It also proves the
+	// optical rotation is right, since a wrongly oriented camera sees nothing at all.
+	start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))});
+	publishFrames(0.0);
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
+
+	const sensor_msgs::msg::Image & img = image32_->front();
+	EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
+		<< "a point straight ahead belongs at the principal point";
+	EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange, 1e-3);
+}
+
+TEST_F(PointCloudToDepthImageMotionTest, ClosesTheGapWhenDrivingAtThePoint)
+{
+	// The camera info is 40 ms younger than the cloud and the robot closes in at 1 m/s, so
+	// by the time of the exposure the point is 4 cm nearer than the lidar measured it.
+	start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'x');
+	publishFrames(kInfoDelay);
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
+
+	const sensor_msgs::msg::Image & img = image32_->front();
+	EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2)
+		<< "driving straight at the point does not move it across the image";
+	EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2), kRange - float(travelled()), 1e-3)
+		<< "the depth must be corrected for the distance travelled";
+}
+
+TEST_F(PointCloudToDepthImageMotionTest, ShiftsThePointWhenDrivingPastIt)
+{
+	// Moving sideways instead: the point slides across the image by fx*d/Z pixels, and
+	// stays at the same range.
+	start({rclcpp::Parameter("fixed_frame_id", std::string("odom"))}, 'y');
+	publishFrames(kInfoDelay);
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
+
+	const sensor_msgs::msg::Image & img = image32_->front();
+	EXPECT_EQ(hitColumn(img, kHeight/2), kWidth/2 + shiftInPixels())
+		<< "the robot moved left, so the point must appear further right";
+	EXPECT_NEAR(pixel32f(img, kHeight/2, kWidth/2 + shiftInPixels()), kRange, 1e-3)
+		<< "only the bearing changed, not the range";
+	EXPECT_FLOAT_EQ(pixel32f(img, kHeight/2, kWidth/2), 0.0f)
+		<< "and it is no longer at the principal point";
+}
+
+TEST_F(PointCloudToDepthImageMotionTest, IgnoresTheStampDifferenceWithoutAFixedFrame)
+{
+	// Without fixed_frame_id there is nothing to measure the motion against, so the cloud
+	// is projected as if both messages were captured at the same instant. That is why the
+	// node logs a fatal error when approximate sync is used without one.
+	start({rclcpp::Parameter("fixed_frame_id", std::string(""))}, 'x');
+	publishFrames(kInfoDelay);
+	ASSERT_TRUE(spinUntil([&]() { return !image32_->empty(); })) << "no depth image";
+
+	EXPECT_NEAR(pixel32f(image32_->front(), kHeight/2, kWidth/2), kRange, 1e-3)
+		<< "no fixed frame, no compensation";
+}
+
+TEST_F(PointCloudToDepthImageMotionTest, FailsWhenTheFixedFrameIsUnknown)
+{
+	// fixed_frame_id is set but odom -> lidar was never published: the displacement cannot
+	// be measured, and projecting anyway would silently misplace the points.
+	start({rclcpp::Parameter("fixed_frame_id", std::string("odom")),
+		   rclcpp::Parameter("wait_for_transform", 0.0)}, '0');
+	publishFrames(kInfoDelay);
+	spinFor(std::chrono::milliseconds(400));
+
+	EXPECT_TRUE(image32_->empty());
+}
diff --git a/rtabmap_util/test/test_rgbd_relay.cpp b/rtabmap_util/test/test_rgbd_relay.cpp
new file mode 100644
index 00000000..6489bef3
--- /dev/null
+++ b/rtabmap_util/test/test_rgbd_relay.cpp
@@ -0,0 +1,350 @@
+/*
+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_util/rgbd_relay.hpp>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/utilite/UException.h>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+class RGBDRelayTest : public NodeTest
+{
+protected:
+	/// Starts the node, wires up the input publisher and the output collector.
+	void start(bool compress, bool uncompress)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
+				.parameter_overrides({
+					rclcpp::Parameter("compress", compress),
+					rclcpp::Parameter("uncompress", uncompress)})));
+
+		out_ = collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
+		pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		ASSERT_TRUE(waitForSubscriber(pub_));
+		ASSERT_TRUE(waitForPublisher(out_->subscription));
+	}
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out_;
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub_;
+};
+
+TEST_F(RGBDRelayTest, RepublishesUnchangedByDefault)
+{
+	start(/*compress=*/false, /*uncompress=*/false);
+
+	const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	pub_->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_EQ(got.header.frame_id, in.header.frame_id);
+	EXPECT_EQ(got.rgb.data, in.rgb.data) << "the payload must be passed through untouched";
+	EXPECT_EQ(got.depth.data, in.depth.data);
+	EXPECT_TRUE(got.rgb_compressed.data.empty());
+	EXPECT_TRUE(got.depth_compressed.data.empty());
+	EXPECT_NEAR(got.rgb_camera_info.p[0], in.rgb_camera_info.p[0], 1e-9);
+}
+
+TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked)
+{
+	start(/*compress=*/true, /*uncompress=*/false);
+
+	pub_->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed";
+	EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed";
+	// Depth is lossless png; color is jpg.
+	EXPECT_EQ(got.depth_compressed.format, "png");
+	EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well";
+}
+
+TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg)
+{
+	// When the camera infos describe a stereo pair, the "depth" slot holds the right
+	// image and is compressed as JPEG, not as a lossless depth PNG.
+	start(/*compress=*/true, /*uncompress=*/false);
+
+	pub_->publish(makeStereoRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_FALSE(got.depth_compressed.data.empty());
+	EXPECT_NE(got.depth_compressed.format, "png")
+		<< "a stereo right image must not take the depth PNG path";
+	EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos)
+		<< "expected a jpeg format, got \"" << got.depth_compressed.format << "\"";
+	EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay";
+}
+
+TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng)
+{
+	// The same call with no baseline is treated as color + depth instead.
+	start(/*compress=*/true, /*uncompress=*/false);
+
+	pub_->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_FALSE(got.depth_compressed.data.empty());
+	EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless";
+	EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo";
+}
+
+TEST_F(RGBDRelayTest, UncompressRestoresAStereoRightImage)
+{
+	// The uncompress path branches on the format: "jpg" means a stereo right image and
+	// goes through cv_bridge, anything else is a depth image and goes through rtabmap.
+	start(/*compress=*/false, /*uncompress=*/true);
+
+	rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+	const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(60));
+	cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", right)
+			.toCompressedImageMsg(in.depth_compressed, cv_bridge::JPG);
+	in.depth = sensor_msgs::msg::Image();
+
+	pub_->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_FALSE(got.depth.data.empty()) << "the right image must be decompressed";
+	EXPECT_EQ(got.depth.encoding, "mono8") << "restored as an 8-bit image, not depth";
+	EXPECT_EQ(got.depth.width, 8u);
+	EXPECT_EQ(got.depth.height, 8u);
+}
+
+TEST_F(RGBDRelayTest, UncompressRestoresRawImages)
+{
+	start(/*compress=*/false, /*uncompress=*/true);
+
+	// Feed it a message that carries only compressed depth.
+	rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(1500));
+	in.depth = sensor_msgs::msg::Image();
+	in.depth_compressed.header = in.header;
+	in.depth_compressed.format = "png";
+	in.depth_compressed.data = rtabmap::compressImage(depth, ".png");
+
+	pub_->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	const rtabmap_msgs::msg::RGBDImage & got = out_->back();
+	ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed";
+	EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(got.depth.width, 8u);
+	EXPECT_EQ(got.depth.height, 8u);
+}
+
+TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne)
+{
+	// A message may carry both. The raw image is already usable, so decompressing the
+	// other copy would be wasted work -- and the two paths must agree, as the depth
+	// branch below does.
+	start(/*compress=*/false, /*uncompress=*/true);
+
+	rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	// A compressed copy whose content differs, so it is obvious which one was used.
+	cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8",
+			cv::Mat(8, 8, CV_8UC3, cv::Scalar(200, 200, 200)))
+					.toCompressedImageMsg(in.rgb_compressed, cv_bridge::PNG);
+
+	pub_->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); }));
+
+	ASSERT_FALSE(out_->back().rgb.data.empty());
+	EXPECT_EQ(out_->back().rgb.data, in.rgb.data)
+		<< "the raw image must be forwarded, not the decompressed copy";
+}
+
+TEST_F(RGBDRelayTest, StaysSilentWithoutASubscriber)
+{
+	// No collector, so the relay's output has no subscriber and it must not do the work.
+	addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()));
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	spinFor(std::chrono::milliseconds(300));
+
+	// Subscribing only now must not retroactively receive anything.
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> late =
+			collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
+	spinFor(std::chrono::milliseconds(200));
+	EXPECT_TRUE(late->empty());
+}
+
+/// Feeds a compressed right image in @p format through the uncompress path.
+class RGBDRelayRightImageTest : public NodeTest
+{
+protected:
+	rtabmap_msgs::msg::RGBDImage relay(cv_bridge::Format format)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDRelay>(rclcpp::NodeOptions()
+				.parameter_overrides({rclcpp::Parameter("uncompress", true)})));
+
+		std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out =
+				collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+				helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+		EXPECT_TRUE(waitForPublisher(out->subscription));
+
+		rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+		cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
+				cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
+						.toCompressedImageMsg(in.depth_compressed, format);
+		in.depth = sensor_msgs::msg::Image();
+
+		pub->publish(in);
+		EXPECT_TRUE(spinUntil([&]() { return !out->empty(); }));
+		return out->empty() ? rtabmap_msgs::msg::RGBDImage() : out->back();
+	}
+};
+
+TEST_F(RGBDRelayRightImageTest, UncompressesAJpegRightImage)
+{
+	const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::JPG);
+	ASSERT_FALSE(got.depth.data.empty());
+	EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(got.depth.step, 8u);
+}
+
+TEST_F(RGBDRelayRightImageTest, UncompressesAPngRightImage)
+{
+	// A losslessly compressed right image must not be mistaken for depth and abort.
+	const rtabmap_msgs::msg::RGBDImage got = relay(cv_bridge::PNG);
+	ASSERT_FALSE(got.depth.data.empty());
+	EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(got.depth.step, 8u);
+}
+
+/// QoS of the two sides, set independently through qos_sub and qos_pub.
+///
+/// A reliable subscription refuses to match a best-effort publisher, while a best-effort
+/// subscription matches either. Every assertion below rests on that asymmetry: whether a
+/// connection is established at all is what tells us which reliability the node picked.
+class RGBDRelayQosTest : public NodeTest
+{
+protected:
+	enum Reliability { kSystemDefault = 0, kReliable = 1, kBestEffort = 2 };
+
+	void startRelay(const std::vector<rclcpp::Parameter> & params)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDRelay>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+	}
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr input(Reliability reliability)
+	{
+		rclcpp::QoS qos(10);
+		reliability == kBestEffort ? qos.best_effort() : qos.reliable();
+		return helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", qos);
+	}
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> output(Reliability reliability)
+	{
+		rclcpp::QoS qos(10);
+		reliability == kBestEffort ? qos.best_effort() : qos.reliable();
+		return collect<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay", qos);
+	}
+};
+
+TEST_F(RGBDRelayQosTest, BridgesABestEffortSourceToAReliableConsumer)
+{
+	// The point of splitting the parameter: a sensor publishing best effort feeding a
+	// consumer that only accepts reliable. Neither could talk to the other directly.
+	startRelay({rclcpp::Parameter("qos_sub", int(kBestEffort)),
+				rclcpp::Parameter("qos_pub", int(kReliable))});
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
+	ASSERT_TRUE(waitForSubscriber(pub)) << "a best-effort source must reach the relay";
+	ASSERT_TRUE(waitForPublisher(out->subscription))
+		<< "a reliable consumer must be able to subscribe to the relayed topic";
+
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
+	EXPECT_EQ(out->back().header.frame_id, "camera_link");
+}
+
+TEST_F(RGBDRelayQosTest, QosSubOverridesQosOnTheInputOnly)
+{
+	// qos says reliable, which a best-effort source could not match; qos_sub overrides it.
+	startRelay({rclcpp::Parameter("qos", int(kReliable)),
+				rclcpp::Parameter("qos_sub", int(kBestEffort))});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
+	EXPECT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
+
+	// The output side kept qos, so a reliable consumer still matches it.
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
+	EXPECT_TRUE(waitForPublisher(out->subscription))
+		<< "qos_sub must not affect the publisher";
+}
+
+TEST_F(RGBDRelayQosTest, QosPubOverridesQosOnTheOutputOnly)
+{
+	// qos says best effort, which no reliable consumer could match; qos_pub overrides it.
+	startRelay({rclcpp::Parameter("qos", int(kBestEffort)),
+				rclcpp::Parameter("qos_pub", int(kReliable))});
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
+	EXPECT_TRUE(waitForPublisher(out->subscription))
+		<< "qos_pub must win over qos on the publisher";
+
+	// The input side kept qos, so it is still best effort and accepts a best-effort source.
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
+	EXPECT_TRUE(waitForSubscriber(pub)) << "qos_pub must not affect the subscription";
+}
+
+TEST_F(RGBDRelayQosTest, BothSidesFallBackToQos)
+{
+	// Only qos is given, so both sides must be best effort -- as before the split.
+	startRelay({rclcpp::Parameter("qos", int(kBestEffort))});
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kBestEffort);
+	EXPECT_TRUE(waitForSubscriber(pub)) << "the subscription must have followed qos";
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
+	spinFor(std::chrono::milliseconds(500));
+	EXPECT_EQ(out->subscription->get_publisher_count(), 0u)
+		<< "the publisher must have followed qos too: best effort, so a reliable "
+		   "consumer cannot match it";
+}
+
+TEST_F(RGBDRelayQosTest, HonorsTheConfiguredQueueDepths)
+{
+	// Queue depth is not directly observable from outside, so this only pins down that
+	// the parameters are accepted and the relay still works with them set.
+	startRelay({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
+
+	std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> out = output(kReliable);
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub = input(kReliable);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(out->subscription));
+
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !out->empty(); }));
+	EXPECT_EQ(out->back().header.frame_id, "camera_link");
+}
+
+TEST_F(RGBDRelayQosTest, RejectsAZeroQueueDepth)
+{
+	// rclcpp::QoS(0) is not a meaningful depth, so say so at construction rather than
+	// leaving the relay silently misconfigured.
+	EXPECT_THROW(
+		startRelay({rclcpp::Parameter("queue_sub", 0)}),
+		UException);
+}
diff --git a/rtabmap_util/test/test_rgbd_split.cpp b/rtabmap_util/test/test_rgbd_split.cpp
new file mode 100644
index 00000000..f032d998
--- /dev/null
+++ b/rtabmap_util/test/test_rgbd_split.cpp
@@ -0,0 +1,397 @@
+/*
+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_util/rgbd_split.hpp>
+
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/utilite/UException.h>
+
+using namespace rtabmap_util_test;
+
+namespace {
+::testing::Environment * const kEnv = registerRclcppEnvironment();
+}
+
+class RGBDSplitTest : public NodeTest {};
+
+TEST_F(RGBDSplitTest, SplitsIntoImageAndCameraInfoTopics)
+{
+	addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+
+	// The node derives its output topics from the input topic name.
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/rgb/camera_info");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() {
+		return !rgb->empty() && !depth->empty() && !rgbInfo->empty() && !depthInfo->empty();
+	})) << "not all four outputs were published";
+
+	EXPECT_EQ(rgb->back().encoding, "bgr8");
+	EXPECT_EQ(rgb->back().data, in.rgb.data);
+	EXPECT_EQ(depth->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+	EXPECT_EQ(depth->back().data, in.depth.data);
+
+	EXPECT_NEAR(rgbInfo->back().p[0], in.rgb_camera_info.p[0], 1e-9);
+	EXPECT_EQ(rgbInfo->back().width, in.rgb_camera_info.width);
+	EXPECT_NEAR(depthInfo->back().p[0], in.depth_camera_info.p[0], 1e-9);
+}
+
+TEST_F(RGBDSplitTest, FallsBackToTheInputHeaderForTheDepthCameraInfo)
+{
+	addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	// Depth camera info with no frame id: the node fills it from the message header.
+	rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	in.depth_camera_info.header.frame_id = "";
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !depthInfo->empty(); }));
+
+	EXPECT_EQ(depthInfo->back().header.frame_id, "camera_link");
+}
+
+TEST_F(RGBDSplitTest, PassesAStereoPairThroughUnchanged)
+{
+	// The node does not distinguish stereo from depth: it forwards whatever is in the
+	// "depth" slot, so a stereo right image is published on .../depth/image along with
+	// the right camera info carrying the baseline.
+	addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/depth/camera_info");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(right->subscription));
+
+	const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !right->empty() && !rightInfo->empty(); }));
+
+	EXPECT_EQ(right->back().encoding, "mono8") << "the right image is forwarded as-is";
+	EXPECT_EQ(right->back().data, in.depth.data);
+	EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the consumer";
+}
+
+TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding)
+{
+	// rtabmap compresses depth as a PNG whose format string cv_bridge cannot interpret.
+	// The node must decode it itself and label it 16UC1, not mono8: the buffer is two
+	// bytes per pixel and a wrong encoding makes every consumer misread it.
+	addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	const cv::Mat original(8, 8, CV_16UC1, cv::Scalar(1500));
+	in.depth = sensor_msgs::msg::Image();
+	in.depth_compressed.header = in.header;
+	in.depth_compressed.format = "png";
+	in.depth_compressed.data = rtabmap::compressImage(original, ".png");
+
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }));
+
+	const sensor_msgs::msg::Image & got = depth->back();
+	EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1)
+		<< "a 16-bit depth buffer must not be labeled mono8";
+	EXPECT_EQ(got.width, 8u);
+	EXPECT_EQ(got.height, 8u);
+	ASSERT_EQ(got.step, 16u) << "two bytes per pixel";
+	EXPECT_EQ(*reinterpret_cast<const uint16_t *>(&got.data[0]), 1500)
+		<< "and the values must survive the round trip";
+}
+
+/// Feeds a compressed right image in @p format and returns what lands on depth/image.
+class RGBDSplitRightImageTest : public NodeTest
+{
+protected:
+	sensor_msgs::msg::Image split(cv_bridge::Format format)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
+
+		std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
+				collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+		rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+				helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+		EXPECT_TRUE(waitForSubscriber(pub));
+		EXPECT_TRUE(waitForPublisher(right->subscription));
+
+		rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+		cv_bridge::CvImage(std_msgs::msg::Header(), "mono8",
+				cv::Mat(8, 8, CV_8UC1, cv::Scalar(60)))
+						.toCompressedImageMsg(in.depth_compressed, format);
+		in.depth = sensor_msgs::msg::Image();
+
+		pub->publish(in);
+		EXPECT_TRUE(spinUntil([&]() { return !right->empty(); }))
+			<< "the right image must be decompressed, not rejected";
+		return right->empty() ? sensor_msgs::msg::Image() : right->back();
+	}
+};
+
+TEST_F(RGBDSplitRightImageTest, DecompressesAJpegRightImage)
+{
+	// What stereo_sync emits.
+	const sensor_msgs::msg::Image got = split(cv_bridge::JPG);
+	EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(got.step, 8u) << "one byte per pixel, not mistaken for 16-bit depth";
+}
+
+TEST_F(RGBDSplitRightImageTest, DecompressesAPngRightImage)
+{
+	// Nothing forbids a producer from compressing the right image losslessly, and a
+	// stereo pipeline may prefer it since JPEG artifacts hurt matching. Going by the
+	// format string alone would send this down the depth path and abort on the assert.
+	const sensor_msgs::msg::Image got = split(cv_bridge::PNG);
+	EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::MONO8);
+	EXPECT_EQ(got.step, 8u);
+}
+
+/// Queue depths and the reach of the qos parameter.
+class RGBDSplitQosTest : public NodeTest
+{
+protected:
+	void startSplit(const std::vector<rclcpp::Parameter> & params)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDSplit>(
+				rclcpp::NodeOptions().parameter_overrides(params)));
+	}
+};
+
+TEST_F(RGBDSplitQosTest, HonorsTheConfiguredQueueDepths)
+{
+	// Queue depth is not observable from outside, so this pins down that the parameters
+	// are accepted and the node still splits with them set.
+	startSplit({rclcpp::Parameter("queue_sub", 20), rclcpp::Parameter("queue_pub", 10)});
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
+	EXPECT_EQ(rgb->back().encoding, "bgr8");
+}
+
+TEST_F(RGBDSplitQosTest, RejectsAZeroQueueDepth)
+{
+	EXPECT_THROW(startSplit({rclcpp::Parameter("queue_pub", 0)}), UException);
+}
+
+TEST_F(RGBDSplitQosTest, AppliesQosToTheCameraInfoPublishersToo)
+{
+	// A best-effort node must be best effort on every output, camera infos included:
+	// a reliable consumer must not match any of them.
+	startSplit({rclcpp::Parameter("qos", 2)});
+
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rgbInfo =
+			collect<sensor_msgs::msg::CameraInfo>(
+					"rgbd_image/rgb/camera_info", rclcpp::QoS(10).reliable());
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> depthInfo =
+			collect<sensor_msgs::msg::CameraInfo>(
+					"rgbd_image/depth/camera_info", rclcpp::QoS(10).reliable());
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_EQ(rgbInfo->subscription->get_publisher_count(), 0u)
+		<< "the rgb camera info publisher ignored qos";
+	EXPECT_EQ(depthInfo->subscription->get_publisher_count(), 0u)
+		<< "the depth camera info publisher ignored qos";
+}
+
+/// Output topic naming, controlled by the stereo parameter.
+class RGBDSplitStereoNamingTest : public NodeTest
+{
+protected:
+	void startSplit(bool stereo)
+	{
+		addNode(std::make_shared<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
+				.parameter_overrides({rclcpp::Parameter("stereo", stereo)})));
+	}
+};
+
+TEST_F(RGBDSplitStereoNamingTest, PublishesOnLeftAndRightWhenStereoIsSet)
+{
+	startSplit(/*stereo=*/true);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
+			collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
+			collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> leftInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/left/camera_info");
+	std::shared_ptr<Collector<sensor_msgs::msg::CameraInfo>> rightInfo =
+			collect<sensor_msgs::msg::CameraInfo>("rgbd_image/right/camera_info");
+
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(left->subscription));
+	ASSERT_TRUE(waitForPublisher(right->subscription));
+
+	const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() {
+		return !left->empty() && !right->empty() && !leftInfo->empty() && !rightInfo->empty();
+	})) << "not all four outputs were published";
+
+	EXPECT_EQ(left->back().data, in.rgb.data) << "the rgb slot feeds the left topic";
+	EXPECT_EQ(right->back().data, in.depth.data) << "the depth slot feeds the right topic";
+	EXPECT_LT(rightInfo->back().p[3], 0.0) << "the baseline must reach the right camera info";
+}
+
+TEST_F(RGBDSplitStereoNamingTest, DoesNotPublishOnRgbAndDepthWhenStereoIsSet)
+{
+	// The two namings are exclusive: nothing must be left publishing the old names.
+	startSplit(/*stereo=*/true);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	spinFor(std::chrono::milliseconds(500));
+
+	EXPECT_EQ(rgb->subscription->get_publisher_count(), 0u);
+	EXPECT_EQ(depth->subscription->get_publisher_count(), 0u);
+}
+
+TEST_F(RGBDSplitStereoNamingTest, KeepsRgbAndDepthByDefault)
+{
+	startSplit(/*stereo=*/false);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> left =
+			collect<sensor_msgs::msg::Image>("rgbd_image/left/image");
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+	EXPECT_EQ(left->subscription->get_publisher_count(), 0u)
+		<< "left/right naming must be opt-in";
+}
+
+TEST_F(RGBDSplitStereoNamingTest, StillPublishesADepthImageOnRightWithStereoSet)
+{
+	// A depth image with stereo set is a misconfiguration: the node warns (once) but
+	// keeps forwarding, so an existing pipeline is never silently broken.
+	startSplit(/*stereo=*/true);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> right =
+			collect<sensor_msgs::msg::Image>("rgbd_image/right/image");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(right->subscription));
+
+	// makeRGBDImage carries 16UC1 depth, not a right image.
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !right->empty(); }))
+		<< "the image must still be forwarded, warning or not";
+	EXPECT_EQ(right->back().encoding, sensor_msgs::image_encodings::TYPE_16UC1);
+}
+
+TEST_F(RGBDSplitStereoNamingTest, StillPublishesARightImageOnDepthWithStereoUnset)
+{
+	// The inverse misconfiguration, and the one this node has always allowed: a stereo
+	// pair with stereo left false. It warns, but the right image must still come out.
+	startSplit(/*stereo=*/false);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(depth->subscription));
+
+	const rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0);
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !depth->empty(); }))
+		<< "the right image must still be forwarded, warning or not";
+	EXPECT_EQ(depth->back().encoding, "mono8");
+	EXPECT_EQ(depth->back().data, in.depth.data);
+}
+
+TEST_F(RGBDSplitStereoNamingTest, DoesNotWarnOnAnEmptySecondHalf)
+{
+	// A color-only RGBDImage leaves the depth slot empty, whose encoding is "". That
+	// must not be mistaken for a right image: nothing is published, nothing to warn about.
+	startSplit(/*stereo=*/false);
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image");
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
+			collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("rgbd_image", 10);
+	ASSERT_TRUE(waitForSubscriber(pub));
+	ASSERT_TRUE(waitForPublisher(rgb->subscription));
+
+	rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0);
+	in.depth = sensor_msgs::msg::Image();
+	pub->publish(in);
+	ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
+	spinFor(std::chrono::milliseconds(300));
+
+	EXPECT_EQ(rgb->back().encoding, "bgr8") << "the color half is unaffected";
+	if(!depth->empty())
+	{
+		EXPECT_TRUE(depth->back().data.empty())
+			<< "an absent depth image must not turn into a non-empty one";
+	}
+}
+
+TEST_F(RGBDSplitQosTest, QosSubAndQosPubOverrideQosPerSide)
+{
+	// qos says reliable, which a best-effort source could not match; qos_sub overrides
+	// it, while qos_pub keeps the outputs reliable for a strict consumer.
+	startSplit({rclcpp::Parameter("qos", 1),
+				rclcpp::Parameter("qos_sub", 2),
+				rclcpp::Parameter("qos_pub", 1)});
+
+	std::shared_ptr<Collector<sensor_msgs::msg::Image>> rgb =
+			collect<sensor_msgs::msg::Image>("rgbd_image/rgb/image", rclcpp::QoS(10).reliable());
+	rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
+			helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>(
+					"rgbd_image", rclcpp::QoS(10).best_effort());
+	ASSERT_TRUE(waitForSubscriber(pub)) << "qos_sub must win over qos on the subscription";
+	ASSERT_TRUE(waitForPublisher(rgb->subscription)) << "qos_pub must keep the output reliable";
+
+	pub->publish(makeRGBDImage("camera_link", 1000.0));
+	ASSERT_TRUE(spinUntil([&]() { return !rgb->empty(); }));
+	EXPECT_EQ(rgb->back().encoding, "bgr8");
+}
diff --git a/rtabmap_viz/CMakeLists.txt b/rtabmap_viz/CMakeLists.txt
index add21b02..a5e043d3 100644
--- a/rtabmap_viz/CMakeLists.txt
+++ b/rtabmap_viz/CMakeLists.txt
@@ -6,21 +6,22 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 if(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64")
-  # issues #1285 #1288
+  # issues #1285 #1288 (best-effort probes: not REQUIRED, a phantom miss under
+  # emulated arm64 should not fail the build)
   find_library(
     builtin_interfaces__rosidl_generator_c_LIB NAMES builtin_interfaces__rosidl_generator_c
     PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
   find_library(
     rcutils_LIB NAMES rcutils
     PATHS "/opt/ros/$ENV{ROS_DISTRO}/lib"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
   find_library(
     crypto_LIB NAMES crypto
     PATHS "/usr/lib/${CMAKE_SYSTEM_PROCESSOR}-linux-gnu"
-    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH REQUIRED
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
   )
 endif()
 
diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml
index 420b9335..d39b6acf 100644
--- a/rtabmap_viz/package.xml
+++ b/rtabmap_viz/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>rtabmap_viz</name>
-  <version>0.23.7</version>
+  <version>0.23.13</version>
   <description>RTAB-Map's visualization package.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp
index 9190794b..61fd4f72 100644
--- a/rtabmap_viz/src/GuiWrapper.cpp
+++ b/rtabmap_viz/src/GuiWrapper.cpp
@@ -71,7 +71,7 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :
 		frameId_("base_link"),
 		odomFrameId_(""),
 		waitForTransform_(0.2), // 200 ms
-		odomSensorSync_(false),
+		odomSensorSync_(true),
 		maxOdomUpdateRate_(10)
 {
 	tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
diff --git a/rtabmap_viz/src/rgbd_image_viewer.cpp b/rtabmap_viz/src/rgbd_image_viewer.cpp
index d9fd1953..0cc36c81 100644
--- a/rtabmap_viz/src/rgbd_image_viewer.cpp
+++ b/rtabmap_viz/src/rgbd_image_viewer.cpp
@@ -165,6 +165,21 @@ void RGBDImageViewer::callback(
         QMetaObject::invokeMethod(warningLabel_, "clear");
     }
 
+    // rgbdImageFromROS() does not copy the pixels: the SensorData points into the ROS
+    // message buffers, which the subscription queue recycles as soon as this callback
+    // returns. The event below is posted asynchronously and so outlives the callback,
+    // therefore the images must be deep-copied first.
+    if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty()) {
+        cv::Mat image = data.imageRaw().clone();
+        cv::Mat depthOrRight = data.depthOrRightRaw().clone();
+        if(!data.stereoCameraModels().empty()) {
+            data.setStereoImage(image, depthOrRight, data.stereoCameraModels());
+        }
+        else {
+            data.setRGBDImage(image, depthOrRight, data.cameraModels());
+        }
+    }
+
     this->post(new rtabmap::SensorEvent(data));
 }
 
diff --git a/tools/set_doc_distro.sh b/tools/set_doc_distro.sh
new file mode 100755
index 00000000..7e9c07b9
--- /dev/null
+++ b/tools/set_doc_distro.sh
@@ -0,0 +1,52 @@
+#!/usr/bin/env bash
+#
+# Rewrites the ROS distro in every docs.ros.org link of the documentation pages.
+#
+# The distro cannot be resolved at documentation build time: rosdoc2 has no notion of one
+# and injects nothing a Markdown page could read, and docs.ros.org has no distro-agnostic
+# URL for message types. So the distro is baked into the links, and this script is how it
+# gets flipped on a per-distro branch.
+#
+# Usage:
+#   tools/set_doc_distro.sh jazzy [path ...]
+#
+# Rewrites every *.md under the given paths, defaulting to the whole repository. Note that
+# it cannot tell a link meant to track the branch from one that deliberately names a
+# distro -- the root README's "Humble minimum required" note, for instance -- so pass
+# explicit paths when that matters.
+#
+# On a distro branch, treat the documentation as *derived* rather than hand-edited, and
+# the merge from the development branch never conflicts:
+#
+#   git merge ros2
+#   git checkout ros2 -- '*/doc' '*/README.md'   # always take the upstream pages
+#   tools/set_doc_distro.sh humble               # then re-stamp the distro
+#   git commit -a
+#
+set -euo pipefail
+
+distro=${1:-}
+if [ -z "$distro" ]; then
+	echo "usage: $(basename "$0") <distro>   e.g. $(basename "$0") jazzy" >&2
+	exit 1
+fi
+
+# An explicit list rather than a wildcard: docs.ros.org also serves /en/api/, which is the
+# legacy ROS 1 message documentation and must not be rewritten into a distro.
+known='humble|iron|jazzy|kilted|lyrical|rolling'
+
+root=$(cd "$(dirname "$0")/.." && pwd)
+shift || true
+paths=("$@")
+[ ${#paths[@]} -eq 0 ] && paths=("$root")
+
+mapfile -t files < <(grep -rlE "docs\.ros\.org/en/($known)/" --include='*.md' "${paths[@]}")
+
+if [ ${#files[@]} -eq 0 ]; then
+	echo "no documentation pages with a distro link found" >&2
+	exit 0
+fi
+
+sed -i -E "s#docs\.ros\.org/en/($known)/#docs.ros.org/en/$distro/#g" "${files[@]}"
+echo "set distro to '$distro' in ${#files[@]} file(s):"
+printf '  %s\n' "${files[@]#$root/}"