rtabmap_sync tests and doc (#1454)

* rtabmap_sync tests and doc

* Added some diagrams

* cleanup some diagrams

* fixing running tests in parallels

rtabmap_util/CMakeLists.txt

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

rtabmap_util/README.md

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

rtabmap_util/doc/imu_to_tf.md

@@ -91,15 +91,45 @@ Node(
     remappings=[('scan_cloud', '/lidar/points/deskewed')])
 ```
 
-The three nodes chain into a single TF tree:
+The three nodes chain into a single TF tree. In terms of data, this node's job is to turn the IMU's orientation into a *frame* that `lidar_deskewing` and `icp_odometry` can look up — while the IMU topic itself still goes straight to the SLAM nodes, which use it for their own purposes:
 
-```text
-map                                rtabmap
-└── icp_odom                       icp_odometry
-    └── base_link_stabilized       imu_to_tf
-        └── base_link
-            ├── lidar_link         robot description (static)
-            └── imu_link
+```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 |

rtabmap_util/doc/map_assembler.md

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

rtabmap_util/doc/obstacles_detection.md

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

rtabmap_util/doc/point_cloud_aggregator.md

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

rtabmap_util/doc/point_cloud_assembler.md

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

rtabmap_util/doc/pointcloud_to_depthimage.md

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

rtabmap_util/doc/rgbd_relay.md

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

rtabmap_util/doc/rgbd_split.md

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