diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index e36b40aa..efa27be9 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1801,6 +1801,7 @@ void CoreWrapper::commonLaserScanCallback( rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) { RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); + syncDataMutex_.unlock(); return; } } @@ -1819,6 +1820,7 @@ void CoreWrapper::commonLaserScanCallback( scanCloudIs2d_)) { RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); + syncDataMutex_.unlock(); return; } } diff --git a/rtabmap_slam/test/README.md b/rtabmap_slam/test/README.md new file mode 100644 index 00000000..99d8f43c --- /dev/null +++ b/rtabmap_slam/test/README.md @@ -0,0 +1,37 @@ +# 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 new file mode 100644 index 00000000..045e5e63 --- /dev/null +++ b/rtabmap_slam/test/scan_tf_recovery.py @@ -0,0 +1,161 @@ +"""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))