From 3bf7043060e8ad88ff5b4b2c5a5b2b79cf0980d3 Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:01:48 +0530 Subject: [PATCH 1/2] feat(ros): relay velocity commands on the simulation clock (T-093d) --- .github/workflows/ros-package.yml | 3 +- CHANGELOG.md | 2 + docs/10-simulator-integration.md | 6 + docs/28-traceability-matrix.md | 5 + ros_ws/src/roboprobe_ros/package.xml | 4 + .../src/roboprobe_ros/roboprobe_ros/base.py | 12 +- .../roboprobe_ros/roboprobe_ros/cmd_delay.py | 123 +++++++++++ ros_ws/src/roboprobe_ros/setup.py | 1 + .../src/roboprobe_ros/test/test_cmd_delay.py | 208 ++++++++++++++++++ 9 files changed, 362 insertions(+), 2 deletions(-) create mode 100644 ros_ws/src/roboprobe_ros/roboprobe_ros/cmd_delay.py create mode 100644 ros_ws/src/roboprobe_ros/test/test_cmd_delay.py diff --git a/.github/workflows/ros-package.yml b/.github/workflows/ros-package.yml index 2513fde1..ccfec37e 100644 --- a/.github/workflows/ros-package.yml +++ b/.github/workflows/ros-package.yml @@ -33,7 +33,8 @@ jobs: - name: Install Python test and numerical dependencies run: | apt-get update - apt-get install -y --no-install-recommends python3-numpy python3-scipy python3-yaml python3-pytest + apt-get install -y --no-install-recommends \ + python3-numpy python3-scipy python3-yaml python3-pytest ros-jazzy-launch-testing - name: Build and test working-directory: ros_ws run: | diff --git a/CHANGELOG.md b/CHANGELOG.md index 9259b11d..af4c6cd2 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 03d09ee7..72ff3af6 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -99,6 +99,12 @@ Materialisation writes into `workdir//`: | P6 delay | `perturb_params.yaml` | FIFO latency in `cmd_delay` | | P8 dropout | `perturb_params.yaml` | Bernoulli beam drop in `sensor_perturber` | +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 981f1607..16a7e58c 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 9c7dd2e8..1f8da128 100644 --- a/ros_ws/src/roboprobe_ros/package.xml +++ b/ros_ws/src/roboprobe_ros/package.xml @@ -11,12 +11,16 @@ sensor_msgs sensor_msgs_py geometry_msgs + std_msgs nav_msgs nav2_msgs python3-numpy python3-scipy python3-yaml python3-pytest + launch_testing + launch_ros + 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 f5606c6f..966e4444 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 00000000..6d8a18c2 --- /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 395d4dbb..da6560e8 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -20,6 +20,7 @@ ), ], install_requires=["setuptools"], + entry_points={"console_scripts": ["cmd_delay = roboprobe_ros.cmd_delay:main"]}, extras_require={"recorder": ["pyarrow>=17"]}, 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 00000000..dd9d11fb --- /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) From 24311b0ca5c591f0e12d7f9a034678c057faa6ae Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Sun, 22 Nov 2026 20:41:26 +0000 Subject: [PATCH 2/2] fix(ros): merge console_scripts entry points after main --- ros_ws/src/roboprobe_ros/setup.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros_ws/src/roboprobe_ros/setup.py b/ros_ws/src/roboprobe_ros/setup.py index 9d5c68ef..2a3f42be 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -20,10 +20,12 @@ ), ], install_requires=["setuptools"], - entry_points={"console_scripts": ["cmd_delay = roboprobe_ros.cmd_delay:main"]}, 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",