i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
Matthew Kotara
2026-09-22 17:33:01 +01:00
parent adef11e0f9
commit 254ca3b667
7 changed files with 233 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/coverage.yml
+2 -2
View File
@@ -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
+4 -4
View File
@@ -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
+20
View File
@@ -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
+3
View File
@@ -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
-37
View File
@@ -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
-161
View File
@@ -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
+204
View File
@@ -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