test(rtabmap_slam): cover scan transform recovery in native tests

Replace the standalone reproducer with registered C++ GoogleTests for 2D and 3D scan recovery. Use real ROS inputs, separate DDS domains and bounded timeouts, and include rtabmap_slam in supported CI and coverage jobs.

Validated on Jazzy with RTAB-Map core 9ed83a7: both negative controls fail map recovery without the unlocks; both pass with the fix, followed by five repeated runs per case (10/10 passed). Broader distro CI has not been run locally.

.github/workflows/coverage.yml

@@ -60,7 +60,7 @@ jobs:
       # tested package depends on them (--packages-up-to), just not measured.
       - uses: ros-tooling/[email protected]
         with:
-          package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_odom rtabmap_python
+          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.
@@ -120,7 +120,7 @@ jobs:
           . /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"
+          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.

.github/workflows/ros2.yml

@@ -25,16 +25,16 @@ jobs:
         include:
         - ros_distro: humble
           skip_keys: ''
-          packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
+          packages: 'rtabmap_slam rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
         - ros_distro: jazzy
           skip_keys: ''
-          packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
+          packages: 'rtabmap_slam rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
         - ros_distro: kilted
           skip_keys: 'grid_map_ros'
-          packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
+          packages: 'rtabmap_slam rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
         - ros_distro: lyrical
           skip_keys: 'velodyne grid_map_ros libpointmatcher'
-          packages: 'rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
+          packages: 'rtabmap_slam rtabmap_ros rtabmap_conversions rtabmap_odom rtabmap_sync rtabmap_util rtabmap_python'
         - ros_distro: rolling
           # Rolling has moved to Ubuntu resolute and much of it has not been rebuilt for
           # that yet, so rosdep finds nothing to install for these. They are all optional

rtabmap_slam/CMakeLists.txt

@@ -240,4 +240,24 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(rcl_interfaces REQUIRED)
+  ament_add_gtest_executable(test_scan_tf_recovery test/test_scan_tf_recovery.cpp)
+  if(TARGET test_scan_tf_recovery)
+    target_link_libraries(test_scan_tf_recovery rtabmap_slam_plugins)
+    if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+      ament_target_dependencies(test_scan_tf_recovery ${AmentLibraries} rcl_interfaces)
+    else()
+      target_link_libraries(test_scan_tf_recovery ${Libraries} rcl_interfaces::rcl_interfaces)
+    endif()
+    # Separate processes/domains keep either deadlock from hiding the other case.
+    # Existing util, sync and odom tests use domains starting at 30, 50 and 70.
+    ament_add_gtest_test(test_scan_tf_recovery TEST_NAME test_scan_tf_recovery_2d
+      ENV ROS_DOMAIN_ID=90 GTEST_FILTER=ScanTfRecovery.Recovers2D TIMEOUT 45)
+    ament_add_gtest_test(test_scan_tf_recovery TEST_NAME test_scan_tf_recovery_3d
+      ENV ROS_DOMAIN_ID=91 GTEST_FILTER=ScanTfRecovery.Recovers3D TIMEOUT 45)
+  endif()
+endif()
+
 ament_package()

rtabmap_slam/package.xml

@@ -35,6 +35,9 @@
   <depend>rtabmap_util</depend>
   <depend>rtabmap_sync</depend>
   
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>rcl_interfaces</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
   </export>

rtabmap_slam/test/README.md

