DO NOT MERGE — Diagnose Plan Path Along Surface CI hang (moveit_pro#20427)

.github/workflows/ci.yaml

@@ -246,7 +246,10 @@ jobs:
       matrix:
         # Stream 2 expands this list one PR at a time as each sim gains a
         # pytest entry.
-        config_package: [lab_sim, hangar_sim]
+        # DIAGNOSTIC BRANCH ONLY (moveit_pro#20427): restricted to hangar_sim
+        # so repeated dispatch runs don't burn lab_sim runners. Restore
+        # [lab_sim, hangar_sim] before any merge.
+        config_package: [hangar_sim]
     permissions:
       # contents: read for checkout; id-token: write so the reusable workflow
       # can assume the LFS S3 cache IAM role via OIDC (moveit_pro_ci v0.5.x).

src/hangar_sim/config/control/picknik_ur.ros2_control.yaml

@@ -249,7 +249,15 @@ joint_trajectory_controller:
         angle_wraparound: true
     constraints:
       stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
+      # A zero goal_time makes the upstream JTC goal-tolerance abort branch
+      # unreachable: a goal whose joints sit outside their goal tolerance
+      # after the last trajectory point neither succeeds nor aborts, and the
+      # per-joint tolerance error is only printed on the abort path. That
+      # silent infinite wait is the `Plan Path Along Surface` CI hang of
+      # moveit_pro#20427. 20 s is generous for post-trajectory convergence
+      # and converts the hang into a diagnosable GOAL_TOLERANCE_VIOLATED
+      # abort that names the offending joint.
+      goal_time: 20.0
       shoulder_pan_joint:
         goal: 0.05
       shoulder_lift_joint:

src/hangar_sim/test/conftest.py

@@ -49,16 +49,106 @@
 """
 
 import os
+import re
+import shutil
 import signal
 import subprocess
 import sys
 import time
+from datetime import datetime
 from pathlib import Path
 
 import pytest
 
 _started_at: dict[str, float] = {}
 
+# Topics recorded per test for post-mortem playback (moveit_pro#20427).
+# Kinematic state + controller tracking only -- no images or point clouds, so
+# a 90 s test stays in the tens of MB. Topics that do not exist on a given
+# distro (e.g. the humble-only `state` topic) are simply never captured;
+# rosbag2 subscribes on discovery and an absent topic is not an error.
+_BAG_TOPICS = [
+    "/clock",
+    "/joint_states",
+    "/tf",
+    "/tf_static",
+    "/joint_trajectory_controller/controller_state",
+    "/joint_trajectory_controller/state",
+    "/joint_trajectory_controller/follow_joint_trajectory/_action/status",
+    "/joint_trajectory_controller/follow_joint_trajectory/_action/feedback",
+    "/platform_velocity_controller/odometry",
+    "/platform_velocity_controller/odom",
+]
+
+# Bags land under build/<pkg>/test_results/ (pytest's cwd is the package
+# build dir), which the CI reusable workflow already uploads as the
+# `test-results-*` artifact with `if: always()` -- no workflow change needed.
+_BAG_ROOT = Path.cwd() / "test_results" / "bags"
+
+# Objectives under investigation in moveit_pro#20427: keep their bags even on
+# PASS so a healthy execution is available to diff against a hung one.
+_ALWAYS_KEEP = re.compile(r"plan_path_along_surface")
+
+
+@pytest.hookimpl(tryfirst=True, hookwrapper=True)
+def pytest_runtest_makereport(item, call):
+    """Stash each phase's report on the item so fixtures can read the outcome.
+
+    The ``record_rosbag`` teardown uses ``item.rep_call`` to decide whether a
+    test failed and its bag must be kept.
+    """
+    outcome = yield
+    rep = outcome.get_result()
+    setattr(item, "rep_" + rep.when, rep)
+
+
+@pytest.fixture(autouse=True)
+def record_rosbag(request):
+    """Record a per-test rosbag of joint/TF/controller state for playback.
+
+    Diagnostic instrumentation for the `Plan Path Along Surface` execute-hang
+    (moveit_pro#20427). ``ros2 bag record`` runs as a subprocess for the same
+    fork/DDS reason as ``capture_rosout`` -- no rclpy state may live in the
+    pytest process (see module docstring).
+
+    Keep policy on teardown: bags of failing tests and of the objectives under
+    investigation are kept (they ride the existing test-results CI artifact);
+    everything else is deleted so the artifact stays small. Playback locally:
+    ``ros2 bag play <dir> --clock`` + RViz with ``use_sim_time``.
+    """
+    safe_name = re.sub(r"[^A-Za-z0-9_.-]+", "_", request.node.name)[-80:]
+    bag_dir = _BAG_ROOT / f"{safe_name}-{datetime.now().strftime('%H%M%S')}"
+    _BAG_ROOT.mkdir(parents=True, exist_ok=True)
+
+    proc = subprocess.Popen(
+        ["ros2", "bag", "record", "--include-hidden-topics", "-o", str(bag_dir)]
+        + _BAG_TOPICS,
+        stdout=subprocess.DEVNULL,
+        stderr=subprocess.DEVNULL,
+    )
+    # Give the recorder a moment to create subscriptions so the first motion
+    # of the objective is not missed.
+    time.sleep(2.0)
+
+    yield
+
+    # SIGINT lets rosbag2 flush and finalize the bag; SIGKILL fallback in
+    # case the recorder wedges.
+    proc.send_signal(signal.SIGINT)
+    try:
+        proc.wait(timeout=15)
+    except subprocess.TimeoutExpired:
+        proc.kill()
+        proc.wait(timeout=5)
+
+    rep_call = getattr(request.node, "rep_call", None)
+    rep_setup = getattr(request.node, "rep_setup", None)
+    failed = (rep_call is not None and rep_call.failed) or (
+        rep_setup is not None and rep_setup.failed
+    )
+    if not failed and not _ALWAYS_KEEP.search(request.node.name):
+        shutil.rmtree(bag_dir, ignore_errors=True)
+
 
 def pytest_runtest_logstart(nodeid, location):
     _started_at[nodeid] = time.monotonic()