rtabmap_odom tests and doc (#1456)

* rtabmap_odom tests and doc

* opengv note

* added ci checks or humble-latest flaky dep cmake errors

* Added real data tests for rgbd_odom and stereo_odom

* added real data for icp_odometry's deskewing test

* fixing json cmake error on lyrical/rolling

* test 2d icp odom deskewing branch

* first review of existing OdometryROS tests

* testing with imu used as guess

* tested imu arrivals sync

* Fixed odom reset on right pose when guess frame id is used

* fixing header errors in ci >=lyrical

* Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test

* Added stereo odom support for features-only frames. Added multicam stereo tests.

* forcing latest rtabmap version

* updated OdometryROS API

* ci: dont build non-latest docker in pull requests

* splitting docker jobs

* doc edit

* Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet)

* updated stereo doc

* ficing rolling ci (rviz Ogre header)

* Added test coverage of alll rgbd_image callbacks

* fixing rolling ci

* making docker ci build/run the tests on pull requests

* fixing ros2 ci testing

* improved sync callback coverage

* improving stereo_odometry test coverage

* improved icp_odometry test coverage

* lyrical voxel_grid ptr error

* make multicam tests working as well without opengv

* removing deps of missing packages on rolling

* PCL empty cloud  conversion compiler errors fix

* fixing icp_odometry test failure on ci witohut libpointmatcher

* fixing nav2 costmap plugin build on lyrical

* joining thread when exiting

* updating icp test to work the same on pcl 1.15 (lyrical)

* Fix parallel tests seg fault

---------

Co-authored-by: mathieu86 <[email protected]>

rtabmap_util/doc/db_player.md

@@ -8,6 +8,16 @@ That makes it the tool for offline work: re-run SLAM with different parameters o
 
 > **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
@@ -67,7 +77,7 @@ Broadcast on every frame unless `publish_tf` is false.
 | 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` | Resume it. |
+| `~/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.
 

rtabmap_util/doc/disparity_to_depth.md

@@ -6,6 +6,14 @@ Most of ROS handles depth, while a stereo pipeline produces disparity. This node
 
 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

rtabmap_util/doc/imu_to_tf.md

@@ -2,10 +2,24 @@
 
 Broadcasts the orientation of an IMU as a TF transform.
 
-The node subscribes to a `sensor_msgs/msg/Imu` 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.
+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:

rtabmap_util/doc/lidar_deskewing.md

@@ -6,6 +6,17 @@ A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving
 
 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

rtabmap_util/doc/map_assembler.md

@@ -8,6 +8,17 @@ It also lets you produce maps RTAB-Map is not currently configured to publish, o
 
 The assembling itself is done by `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)
+- [Octomap tree type](#octomap-tree-type)
+- [Start-up](#start-up)
+- [Notes](#notes)
+
 ## Usage
 
 ```bash

rtabmap_util/doc/obstacles_detection.md

@@ -6,6 +6,17 @@ The node takes a cloud, works out which points belong to the floor and which sti
 
 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

rtabmap_util/doc/point_cloud_aggregator.md

@@ -8,6 +8,17 @@ The sensors do not have to fire together: the clouds are matched by nearest stam
 
 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

rtabmap_util/doc/point_cloud_assembler.md

@@ -8,6 +8,20 @@ That is used for two quite different things. With a **narrow field of view** —
 
 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:

rtabmap_util/doc/point_cloud_xyz.md

@@ -8,6 +8,15 @@ The node takes a depth image and its calibration and produces a [`sensor_msgs/ms
 
 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

rtabmap_util/doc/point_cloud_xyzrgb.md

@@ -4,6 +4,15 @@ Projects an RGB-D frame, a stereo pair or a disparity image into a colored point
 
 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

rtabmap_util/doc/pointcloud_to_depthimage.md

@@ -9,6 +9,17 @@ 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

rtabmap_util/doc/rgbd_relay.md

@@ -6,6 +6,15 @@ An `RGBDImage` can carry its images raw or compressed. This node converts betwee
 
 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:

rtabmap_util/doc/rgbd_split.md

@@ -6,6 +6,15 @@ Splits an [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap
 
 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