@@ -1,37 +0,0 @@
-# Scan transform recovery regression
-
-`scan_tf_recovery.py` is a standalone ROS 2 integration reproducer for the
-scan-conversion error paths in `CoreWrapper::commonLaserScanCallback`.
-It is not registered with CTest yet.
-
-Source your ROS installation and the workspace containing the RTAB-Map build
-under test. Use an unused ROS domain and run the two cases sequentially:
-
-```bash
-source /opt/ros/jazzy/setup.bash
-source install/setup.bash
-export ROS_DOMAIN_ID=171
-python3 src/rtabmap_ros/rtabmap_slam/test/scan_tf_recovery.py
-python3 src/rtabmap_ros/rtabmap_slam/test/scan_tf_recovery.py --late-tf
-```
-
-Adjust the script path for your checkout layout. Python dependencies are
-`rclpy`, `ament_index_python`, `geometry_msgs`, `nav_msgs`, `sensor_msgs`,
-`sensor_msgs_py`, `std_msgs`, `tf2_ros` and PyYAML.
-
-The script starts the installed `rtabmap_slam/rtabmap` executable, publishes
-odometry and `odom -> base_link`, and begins publishing PointCloud2 data after
-four seconds. The control provides `base_link -> lidar` immediately; `--late-tf`
-withholds it until eight seconds. No simulator, hardware or recorded data is
-required. Parameters and captured logs are written under `/tmp`.
-
-Both cases require a nonempty occupancy grid within 18 seconds and a clean
-process exit after SIGINT. The late-TF case also requires a scan-conversion error
-in the log, ensuring the failure path was exercised. A JSON summary reports the
-map dimensions, conversion failure and process exit code.
-
-On stock Jazzy 0.23.7, the control passes but the late-TF case produces no map and
-requires forced shutdown. With the two missing unlocks added, both pass. The
-patched late-TF case passed five runs, including two using default FastDDS and
-two using UDP-only transport. This reproducer exercises the 3D scan path; the
-analogous 2D error path is fixed by inspection but is not independently tested.

rtabmap_slam/test/scan_tf_recovery.py

