diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml
index 03d5ee80..edf80327 100644
--- a/.github/workflows/coverage.yml
+++ b/.github/workflows/coverage.yml
@@ -60,7 +60,7 @@ jobs:
# 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_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.
diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml
index 6e0cf1f5..9064fe1d 100644
--- a/.github/workflows/ros2.yml
+++ b/.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
diff --git a/rtabmap_slam/CMakeLists.txt b/rtabmap_slam/CMakeLists.txt
index 6107b23a..6af5b9fc 100644
--- a/rtabmap_slam/CMakeLists.txt
+++ b/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()
diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml
index 6d2c94e2..d6bc4ed0 100644
--- a/rtabmap_slam/package.xml
+++ b/rtabmap_slam/package.xml
@@ -35,6 +35,9 @@
rtabmap_util
rtabmap_sync
+ ament_cmake_gtest
+ rcl_interfaces
+
ament_cmake
diff --git a/rtabmap_slam/test/README.md b/rtabmap_slam/test/README.md
deleted file mode 100644
index 99d8f43c..00000000
--- a/rtabmap_slam/test/README.md
+++ /dev/null
@@ -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.
diff --git a/rtabmap_slam/test/scan_tf_recovery.py b/rtabmap_slam/test/scan_tf_recovery.py
deleted file mode 100644
index 045e5e63..00000000
--- a/rtabmap_slam/test/scan_tf_recovery.py
+++ /dev/null
@@ -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))
diff --git a/rtabmap_slam/test/test_scan_tf_recovery.cpp b/rtabmap_slam/test/test_scan_tf_recovery.cpp
new file mode 100644
index 00000000..ab5e6796
--- /dev/null
+++ b/rtabmap_slam/test/test_scan_tf_recovery.cpp
@@ -0,0 +1,204 @@
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace {
+
+class ScanTfRecovery : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ rclcpp::init(0, nullptr);
+ executor_ = std::make_unique(
+ rclcpp::ExecutorOptions(), 2);
+ helper_ = std::make_shared("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 & done, const std::function & 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(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(
+ "/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(
+ "/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("/odom", 10);
+ auto scanPub = helper_->create_publisher("/scan", rclcpp::SensorDataQoS());
+ auto cloudPub = helper_->create_publisher("/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 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 executor_;
+ std::thread spinThread_;
+ std::atomic conversionFailed_{false};
+ std::atomic hasMap_{false};
+ rclcpp::Node::SharedPtr helper_;
+ std::shared_ptr slam_;
+};
+
+TEST_F(ScanTfRecovery, Recovers2D) { expectRecovery(false); }
+TEST_F(ScanTfRecovery, Recovers3D) { expectRecovery(true); }
+
+} // namespace