diff --git a/CHANGELOG.md b/CHANGELOG.md index 9c854f3..81addbb 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -26,6 +26,8 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33 - RoboProbe ROS package (T-093a) loads episode parameters and topic QoS, provides seeded node utilities, and checks vendored numerical modules against the backend source. - Jazzy lidar relay (T-093b) applies seeded Gaussian noise and dropout and publishes beam counters. +- Jazzy depth relay (T-093c) applies seeded low-light dropout and radial range noise to + PointCloud2 frames while preserving their timestamps and frames of reference. - Opt-in experiment API launch atomically creates the planned campaign matrix and runs it through the orchestrator and workers (T-084a). - Casebook and Insights (T-110a/b): filter and edit analyzed failure modes, replay or export diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index 27f2a0a..827a433 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -109,6 +109,12 @@ observed latency in **seconds** as `std_msgs/msg/Float64` on `/roboprobe/cmd_del 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. +The Jazzy depth relay reads `illuminance_lux` and `perception_model_file` from the episode's +`perturb_params.yaml`, with the model path taken from `backends.gazebo_nav2.perception_model_file`. +It consumes `depth_points_raw` and publishes XYZ points on `depth_points` with depth-one sensor QoS. +It preserves the source frame and timestamp, omits invalid and lost returns, and logs elapsed milliseconds +for each processed cloud. The installed OAK-D simulation publishes 320×240 points at 10 Hz. + 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 a637af2..4613427 100644 --- a/docs/28-traceability-matrix.md +++ b/docs/28-traceability-matrix.md @@ -31,6 +31,10 @@ | PR-21 S-CEM | FR-SEL-009 | 07 §8 | T-062 | contract | | PR-22 Observability profile | NFR-OBS-001/002 | 21 §5 | T-004, T-125, T-126 | metrics registry cross-check; tracing no-import test | +Gazebo depth verification (T-093c, #194, FR-SIM-003): the Jazzy test +`ros_ws/src/roboprobe_ros/test/test_perception_degrader.py` checks seeded depth loss and +radial noise, camera header preservation, depth-one QoS and processing within the camera period. + Catalogue API verification (T-081a, #146; FR-API-001, FR-SCN-001, UC-01/UC-10): `backend/tests/api/test_spaces.py`, `test_sut_profiles.py` and `test_upload_body.py` cover YAML/JSON uploads, semantic validation, JSON pointers, versioning, idempotency, profile conflicts, diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/perception_degrader.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/perception_degrader.py new file mode 100644 index 0000000..ced3f19 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/perception_degrader.py @@ -0,0 +1,99 @@ +"""ROS 2 camera relay for episode-constant low-light depth loss (FR-SIM-003).""" + +from __future__ import annotations + +from pathlib import Path +from time import perf_counter +from typing import TYPE_CHECKING, Final + +import numpy as np +import rclpy +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 + +from roboprobe_ros._vendor.perturbation.perception import PerceptionModel +from roboprobe_ros.base import RoboProbeNode + +if TYPE_CHECKING: + from collections.abc import Sequence + + from rclpy.context import Context + from rclpy.parameter import Parameter + +_FIELDS: Final = ("x", "y", "z") +_QUEUE_DEPTH: Final = 1 + + +class PerceptionDegrader(RoboProbeNode): + """Apply the configured radial depth model to the current camera frame.""" + + def __init__( + self, + *, + context: Context | None = None, + parameter_overrides: list[Parameter] | None = None, + ) -> None: + """Load the versioned model and attach the two depth-one endpoints.""" + super().__init__( + "perception_degrader", context=context, parameter_overrides=parameter_overrides + ) + try: + self._configure() + except Exception: + self.destroy_node() + raise + + def _configure(self) -> None: + illuminance = self.perturbation.get("illuminance_lux") + if not isinstance(illuminance, int | float) or isinstance(illuminance, bool): + msg = "perturb_params.yaml must specify numeric illuminance_lux" + raise TypeError(msg) + model_file = self.perturbation.get("perception_model_file") + if not isinstance(model_file, str) or not model_file: + msg = "perturb_params.yaml must specify perception_model_file" + raise ValueError(msg) + self._illuminance = float(illuminance) + self._model = PerceptionModel.from_yaml(Path(model_file)) + self._model.eta(self._illuminance) + + raw = self.topic("depth_points_raw") + processed = self.topic("depth_points") + if self.resolve_topic_name(raw) == self.resolve_topic_name(processed): + msg = "depth_points_raw and depth_points must resolve to distinct topics" + raise ValueError(msg) + raw_qos = self.qos("depth_points_raw") + raw_qos.depth = _QUEUE_DEPTH + processed_qos = self.qos("depth_points") + processed_qos.depth = _QUEUE_DEPTH + self._publisher = self.create_publisher(PointCloud2, processed, processed_qos) + self.create_subscription(PointCloud2, raw, self._process_cloud, raw_qos) + + def _process_cloud(self, cloud: PointCloud2) -> None: + started = perf_counter() + xyz = point_cloud2.read_points_numpy(cloud, field_names=_FIELDS) + coordinates = np.asarray(xyz, dtype=np.float64) + ranges = np.linalg.norm(coordinates, axis=1) + noisy = self._model.degrade(ranges, self._illuminance, self.rng) + retained = np.isfinite(noisy) + distances = ranges[retained] + scale = np.divide( + noisy[retained], distances, out=np.zeros_like(distances), where=distances > 0.0 + ) + points = (coordinates[retained] * scale[:, None]).astype(np.float32) + self._publisher.publish(point_cloud2.create_cloud_xyz32(cloud.header, points)) + self.log_event( + "perception.cloud_processed", + input_points=len(coordinates), + retained_points=len(points), + processing_ms=(perf_counter() - started) * 1000, + ) + + +def main(args: Sequence[str] | None = None) -> None: + """Run the perception relay until ROS shuts down.""" + with rclpy.init(args=args): + node = PerceptionDegrader() + try: + rclpy.spin(node) + finally: + node.destroy_node() diff --git a/ros_ws/src/roboprobe_ros/setup.py b/ros_ws/src/roboprobe_ros/setup.py index 2a3f42b..6a01d62 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -24,6 +24,7 @@ entry_points={ "console_scripts": [ "cmd_delay = roboprobe_ros.cmd_delay:main", + "perception_degrader = roboprobe_ros.perception_degrader:main", "sensor_perturber = roboprobe_ros.sensor_perturber:main", ], }, diff --git a/ros_ws/src/roboprobe_ros/test/test_perception_degrader.py b/ros_ws/src/roboprobe_ros/test/test_perception_degrader.py new file mode 100644 index 0000000..4178c30 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/test/test_perception_degrader.py @@ -0,0 +1,146 @@ +"""Test the Jazzy point-cloud relay against the configured model (T-093c).""" + +# ruff: noqa: S101 + +from __future__ import annotations + +import json +from pathlib import Path +from time import monotonic +from typing import TYPE_CHECKING + +import numpy as np +import numpy.testing as npt +import pytest +import rclpy +from rclpy.context import Context +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import QoSProfile, QoSReliabilityPolicy +from roboprobe_ros._vendor.perturbation.perception import PerceptionModel +from roboprobe_ros.perception_degrader import PerceptionDegrader +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 +from std_msgs.msg import Header + +if TYPE_CHECKING: + from collections.abc import Iterator + +_MODEL = Path(__file__).resolve().parents[4] / "configs/perception/oakd_lowlight_v1.yaml" +_INPUT = "/test/depth_raw" +_OUTPUT = "/test/depth" +_LUX_LEVELS = (5.0, 50.0, 500.0) +_DEPTH = 1 +_SIZE = 320 * 240 +_CAMERA_PERIOD_MS = 100.0 +_SEED = 303 + + +@pytest.fixture +def ros_context() -> Iterator[Context]: + """Start and stop an isolated ROS context.""" + context = Context() + rclpy.init(context=context) + try: + yield context + finally: + rclpy.shutdown(context=context) + + +def _write_parameters(path: Path, lux: float, output: str = _OUTPUT) -> list[Parameter]: + topics = path / "topics.yaml" + perturb = path / "perturb_params.yaml" + topics.write_text( + f"topics:\n" + f" depth_points_raw: {{name: {_INPUT}, qos: sensor_data}}\n" + f" depth_points: {{name: {output}, qos: sensor_data}}\n", + encoding="utf-8", + ) + perturb.write_text( + f"illuminance_lux: {lux}\nperception_model_file: {_MODEL}\n", encoding="utf-8" + ) + return [ + Parameter("topics_file", value=str(topics)), + Parameter("perturb_file", value=str(perturb)), + Parameter("seed", value=_SEED), + ] + + +@pytest.mark.parametrize("lux", _LUX_LEVELS) +def test_point_cloud_relay( + lux: float, ros_context: Context, tmp_path: Path, capsys: pytest.CaptureFixture[str] +) -> None: + """Verify deterministic output, retention, metadata, and QoS via DDS.""" + overrides = _write_parameters(tmp_path, lux) + relay = PerceptionDegrader(context=ros_context, parameter_overrides=overrides) + peer = Node("cloud_test", context=ros_context) + executor = SingleThreadedExecutor(context=ros_context) + executor.add_node(relay) + executor.add_node(peer) + received: list[PointCloud2] = [] + qos = QoSProfile(depth=_DEPTH, reliability=QoSReliabilityPolicy.BEST_EFFORT) + sink = peer.create_subscription(PointCloud2, _OUTPUT, received.append, qos) + source = peer.create_publisher(PointCloud2, _INPUT, qos) + try: + header = Header(frame_id="camera_optical_frame") + header.stamp.sec = 123 + header.stamp.nanosec = 456 + xyz = np.tile(np.array([0.3, 0.0, 0.8], dtype=np.float32), (_SIZE, 1)) + # A NaN from the camera and a point beyond every configured range are both invalid. + unmeasurable = np.array([[np.nan, 0.0, 0.0], [10.0, 0.0, 0.0]], dtype=np.float32) + cloud = point_cloud2.create_cloud_xyz32(header, np.concatenate((xyz, unmeasurable))) + + deadline = monotonic() + 5.0 + while not received and monotonic() < deadline: + source.publish(cloud) + executor.spin_once(timeout_sec=0.1) + executor.spin_once(timeout_sec=0.1) + assert received, "Jazzy DDS did not deliver the relayed cloud" + actual = received[0] + assert actual.header == header + # DDS discovery may not report endpoint history depth. Check the local profiles. + depth_endpoints = [ + endpoint + for endpoint in (*relay.publishers, *relay.subscriptions) + if endpoint.topic_name in {_INPUT, _OUTPUT} + ] + assert {endpoint.topic_name for endpoint in depth_endpoints} == {_INPUT, _OUTPUT} + assert all(endpoint.qos_profile.depth == _DEPTH for endpoint in depth_endpoints) + assert all( + endpoint.qos_profile.reliability == qos.reliability for endpoint in depth_endpoints + ) + assert source.get_subscription_count() == 1 + assert sink.get_publisher_count() == 1 + + model = PerceptionModel.from_yaml(_MODEL) + lengths = np.linalg.norm(xyz.astype(np.float64), axis=1) + expected = model.degrade( + np.concatenate((lengths, [np.nan, 10.0])), lux, np.random.default_rng(_SEED) + )[:_SIZE] + retained = np.isfinite(expected) + points = point_cloud2.read_points_numpy(actual, field_names=("x", "y", "z")) + scaled = xyz[retained] * (expected[retained] / lengths[retained])[:, None] + npt.assert_allclose(points, scaled) + assert actual.width / _SIZE == pytest.approx(model.p_valid(model.eta(lux)), abs=0.012) + events = [json.loads(line) for line in capsys.readouterr().out.splitlines()] + first = next(event for event in events if event["event"] == "perception.cloud_processed") + assert first["input_points"] == _SIZE + len(unmeasurable) + assert first["retained_points"] == actual.width + assert 0.0 <= first["processing_ms"] < _CAMERA_PERIOD_MS + finally: + executor.shutdown() + peer.destroy_node() + relay.destroy_node() + + +def test_rejects_invalid_episode_setup(ros_context: Context, tmp_path: Path) -> None: + """Reject invalid illuminance and refuse to relay onto the input topic.""" + with pytest.raises(ValueError, match="illuminance"): + PerceptionDegrader( + context=ros_context, parameter_overrides=_write_parameters(tmp_path, 0.0) + ) + with pytest.raises(ValueError, match="distinct topics"): + PerceptionDegrader( + context=ros_context, parameter_overrides=_write_parameters(tmp_path, 50.0, _INPUT) + )