@@ -1,161 +0,0 @@
-"""Probe first-map recovery when the scan's static transform arrives late."""
-
-import argparse
-import json
-import os
-import signal
-import subprocess
-import time
-from pathlib import Path
-
-import rclpy
-import yaml
-from ament_index_python.packages import get_package_prefix
-from geometry_msgs.msg import TransformStamped
-from nav_msgs.msg import OccupancyGrid, Odometry
-from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
-from sensor_msgs.msg import PointCloud2
-from sensor_msgs_py import point_cloud2
-from std_msgs.msg import Header
-from tf2_ros import StaticTransformBroadcaster, TransformBroadcaster
-
-
-def run(late_tf: bool) -> int:
-    rclpy.init()
-    node = rclpy.create_node("startup_input_probe")
-    best = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
-    cloud_pub = node.create_publisher(PointCloud2, "/scan_cloud", best)
-    odom_pub = node.create_publisher(Odometry, "/odom", 10)
-    static = StaticTransformBroadcaster(node)
-    dynamic = TransformBroadcaster(node)
-    received = []
-
-    def on_map(msg: OccupancyGrid) -> None:
-        if msg.info.width and msg.info.height:
-            received.append([msg.info.width, msg.info.height])
-
-    node.create_subscription(
-        OccupancyGrid,
-        "/map",
-        on_map,
-        QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL),
-    )
-    parameters = {
-        "frame_id": "base_link",
-        "odom_frame_id": "odom",
-        "map_frame_id": "map",
-        "subscribe_depth": "false",
-        "subscribe_rgb": "false",
-        "subscribe_scan_cloud": "true",
-        "subscribe_odom": "true",
-        "approx_sync": "true",
-        "qos_scan": "2",
-        "qos_odom": "1",
-        "wait_for_transform": "0.2",
-        "database_path": "",
-        "Reg/Strategy": "1",
-        "Grid/3D": "true",
-        "Grid/CellSize": "0.1",
-        "Grid/NormalsSegmentation": "false",
-        "Grid/MinGroundHeight": "-0.3",
-        "Grid/MaxGroundHeight": "0.1",
-        "Grid/RangeMax": "20.0",
-        "Grid/RangeMin": "0.1",
-    }
-    args = [
-        str(Path(get_package_prefix("rtabmap_slam")) / "lib/rtabmap_slam/rtabmap"),
-        "--ros-args",
-        "--log-level",
-        "warn",
-    ]
-    parameters = {
-        key: (value if "/" in key or key == "database_path" else yaml.safe_load(value))
-        for key, value in parameters.items()
-    }
-    Path("/tmp/rtab-startup-params.yaml").write_text(
-        yaml.safe_dump({"/**": {"ros__parameters": parameters}})
-    )
-    args += ["--params-file", "/tmp/rtab-startup-params.yaml"]
-    log_path = Path("/tmp/rtab-startup-probe.log")
-    with log_path.open("w") as log:
-        process = subprocess.Popen(
-            args, stdout=log, stderr=subprocess.STDOUT, start_new_session=True
-        )
-        start = time.monotonic()
-        static_sent = False
-        cloud_sent = False
-        try:
-            while time.monotonic() - start < 18 and not received:
-                elapsed = time.monotonic() - start
-                stamp = node.get_clock().now().to_msg()
-                transform = TransformStamped()
-                transform.header.stamp = stamp
-                transform.header.frame_id = "odom"
-                transform.child_frame_id = "base_link"
-                transform.transform.rotation.w = 1.0
-                dynamic.sendTransform(transform)
-                odom = Odometry()
-                odom.header.stamp = stamp
-                odom.header.frame_id = "odom"
-                odom.child_frame_id = "base_link"
-                odom.pose.pose.orientation.w = 1.0
-                for i in [0, 7, 14, 21, 28, 35]:
-                    odom.pose.covariance[i] = 0.001
-                odom_pub.publish(odom)
-                if not static_sent and (not late_tf or elapsed > 8):
-                    transform.header.frame_id = "base_link"
-                    transform.child_frame_id = "lidar"
-                    transform.transform.translation.z = 0.2
-                    static.sendTransform(transform)
-                    static_sent = True
-                if elapsed > 4 and cloud_pub.get_subscription_count() > 0:
-                    header = Header(stamp=stamp, frame_id="lidar")
-                    points = [
-                        (x / 10.0, y / 10.0, -0.2)
-                        for x in range(5, 41, 2)
-                        for y in range(-20, 21, 2)
-                    ]
-                    points += [
-                        (4.0, y / 10.0, z / 10.0)
-                        for y in range(-20, 21, 2)
-                        for z in range(0, 21, 2)
-                    ]
-                    cloud_pub.publish(point_cloud2.create_cloud_xyz32(header, points))
-                    cloud_sent = True
-                rclpy.spin_once(node, timeout_sec=0.05)
-                if process.poll() is not None:
-                    break
-        finally:
-            if process.poll() is None:
-                os.killpg(process.pid, signal.SIGINT)
-            try:
-                process.wait(timeout=5)
-            except subprocess.TimeoutExpired:
-                os.killpg(process.pid, signal.SIGKILL)
-                process.wait()
-            node.destroy_node()
-            rclpy.shutdown()
-    content = log_path.read_text()
-    result = {
-        "lateTf": late_tf,
-        "cloudSent": cloud_sent,
-        "map": received[:1],
-        "conversionFailure": "Could not convert 3d laser scan msg" in content,
-        "processExit": process.returncode,
-    }
-    print(json.dumps(result), flush=True)
-    passed = (
-        bool(received)
-        and process.returncode == 0
-        and (not late_tf or result["conversionFailure"])
-    )
-    if not passed:
-        print(content[-5000:], flush=True)
-    return 0 if passed else 1
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--late-tf", action="store_true")
-    args = parser.parse_args()
-    raise SystemExit(run(args.late_tf))

rtabmap_slam/test/test_scan_tf_recovery.cpp

