From 0a2e9e50c0ef97a88c5bc150cd948943cdb12852 Mon Sep 17 00:00:00 2001 From: Hamnah Date: Sat, 12 Sep 2026 13:53:55 -0600 Subject: [PATCH] Add diagnostics session recorder --- .gitignore | 4 + waybionic_rviz_plugins/CMakeLists.txt | 16 +- waybionic_rviz_plugins/README.md | 44 ++++ waybionic_rviz_plugins/package.xml | 3 + .../scripts/diagnostics_recorder.py | 211 ++++++++++++++++++ .../test/test_diagnostics_recorder.py | 131 +++++++++++ .../test_diagnostics_recorder_roundtrip.py | 108 +++++++++ 7 files changed, 516 insertions(+), 1 deletion(-) create mode 100755 waybionic_rviz_plugins/scripts/diagnostics_recorder.py create mode 100644 waybionic_rviz_plugins/test/test_diagnostics_recorder.py create mode 100644 waybionic_rviz_plugins/test/test_diagnostics_recorder_roundtrip.py diff --git a/.gitignore b/.gitignore index 37fce43..8b0ab3b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ +diagnostics-sessions/ +*.db3 +*.mcap + # ROS folders build/ install/ diff --git a/waybionic_rviz_plugins/CMakeLists.txt b/waybionic_rviz_plugins/CMakeLists.txt index ade07c3..413e3f0 100644 --- a/waybionic_rviz_plugins/CMakeLists.txt +++ b/waybionic_rviz_plugins/CMakeLists.txt @@ -67,7 +67,9 @@ install( ) install( - PROGRAMS scripts/temporary_diagnostics_publisher.py + PROGRAMS + scripts/temporary_diagnostics_publisher.py + scripts/diagnostics_recorder.py DESTINATION lib/${PROJECT_NAME} ) @@ -86,6 +88,18 @@ if(BUILD_TESTING) TIMEOUT 60 ) + ament_add_pytest_test( + test_diagnostics_recorder + test/test_diagnostics_recorder.py + TIMEOUT 60 + ) + + ament_add_pytest_test( + test_diagnostics_recorder_roundtrip + test/test_diagnostics_recorder_roundtrip.py + TIMEOUT 120 + ) + # Built from sources directly so the stress test links neither Qt nor RViz and # can run headless. ament_add_gtest(test_ros_diagnostics_source diff --git a/waybionic_rviz_plugins/README.md b/waybionic_rviz_plugins/README.md index 18eb62c..8904fa5 100644 --- a/waybionic_rviz_plugins/README.md +++ b/waybionic_rviz_plugins/README.md @@ -38,6 +38,7 @@ waybionic_rviz_plugins/ plugin_description.xml # Registers DiagnosticsPanel scripts/ temporary_diagnostics_publisher.py + diagnostics_recorder.py include/waybionic_rviz_plugins/ diagnostics_contract.hpp # Normalized DiagnosticMessage model diagnostics_source.hpp # DiagnosticsSource interface @@ -139,6 +140,49 @@ DiagnosticsSource `RosDiagnosticsSource` maps ROS diagnostic levels and fields into the internal `DiagnosticMessage` model before the Qt panel renders them. See `docs/DIAGNOSTICS_CONTRACT.md` for the full mapping Korede/backend should follow, and `docs/DIAGNOSTICS_BACKEND_INTEGRATION.md` for backend replacement guidance. +### Recording a diagnostics session + +The recorder saves the original ROS messages in a standard rosbag2 session and +writes `metadata.json` beside the bag. It records `/diagnostics` by default; +additional topics must be named explicitly. + +```bash +ros2 run waybionic_rviz_plugins diagnostics_recorder.py \ + --duration 30 \ + --output-directory ~/diagnostics-sessions/fault-001 \ + --source-label mock \ + --tested-commit "$(git rev-parse HEAD)" +``` + +The output directory must not already exist. Inspect or replay a session in an +isolated ROS domain so it cannot interfere with an active robot or publisher: + +```bash +ROS_DOMAIN_ID=42 ros2 bag info ~/diagnostics-sessions/fault-001/bag +ROS_DOMAIN_ID=42 ros2 bag play ~/diagnostics-sessions/fault-001/bag +ROS_DOMAIN_ID=42 ros2 topic echo /diagnostics +``` + +Label replayed data as `recorded/mock` when sharing it. Replay does not require +the original diagnostics publisher to be running. Generated bag directories +should remain outside Git; the repository ignores local recording output. + +For a complete temporary-publisher validation, record each mode in a separate +new directory. Use a duration long enough for the first rosbag2 startup on the +machine: + +```bash +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=normal +ros2 run waybionic_rviz_plugins diagnostics_recorder.py --duration 30 \ + --output-directory ~/diagnostics-sessions/normal \ + --source-label mock +``` + +Repeat with `mode:=fault`, `mode:=stale`, and `mode:=cycle`. To preserve a +publisher message gap, stop the publisher with `Ctrl+C`, leave the recorder +running, restart the publisher, and then let the recorder finish. The recorder +does not insert samples during that gap. + Switching between mock and live replaces the active source while a ROS callback may still be running. `docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md` documents the ownership rules that keep that handoff safe and the stress test that guards it. ## Platform Notes diff --git a/waybionic_rviz_plugins/package.xml b/waybionic_rviz_plugins/package.xml index 1b076cd..5b3285c 100644 --- a/waybionic_rviz_plugins/package.xml +++ b/waybionic_rviz_plugins/package.xml @@ -18,6 +18,9 @@ launch launch_ros rclpy + rosbag2_py + rosbag2_storage + rosbag2_storage_mcap rviz2 ament_cmake_gtest diff --git a/waybionic_rviz_plugins/scripts/diagnostics_recorder.py b/waybionic_rviz_plugins/scripts/diagnostics_recorder.py new file mode 100755 index 0000000..f18984b --- /dev/null +++ b/waybionic_rviz_plugins/scripts/diagnostics_recorder.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 +"""Record an explicit ROS 2 diagnostics session into a rosbag2 directory.""" + +import argparse +import json +import signal +import subprocess +import sys +from datetime import datetime, timezone +from pathlib import Path +from typing import Optional, Sequence + + +DEFAULT_TOPIC = "/diagnostics" + + +def parse_args(arguments: Optional[Sequence[str]] = None) -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Record selected ROS 2 topics and write session metadata." + ) + parser.add_argument( + "--topic", + action="append", + dest="topics", + metavar="TOPIC", + help=( + "Topic to record; repeat for additional topics. Defaults to " + f"{DEFAULT_TOPIC}. Other topics must be named explicitly." + ), + ) + parser.add_argument( + "--duration", + type=float, + help="Stop after this many seconds; omit to record until Ctrl+C.", + ) + parser.add_argument( + "--output-directory", + required=True, + type=Path, + help="New session directory to create; an existing directory is rejected.", + ) + parser.add_argument( + "--source-label", + required=True, + choices=("mock", "live"), + help="Label describing whether the session source is mock or live.", + ) + parser.add_argument( + "--tested-commit", + help="Optional commit identifier tested during this session.", + ) + parsed = parser.parse_args(arguments) + if parsed.duration is not None and parsed.duration <= 0: + parser.error("--duration must be greater than zero") + parsed.topics = parsed.topics or [DEFAULT_TOPIC] + return parsed + + +def build_record_command(bag_directory: Path, topics: Sequence[str]) -> list[str]: + if not topics: + raise ValueError("at least one topic must be selected") + return [ + "ros2", + "bag", + "record", + "--disable-keyboard-controls", + "--output", + str(bag_directory), + "--topics", + *topics, + ] + + +def utc_now() -> str: + return datetime.now(timezone.utc).isoformat() + + +def write_metadata( + session_directory: Path, + topics: Sequence[str], + source_label: str, + start_time: str, + end_time: str, + tested_commit: Optional[str], + message_count: int, +) -> None: + metadata = { + "topic": topics[0] if len(topics) == 1 else list(topics), + "start_time": start_time, + "end_time": end_time, + "source_label": source_label, + "message_count": message_count, + } + if tested_commit: + metadata["tested_commit"] = tested_commit + (session_directory / "metadata.json").write_text( + json.dumps(metadata, indent=2) + "\n", encoding="utf-8" + ) + + +def count_recorded_messages(bag_directory: Path) -> int: + if not bag_directory.exists(): + return 0 + try: + import rosbag2_py + except ImportError as exc: + raise RuntimeError( + "rosbag2_py is required to finalize and validate the recording" + ) from exc + + reader = rosbag2_py.SequentialReader() + reader.open( + rosbag2_py.StorageOptions(uri=str(bag_directory), storage_id="mcap"), + rosbag2_py.ConverterOptions("", ""), + ) + count = 0 + while reader.has_next(): + reader.read_next() + count += 1 + return count + + +def stop_recorder(process: subprocess.Popen[bytes]) -> None: + if process.poll() is not None: + return + process.send_signal(signal.SIGINT) + try: + process.wait(timeout=10) + except subprocess.TimeoutExpired: + process.terminate() + try: + process.wait(timeout=5) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + + +def run(arguments: Optional[Sequence[str]] = None) -> int: + options = parse_args(arguments) + session_directory = options.output_directory + bag_directory = session_directory / "bag" + + if session_directory.exists(): + print(f"error: output directory already exists: {session_directory}", file=sys.stderr) + return 2 + try: + session_directory.mkdir(parents=True) + except OSError as exc: + print(f"error: cannot create output directory: {exc}", file=sys.stderr) + return 2 + + start_time = utc_now() + command = build_record_command(bag_directory, options.topics) + try: + process = subprocess.Popen(command) + except FileNotFoundError: + print( + "error: missing dependency: the 'ros2' command is not available; " + "source the ROS 2 environment first", + file=sys.stderr, + ) + return 1 + except (OSError, subprocess.SubprocessError) as exc: + print(f"error: could not start ros2 bag record: {exc}", file=sys.stderr) + return 1 + + try: + if options.duration is None: + process.wait() + else: + process.wait(timeout=options.duration) + except subprocess.TimeoutExpired: + print("Recording duration reached; finalizing bag.") + except KeyboardInterrupt: + print("Stopping recording; finalizing bag.") + finally: + stop_recorder(process) + + end_time = utc_now() + if process.returncode not in (0, 130, -signal.SIGINT): + print( + f"error: ros2 bag record failed with exit code {process.returncode}", + file=sys.stderr, + ) + return 1 + + try: + message_count = count_recorded_messages(bag_directory) + write_metadata( + session_directory, + options.topics, + options.source_label, + start_time, + end_time, + options.tested_commit, + message_count, + ) + except Exception as exc: + print(f"error: could not finalize recording: {exc}", file=sys.stderr) + return 1 + + if message_count == 0: + print("error: recording completed but contained no messages", file=sys.stderr) + return 1 + + print(f"Recorded {message_count} messages in {session_directory}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(run()) \ No newline at end of file diff --git a/waybionic_rviz_plugins/test/test_diagnostics_recorder.py b/waybionic_rviz_plugins/test/test_diagnostics_recorder.py new file mode 100644 index 0000000..23afa88 --- /dev/null +++ b/waybionic_rviz_plugins/test/test_diagnostics_recorder.py @@ -0,0 +1,131 @@ +import importlib.util +import json +import signal +import subprocess +from pathlib import Path + +import pytest + + +SCRIPT = Path(__file__).resolve().parents[1] / "scripts" / "diagnostics_recorder.py" +SPEC = importlib.util.spec_from_file_location("diagnostics_recorder", SCRIPT) +recorder = importlib.util.module_from_spec(SPEC) +assert SPEC.loader is not None +SPEC.loader.exec_module(recorder) + + +def test_default_topic_and_explicit_topic_selection(): + options = recorder.parse_args( + ["--output-directory", "/tmp/session", "--source-label", "mock"] + ) + assert options.topics == ["/diagnostics"] + + options = recorder.parse_args( + [ + "--topic", + "/diagnostics", + "--topic", + "/imu", + "--output-directory", + "/tmp/session", + "--source-label", + "live", + ] + ) + assert options.topics == ["/diagnostics", "/imu"] + + +def test_invalid_duration_is_rejected(): + with pytest.raises(SystemExit): + recorder.parse_args( + [ + "--duration", + "0", + "--output-directory", + "/tmp/session", + "--source-label", + "mock", + ] + ) + + +def test_existing_output_directory_is_not_overwritten(tmp_path, capsys): + output = tmp_path / "session" + output.mkdir() + marker = output / "keep.txt" + marker.write_text("keep", encoding="utf-8") + + result = recorder.run( + ["--output-directory", str(output), "--source-label", "mock"] + ) + + assert result == 2 + assert marker.read_text(encoding="utf-8") == "keep" + assert "already exists" in capsys.readouterr().err + + +def test_recorder_failure_is_reported(tmp_path, monkeypatch, capsys): + class FailedProcess: + returncode = 7 + + def poll(self): + return self.returncode + + def wait(self, **_kwargs): + return self.returncode + + monkeypatch.setattr(recorder.subprocess, "Popen", lambda _command: FailedProcess()) + + result = recorder.run( + ["--output-directory", str(tmp_path / "session"), "--source-label", "live"] + ) + + assert result == 1 + assert "failed with exit code 7" in capsys.readouterr().err + + +def test_timed_shutdown_sends_sigint_and_writes_metadata(tmp_path, monkeypatch): + class TimedProcess: + returncode = None + stopped = False + + def poll(self): + return self.returncode + + def wait(self, **kwargs): + if "timeout" in kwargs and not self.stopped: + raise subprocess.TimeoutExpired("ros2", kwargs["timeout"]) + self.returncode = 0 + return 0 + + def send_signal(self, value): + assert value == signal.SIGINT + self.stopped = True + + def terminate(self): + raise AssertionError("terminate should not be needed") + + process = TimedProcess() + monkeypatch.setattr(recorder.subprocess, "Popen", lambda _command: process) + monkeypatch.setattr(recorder, "count_recorded_messages", lambda _bag: 3) + + output = tmp_path / "session" + result = recorder.run( + [ + "--duration", + "1", + "--output-directory", + str(output), + "--source-label", + "mock", + "--tested-commit", + "abc123", + ] + ) + + assert result == 0 + metadata = json.loads((output / "metadata.json").read_text(encoding="utf-8")) + assert metadata["topic"] == "/diagnostics" + assert metadata["source_label"] == "mock" + assert metadata["tested_commit"] == "abc123" + assert metadata["message_count"] == 3 diff --git a/waybionic_rviz_plugins/test/test_diagnostics_recorder_roundtrip.py b/waybionic_rviz_plugins/test/test_diagnostics_recorder_roundtrip.py new file mode 100644 index 0000000..bb1445e --- /dev/null +++ b/waybionic_rviz_plugins/test/test_diagnostics_recorder_roundtrip.py @@ -0,0 +1,108 @@ +import json +import os +import subprocess +import sys +import threading +import time +from pathlib import Path + +import pytest + +rclpy = pytest.importorskip("rclpy") +rosbag2_py = pytest.importorskip("rosbag2_py") +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from rclpy.serialization import deserialize_message + + +PACKAGE_ROOT = Path(__file__).resolve().parents[1] +RECORDER = PACKAGE_ROOT / "scripts" / "diagnostics_recorder.py" + + +def test_real_rosbag_round_trip_preserves_diagnostics(tmp_path): + domain_id = str(100 + os.getpid() % 100) + environment = os.environ.copy() + environment["ROS_DOMAIN_ID"] = domain_id + + previous_domain = os.environ.get("ROS_DOMAIN_ID") + os.environ["ROS_DOMAIN_ID"] = domain_id + rclpy.init(args=None) + node = rclpy.create_node("recorder_roundtrip_publisher") + publisher = node.create_publisher(DiagnosticArray, "/diagnostics", 10) + executor = rclpy.executors.SingleThreadedExecutor() + executor.add_node(node) + spin_thread = threading.Thread(target=executor.spin, daemon=True) + spin_thread.start() + + timestamp = node.get_clock().now().to_msg() + message = DiagnosticArray() + message.header.stamp = timestamp + status = DiagnosticStatus() + status.name = "board.temperature" + status.level = DiagnosticStatus.ERROR + status.message = "High temperature detected" + status.values = [ + KeyValue(key="value", value="82.5"), + KeyValue(key="unit", value="C"), + ] + message.status = [status] + timer = node.create_timer(0.1, lambda: publisher.publish(message)) + + try: + output = tmp_path / "roundtrip" + process = subprocess.Popen( + [ + sys.executable, + str(RECORDER), + "--duration", + "5", + "--output-directory", + str(output), + "--source-label", + "mock", + ], + env=environment, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + ) + stdout, stderr = process.communicate(timeout=10) + assert process.returncode == 0, stderr or stdout + finally: + timer.cancel() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + spin_thread.join(timeout=2) + if previous_domain is None: + os.environ.pop("ROS_DOMAIN_ID", None) + else: + os.environ["ROS_DOMAIN_ID"] = previous_domain + + metadata = json.loads((output / "metadata.json").read_text(encoding="utf-8")) + assert metadata["topic"] == "/diagnostics" + assert metadata["source_label"] == "mock" + + reader = rosbag2_py.SequentialReader() + reader.open( + rosbag2_py.StorageOptions(uri=str(output / "bag"), storage_id="mcap"), + rosbag2_py.ConverterOptions("", ""), + ) + recorded = [] + while reader.has_next(): + _, serialized, recorded_timestamp = reader.read_next() + recorded.append( + (deserialize_message(serialized, DiagnosticArray), recorded_timestamp) + ) + + assert recorded + recorded_message, bag_timestamp = recorded[0] + recorded_status = recorded_message.status[0] + assert recorded_status.name == "board.temperature" + assert recorded_status.level == DiagnosticStatus.ERROR + assert recorded_status.message == "High temperature detected" + assert [(item.key, item.value) for item in recorded_status.values] == [ + ("value", "82.5"), + ("unit", "C"), + ] + assert recorded_message.header.stamp == timestamp + assert bag_timestamp > 0