diff --git a/CHANGELOG.md b/CHANGELOG.md index c0cc777..9c854f3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,8 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33 - Dashboard polish (T-113a/b, T-115): Spaces list with YAML view and validation, Workbench worker table, Settings (session token, theme, number locale, SSE reconnect), report export with sandboxed preview route, and global keyboard shortcuts dialog. +- Jazzy command relay (T-093d) applies FIFO velocity delays on the simulation clock and + publishes observed delay for the episode recorder. - Web production image (T-116): read-only nginx-unprivileged SPA, runtime `/config.json`, CSP and security headers, and a Compose `app` profile that starts the authenticated API, workers and dashboard with same-origin SSE. diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index e5351fe..27f2a0a 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -103,6 +103,12 @@ Give the sensor-data topics `scan_raw` and `scan` separate resolved names to pre The node publishes cumulative `count` (processed beams) and `dropped` (new `+inf` returns) as JSON in `std_msgs/msg/String` once per second on `/roboprobe/perturber_stats`. +The `cmd_delay` node reads `cmd_delay_ms` and the Nav2 controller's `rate_hz` from +`perturb_params.yaml`. It keeps messages in order on the simulation clock and publishes the +observed latency in **seconds** as `std_msgs/msg/Float64` on `/roboprobe/cmd_delay_actual`. +The `stamped_cmd_vel` flag in `topics.yaml` selects the ROS message type. Zero delay forwards +the message in its subscription callback with a reported latency of zero. + Friction and lighting are world properties ⇒ a fresh world load per run (no warm reset in MVP; see §8). **SUT profiles** (`configs/sut_profiles/.yaml`, schema `schemas/sut_profile.schema.json`, loader diff --git a/docs/28-traceability-matrix.md b/docs/28-traceability-matrix.md index 8257658..a637af2 100644 --- a/docs/28-traceability-matrix.md +++ b/docs/28-traceability-matrix.md @@ -42,6 +42,11 @@ and ETags; `test_uow_lifecycle.py` covers transaction entry/exit. Shared factory `tests/unit/runner/test_campaign_factory.py`; SQL filter/keyset checks and PostgreSQL pagination are in `tests/unit/store/test_run_queries.py` and `tests/integration/store/test_run_queries.py`. +Gazebo velocity relay verification (T-093d, #195, FR-SIM-003): +`ros_ws/src/roboprobe_ros/test/test_cmd_delay.py` launches installed Jazzy relays +against a fake simulation clock and verifies both velocity message types, FIFO ordering, +reported latency, zero-delay delivery and process shutdown. + Trace transport verification (T-081e, #150; FR-DSH-005, FR-STO-002, NFR-SEC-003): `tests/api/test_trace_artifacts.py` covers clearance minima, flag transitions, rate caps, column selection, Parquet/attachment headers, unknown ids, and malicious stored paths (TM-04). diff --git a/ros_ws/src/roboprobe_ros/package.xml b/ros_ws/src/roboprobe_ros/package.xml index 7ac33e9..ee2d52b 100644 --- a/ros_ws/src/roboprobe_ros/package.xml +++ b/ros_ws/src/roboprobe_ros/package.xml @@ -12,6 +12,7 @@ sensor_msgs_py std_msgs geometry_msgs + std_msgs nav_msgs nav2_msgs python3-numpy @@ -20,6 +21,7 @@ python3-pytest launch_ros launch_testing + rosgraph_msgs ament_python diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py index f5606c6..966e444 100644 --- a/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py @@ -36,6 +36,14 @@ def _read_yaml(path: Path) -> dict[str, object]: return _mapping(yaml.safe_load(stream), str(path)) +def _stamped_cmd_vel(config: dict[str, object]) -> bool: + stamped = config.get("stamped_cmd_vel", False) + if not isinstance(stamped, bool): + msg = "stamped_cmd_vel must be a boolean" + raise TypeError(msg) + return stamped + + class RoboProbeNode(Node): """Load generated episode data and expose deterministic node utilities.""" @@ -55,7 +63,9 @@ def __init__( seed = self.declare_parameter("seed", 0) topics_path = Path(topic_file.get_parameter_value().string_value) perturb_path = perturb_file.get_parameter_value().string_value - self._topics = _mapping(_read_yaml(topics_path).get("topics"), "topics") + topic_config = _read_yaml(topics_path) + self._topics = _mapping(topic_config.get("topics"), "topics") + self.stamped_cmd_vel = _stamped_cmd_vel(topic_config) self.perturbation = _read_yaml(Path(perturb_path)) if perturb_path else {} self.rng = Generator(PCG64(seed.get_parameter_value().integer_value)) except Exception: diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/cmd_delay.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/cmd_delay.py new file mode 100644 index 0000000..6d8a18c --- /dev/null +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/cmd_delay.py @@ -0,0 +1,123 @@ +"""Sim-time command latency relay for Jazzy Nav2 (T-093d, FR-SIM-003).""" + +from __future__ import annotations + +import math +from typing import TYPE_CHECKING, Final + +import rclpy +from geometry_msgs.msg import Twist, TwistStamped +from rclpy.clock import JumpThreshold +from rclpy.duration import Duration +from rclpy.executors import ExternalShutdownException +from rclpy.parameter import Parameter +from std_msgs.msg import Float64 + +from roboprobe_ros._vendor.perturbation.delay import CommandDelayBuffer +from roboprobe_ros.base import RoboProbeNode + +if TYPE_CHECKING: + from rclpy.clock import TimeJump + from rclpy.context import Context + +__all__ = ["CommandDelay", "main"] + +_DELAY_ACTUAL_TOPIC: Final = "/roboprobe/cmd_delay_actual" +_MS_PER_S: Final = 1000 +_NS_PER_S: Final = 1_000_000_000 + +type Command = Twist | TwistStamped + + +def _number(values: dict[str, object], key: str) -> float: + value = values.get(key) + if isinstance(value, bool) or not isinstance(value, int | float): + msg = f"perturb_params.yaml must specify numeric {key}" + raise TypeError(msg) + if not math.isfinite(value): + msg = f"{key} must be finite" + raise ValueError(msg) + return float(value) + + +class CommandDelay(RoboProbeNode): + """Apply the shared FIFO model on the same clock as the simulation.""" + + def __init__( + self, + *, + context: Context | None = None, + parameter_overrides: list[Parameter] | None = None, + ) -> None: + """Enable simulation time and configure the episode relay.""" + overrides = [*(parameter_overrides or ()), Parameter("use_sim_time", value=True)] + super().__init__("cmd_delay", context=context, parameter_overrides=overrides) + try: + self._configure() + except Exception: + self.destroy_node() + raise + + def _configure(self) -> None: + self._delay_s = _number(self.perturbation, "cmd_delay_ms") / _MS_PER_S + rate_hz = _number(self.perturbation, "rate_hz") + if rate_hz <= 0.0: + msg = "rate_hz must be positive" + raise ValueError(msg) + self._queue = CommandDelayBuffer[tuple[float, Command]](self._delay_s) + raw = self.topic("cmd_vel_nav") + output = self.topic("cmd_vel") + if self.resolve_topic_name(raw) == self.resolve_topic_name(output): + msg = "cmd_vel_nav and cmd_vel must resolve to distinct topics" + raise ValueError(msg) + + message_type = TwistStamped if self.stamped_cmd_vel else Twist + output_qos = self.qos("cmd_vel") + self._command = self.create_publisher(message_type, output, output_qos) + self._latency = self.create_publisher(Float64, _DELAY_ACTUAL_TOPIC, output_qos) + self.create_subscription(message_type, raw, self._receive, self.qos("cmd_vel_nav")) + if self._delay_s > 0.0: + self.create_timer(1.0 / rate_hz, self._release) + self._jump_handler = self.get_clock().create_jump_callback( + JumpThreshold( + min_forward=None, min_backward=Duration(nanoseconds=-1), on_clock_change=True + ), + post_callback=self._reset_queue, + ) + + def _reset_queue(self, _jump: TimeJump) -> None: + # The shared FIFO disallows clock regressions, so a new timeline gets a new FIFO. + self._queue = CommandDelayBuffer[tuple[float, Command]](self._delay_s) + + def _receive(self, command: Command) -> None: + if self._delay_s == 0.0: + self._send(command, 0.0) + return + now = self.get_clock().now().nanoseconds / _NS_PER_S + # ROS reports zero until the first simulation clock sample arrives. + if now > 0.0: + self._queue.push(now, (now, command)) + + def _release(self) -> None: + now = self.get_clock().now().nanoseconds / _NS_PER_S + for enqueued, command in self._queue.pop_due(now): + self._send(command, now - enqueued) + + def _send(self, command: Command, actual_s: float) -> None: + self._command.publish(command) + self._latency.publish(Float64(data=actual_s)) + + +def main(args: list[str] | None = None) -> None: + """Start the relay and release its resources on ROS shutdown.""" + rclpy.init(args=args) + try: + node = CommandDelay() + try: + rclpy.spin(node) + finally: + node.destroy_node() + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + rclpy.try_shutdown() diff --git a/ros_ws/src/roboprobe_ros/setup.py b/ros_ws/src/roboprobe_ros/setup.py index e27b0ca..2a3f42b 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -22,7 +22,10 @@ install_requires=["setuptools"], extras_require={"recorder": ["pyarrow>=17"]}, entry_points={ - "console_scripts": ["sensor_perturber = roboprobe_ros.sensor_perturber:main"], + "console_scripts": [ + "cmd_delay = roboprobe_ros.cmd_delay:main", + "sensor_perturber = roboprobe_ros.sensor_perturber:main", + ], }, zip_safe=False, maintainer="CodewithJha", diff --git a/ros_ws/src/roboprobe_ros/test/test_cmd_delay.py b/ros_ws/src/roboprobe_ros/test/test_cmd_delay.py new file mode 100644 index 0000000..dd9d11f --- /dev/null +++ b/ros_ws/src/roboprobe_ros/test/test_cmd_delay.py @@ -0,0 +1,208 @@ +"""Exercise command delivery against a deterministic fake Jazzy clock (T-093d).""" + +# ruff: noqa: S101 + +from __future__ import annotations + +from pathlib import Path +from tempfile import TemporaryDirectory +from time import monotonic +from typing import TYPE_CHECKING +from unittest import TestCase + +import launch +import launch_testing +import pytest +import rclpy +import yaml +from geometry_msgs.msg import Twist, TwistStamped +from launch_ros.actions import Node as LaunchNode +from launch_testing.actions import ReadyToTest +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data +from rosgraph_msgs.msg import Clock +from std_msgs.msg import Float64 + +if TYPE_CHECKING: + from collections.abc import Callable + +_DELAY_MS = 65.0 +_RATE_HZ = 20.0 +_CLOCK_STEP_MS = 10 +_CONTROL_QUEUE_DEPTH = 10 +_CLOCK_SETTLE_S = 0.050 +_NANOSECONDS_PER_MILLISECOND = 1_000_000 +_ZERO = "zero" +_PLAIN = "plain" +_STAMPED = "stamped" +_CASES = {_PLAIN: Twist, _STAMPED: TwistStamped, _ZERO: Twist} + + +def _topic(case: str, kind: str) -> str: + return f"/cmd_delay_test/{case}/{kind}" + + +@pytest.mark.launch_test +def generate_test_description() -> tuple[launch.LaunchDescription, dict[str, object]]: + """Start installed command relays for both ROS message types and zero delay.""" + files = TemporaryDirectory(prefix="roboprobe-cmd-delay-") + root = Path(files.name) + nodes = [] + for case, message in _CASES.items(): + topics = root / f"{case}_topics.yaml" + perturb = root / f"{case}_perturb.yaml" + topics.write_text( + yaml.safe_dump( + { + "stamped_cmd_vel": message is TwistStamped, + "topics": { + "cmd_vel_nav": {"name": _topic(case, "in"), "qos": "default"}, + "cmd_vel": {"name": _topic(case, "out"), "qos": "default"}, + }, + } + ), + encoding="utf-8", + ) + perturb.write_text( + yaml.safe_dump( + {"cmd_delay_ms": 0.0 if case == _ZERO else _DELAY_MS, "rate_hz": _RATE_HZ} + ), + encoding="utf-8", + ) + nodes.append( + LaunchNode( + package="roboprobe_ros", + executable="cmd_delay", + name=f"test_cmd_delay_{case}", + parameters=[{"topics_file": str(topics), "perturb_file": str(perturb)}], + remappings=[ + ("/clock", _topic(case, "clock")), + ("/roboprobe/cmd_delay_actual", _topic(case, "delay")), + ], + ) + ) + return launch.LaunchDescription([*nodes, ReadyToTest()]), {"test_files": files} + + +def _spin_until(executor: SingleThreadedExecutor, ready: Callable[[], bool]) -> None: + deadline = monotonic() + 10.0 + while not ready(): + assert monotonic() < deadline, "Timed out waiting for command relay DDS" + executor.spin_once(timeout_sec=0.05) + + +def _advance(executor: SingleThreadedExecutor, duration: float) -> None: + deadline = monotonic() + duration + while monotonic() < deadline: + executor.spin_once(timeout_sec=max(0.0, deadline - monotonic())) + + +def _command(msg_type: type[Twist] | type[TwistStamped], speed: float) -> Twist | TwistStamped: + if msg_type is TwistStamped: + stamped = TwistStamped() + stamped.header.frame_id = "base_link" + stamped.header.stamp.sec = 3 + stamped.twist.linear.x = speed + return stamped + plain = Twist() + plain.linear.x = speed + return plain + + +class TestCommandDelay(TestCase): + """Test FIFO delivery, message preservation, and latency in simulation time.""" + + def test_twist(self) -> None: + """Release two unstamped commands in FIFO order.""" + self._check(_PLAIN) + + def test_twist_stamped(self) -> None: + """Preserve the TwistStamped header and release in FIFO order.""" + self._check(_STAMPED) + + def test_zero_delay(self) -> None: + """Forward commands immediately while the simulation clock is frozen.""" + self._check(_ZERO) + + def _check(self, case: str) -> None: + context = Context() + rclpy.init(context=context) + node = Node(f"probe_{case}", context=context) + executor = SingleThreadedExecutor(context=context) + executor.add_node(node) + msg_type = _CASES[case] + clock = node.create_publisher(Clock, _topic(case, "clock"), qos_profile_sensor_data) + control_qos = QoSProfile(depth=_CONTROL_QUEUE_DEPTH) + source = node.create_publisher(msg_type, _topic(case, "in"), control_qos) + messages: list[Twist | TwistStamped] = [] + latencies: list[float] = [] + output = node.create_subscription( + msg_type, _topic(case, "out"), messages.append, control_qos + ) + delay = node.create_subscription( + Float64, + _topic(case, "delay"), + lambda value: latencies.append(value.data), + control_qos, + ) + + def clock_to(elapsed_ms: int) -> None: + stamp = Clock() + stamp.clock.sec = 10 + stamp.clock.nanosec = elapsed_ms * _NANOSECONDS_PER_MILLISECOND + clock.publish(stamp) + _advance(executor, _CLOCK_SETTLE_S) + + try: + _spin_until( + executor, + lambda: all( + ( + clock.get_subscription_count(), + source.get_subscription_count(), + output.get_publisher_count(), + delay.get_publisher_count(), + ) + ), + ) + for _ in range(3): + clock_to(0) + expected = [_command(msg_type, 0.10), _command(msg_type, 0.20)] + for command in expected: + source.publish(command) + _advance(executor, _CLOCK_SETTLE_S) + if case == _ZERO: + _spin_until(executor, lambda: bool(messages and latencies)) + else: + _advance(executor, 0.150) + assert not messages + for instant in range(_CLOCK_STEP_MS, int(_DELAY_MS), _CLOCK_STEP_MS): + clock_to(instant) + assert not messages + for instant in range(70, 121, _CLOCK_STEP_MS): + clock_to(instant) + _spin_until(executor, lambda: len(messages) == len(latencies) == len(expected)) + assert messages == expected + if case == _ZERO: + assert latencies == [0.0] * len(expected) + else: + expected_sec = _DELAY_MS / 1000.0 + assert all( + expected_sec - 1e-9 <= actual <= expected_sec + 1.0 / _RATE_HZ + for actual in latencies + ) + finally: + executor.shutdown() + node.destroy_node() + rclpy.shutdown(context=context) + + +@launch_testing.post_shutdown_test() +class TestCommandDelayShutdown(TestCase): + """The installed relays handle launch teardown cleanly.""" + + def test_exit_codes(self, proc_info: object) -> None: + """Require every command relay to exit normally.""" + launch_testing.asserts.assertExitCodes(proc_info)