fix(rtabmap_slam): release scan mutex on conversion failure

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;
 			}
 		}

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.

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))