From d1d0656ade50d24e91680d4e112203c8535ce6b6 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Mon, 13 Jul 2026 16:00:59 -0400 Subject: [PATCH] test(hangar_sim): instrument Plan Path Along Surface hang for diagnosis --- .github/workflows/ci.yaml | 5 +- .../control/picknik_ur.ros2_control.yaml | 10 ++- src/hangar_sim/test/conftest.py | 90 +++++++++++++++++++ 3 files changed, 103 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3d7a2c22b..66e54684f 100644 --- a/.github/workflows/ci.yaml +++ b/.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). diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index abdd64463..5bcfe0d30 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/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: diff --git a/src/hangar_sim/test/conftest.py b/src/hangar_sim/test/conftest.py index f1dcdfee0..32c9655b9 100644 --- a/src/hangar_sim/test/conftest.py +++ b/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//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 --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()