@@ -0,0 +1,204 @@
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <rcl_interfaces/msg/log.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <tf2_ros/static_transform_broadcaster.hpp>
+#include <tf2_ros/transform_broadcaster.hpp>
+#include <rtabmap_slam/CoreWrapper.h>
+
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <string>
+#include <thread>
+
+namespace {
+
+class ScanTfRecovery : public ::testing::Test
+{
+protected:
+	void SetUp() override
+	{
+		rclcpp::init(0, nullptr);
+		executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>(
+				rclcpp::ExecutorOptions(), 2);
+		helper_ = std::make_shared<rclcpp::Node>("scan_tf_recovery_input");
+		executor_->add_node(helper_);
+	}
+
+	void TearDown() override
+	{
+		executor_->cancel();
+		if(spinThread_.joinable())
+		{
+			spinThread_.join();
+		}
+		if(slam_)
+		{
+			executor_->remove_node(slam_);
+			slam_.reset();
+		}
+		executor_->remove_node(helper_);
+		helper_.reset();
+		executor_.reset();
+		rclcpp::shutdown();
+	}
+
+	bool spinUntil(const std::function<bool()> & done, const std::function<void()> & publish)
+	{
+		const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10);
+		auto nextInput = std::chrono::steady_clock::now();
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline && !done())
+		{
+			if(std::chrono::steady_clock::now() >= nextInput)
+			{
+				publish();
+				nextInput = std::chrono::steady_clock::now() + std::chrono::milliseconds(50);
+			}
+			std::this_thread::sleep_for(std::chrono::milliseconds(10));
+		}
+		return done();
+	}
+
+	void expectRecovery(bool cloud)
+	{
+		rclcpp::NodeOptions options;
+		options.parameter_overrides({
+			rclcpp::Parameter("frame_id", "base_link"),
+			rclcpp::Parameter("odom_frame_id", "odom"),
+			rclcpp::Parameter("subscribe_rgb", false),
+			rclcpp::Parameter("subscribe_depth", false),
+			rclcpp::Parameter("subscribe_scan", !cloud),
+			rclcpp::Parameter("subscribe_scan_cloud", cloud),
+			rclcpp::Parameter("qos_scan", 2),
+			rclcpp::Parameter("wait_for_transform", 0.05),
+			rclcpp::Parameter("database_path", ""),
+			rclcpp::Parameter("Reg/Strategy", "1"),
+			rclcpp::Parameter("Grid/3D", cloud ? "true" : "false"),
+			rclcpp::Parameter("Grid/CellSize", "0.1"),
+			rclcpp::Parameter("Grid/NormalsSegmentation", "false"),
+			rclcpp::Parameter("Grid/MinGroundHeight", "-0.3"),
+			rclcpp::Parameter("Grid/MaxGroundHeight", "0.1"),
+			rclcpp::Parameter("Grid/RangeMin", "0.1"),
+			rclcpp::Parameter("Grid/RangeMax", "20.0")
+		});
+		slam_ = std::make_shared<rtabmap_slam::CoreWrapper>(options);
+		executor_->add_node(slam_);
+
+		const std::string error = cloud ? "Could not convert 3d laser scan msg" : "Could not convert laser scan msg";
+		auto logs = helper_->create_subscription<rcl_interfaces::msg::Log>(
+				"/rosout", rclcpp::QoS(100).reliable().transient_local(),
+				[this, error](rcl_interfaces::msg::Log::ConstSharedPtr msg) {
+					if(msg->name == "rtabmap" && msg->msg.find(error) != std::string::npos)
+					{
+						conversionFailed_ = true;
+					}
+				});
+		auto maps = helper_->create_subscription<nav_msgs::msg::OccupancyGrid>(
+				"/map", rclcpp::QoS(1).transient_local(),
+				[this](nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) {
+					hasMap_ = msg->info.width > 0 && msg->info.height > 0 && !msg->data.empty();
+				});
+		auto odomPub = helper_->create_publisher<nav_msgs::msg::Odometry>("/odom", 10);
+		auto scanPub = helper_->create_publisher<sensor_msgs::msg::LaserScan>("/scan", rclcpp::SensorDataQoS());
+		auto cloudPub = helper_->create_publisher<sensor_msgs::msg::PointCloud2>("/scan_cloud", rclcpp::SensorDataQoS());
+		tf2_ros::TransformBroadcaster dynamicTf(helper_);
+		tf2_ros::StaticTransformBroadcaster staticTf(helper_);
+
+		sensor_msgs::msg::LaserScan scan;
+		scan.header.frame_id = "lidar";
+		scan.angle_min = -1.0f;
+		scan.angle_max = 1.0f;
+		scan.angle_increment = 0.01f;
+		scan.range_min = 0.1f;
+		scan.range_max = 20.0f;
+		for(int i=0; i<=200; ++i)
+		{
+			scan.ranges.push_back(4.0f / std::cos(scan.angle_min + i * scan.angle_increment));
+		}
+		sensor_msgs::msg::PointCloud2 points;
+		points.header.frame_id = "lidar";
+		sensor_msgs::PointCloud2Modifier modifier(points);
+		modifier.setPointCloud2FieldsByString(1, "xyz");
+		modifier.resize(18 * 21 + 21 * 11);
+		sensor_msgs::PointCloud2Iterator<float> x(points, "x"), y(points, "y"), z(points, "z");
+		for(int ix=5; ix<=40; ix+=2)
+		{
+			for(int iy=-20; iy<=20; iy+=2, ++x, ++y, ++z)
+			{
+				*x = ix / 10.0f; *y = iy / 10.0f; *z = -0.2f;
+			}
+		}
+		for(int iy=-20; iy<=20; iy+=2)
+		{
+			for(int iz=0; iz<=20; iz+=2, ++x, ++y, ++z)
+			{
+				*x = 4.0f; *y = iy / 10.0f; *z = iz / 10.0f;
+			}
+		}
+
+		auto publish = [&]() {
+			const auto stamp = helper_->now();
+			geometry_msgs::msg::TransformStamped tf;
+			tf.header.stamp = stamp;
+			tf.header.frame_id = "odom";
+			tf.child_frame_id = "base_link";
+			tf.transform.rotation.w = 1.0;
+			dynamicTf.sendTransform(tf);
+			nav_msgs::msg::Odometry odom;
+			odom.header = tf.header;
+			odom.child_frame_id = "base_link";
+			odom.pose.pose.orientation.w = 1.0;
+			for(int i : {0, 7, 14, 21, 28, 35})
+			{
+				odom.pose.covariance[i] = 0.001;
+			}
+			odomPub->publish(odom);
+			if(cloud)
+			{
+				points.header.stamp = stamp;
+				cloudPub->publish(points);
+			}
+			else
+			{
+				scan.header.stamp = stamp;
+				scanPub->publish(scan);
+			}
+		};
+
+		// Match the production node: processing and sensor callbacks run on different
+		// workers. A single thread can reacquire the leaked recursive mutex.
+		spinThread_ = std::thread([this]() { executor_->spin(); });
+		// Observe the actual conversion failure before making sensor TF available.
+		ASSERT_TRUE(spinUntil([&]() { return conversionFailed_.load(); }, publish));
+		EXPECT_FALSE(hasMap_.load());
+		geometry_msgs::msg::TransformStamped sensorTf;
+		sensorTf.header.stamp = helper_->now();
+		sensorTf.header.frame_id = "base_link";
+		sensorTf.child_frame_id = "lidar";
+		sensorTf.transform.translation.z = 0.2;
+		sensorTf.transform.rotation.w = 1.0;
+		staticTf.sendTransform(sensorTf);
+		EXPECT_TRUE(spinUntil([&]() { return hasMap_.load(); }, publish))
+				<< "Mapping must recover after a transient scan transform failure";
+	}
+
+	std::unique_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+	std::thread spinThread_;
+	std::atomic<bool> conversionFailed_{false};
+	std::atomic<bool> hasMap_{false};
+	rclcpp::Node::SharedPtr helper_;
+	std::shared_ptr<rtabmap_slam::CoreWrapper> slam_;
+};
+
+TEST_F(ScanTfRecovery, Recovers2D) { expectRecovery(false); }
+TEST_F(ScanTfRecovery, Recovers3D) { expectRecovery(true); }
+
+} // namespace