From 4f9c903340edf58f3f66ef72b4af4c1193fb3e8e Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 01:52:50 +0530 Subject: [PATCH 1/2] feat(ros): degrade depth point clouds using episode light level (T-093c) --- CHANGELOG.md | 2 + docs/10-simulator-integration.md | 6 + docs/28-traceability-matrix.md | 2 +- ros_ws/src/roboprobe_ros/package.xml | 1 + .../roboprobe_ros/perception_degrader.py | 99 ++++++++++++ ros_ws/src/roboprobe_ros/setup.py | 5 + .../test/test_perception_degrader.py | 146 ++++++++++++++++++ 7 files changed, 260 insertions(+), 1 deletion(-) create mode 100644 ros_ws/src/roboprobe_ros/roboprobe_ros/perception_degrader.py create mode 100644 ros_ws/src/roboprobe_ros/test/test_perception_degrader.py diff --git a/CHANGELOG.md b/CHANGELOG.md index f8884cf1..569f444d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,8 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33 workers and dashboard with same-origin SSE. - 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 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 03d09ee7..a1708c4f 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 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 10ee8fc9..ee69f3b5 100644 --- a/docs/28-traceability-matrix.md +++ b/docs/28-traceability-matrix.md @@ -9,7 +9,7 @@ | PRD | SRS requirements | Design (docs §) | Tasks (22) | Verification (18) | |-----|------------------|-----------------|-----------|-------------------| | PR-01 Scenario space | FR-SCN-001…007 | 06 §2–5, 05 §3 | T-020…T-025 | unit/property `tests/unit/scenario`, schema fixtures | -| PR-02 Autonomous campaigns | FR-RUN-001…006, FR-SIM-001, FR-SIM-003 | 04 §4–6, 05 §5, 10, 11 §4, 14 §2–5 | T-012, T-033, T-035, T-040…T-047, T-091, T-093a, T-096 | integration runner, chaos, backend contract, Jazzy package tests | +| PR-02 Autonomous campaigns | FR-RUN-001…006, FR-SIM-001, FR-SIM-003 | 04 §4–6, 05 §5, 10, 11 §4, 14 §2–5 | T-012, T-033, T-035, T-040…T-047, T-091, T-093a, T-093c, T-096 | integration runner, chaos, backend contract, Jazzy package tests, `test_perception_degrader.py` | | PR-03 Intelligent selection | FR-SEL-002/003/005/006/007/008 | 07 §4–6 | T-055…T-061 | strategy contract, benchmark gates 18 §4.4, EXP-01 | | PR-04 Baselines | FR-SEL-001 | 07 §3 | T-050, T-051 | strategy contract, design tests | | PR-05 Failure detection | FR-ORC-001…006 | 08 | T-036…T-039 | golden traces, invariants | diff --git a/ros_ws/src/roboprobe_ros/package.xml b/ros_ws/src/roboprobe_ros/package.xml index 9c7dd2e8..4a485f92 100644 --- a/ros_ws/src/roboprobe_ros/package.xml +++ b/ros_ws/src/roboprobe_ros/package.xml @@ -17,6 +17,7 @@ python3-scipy python3-yaml python3-pytest + std_msgs ament_python 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 00000000..ced3f199 --- /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 395d4dbb..fc44e6ce 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -20,6 +20,11 @@ ), ], install_requires=["setuptools"], + entry_points={ + "console_scripts": [ + "perception_degrader = roboprobe_ros.perception_degrader:main", + ], + }, extras_require={"recorder": ["pyarrow>=17"]}, zip_safe=False, maintainer="CodewithJha", 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 00000000..4178c30c --- /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) + ) From bb47771406b0b76483ff7466088661d62eaab5ef Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Sun, 22 Nov 2026 20:41:09 +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, 2 insertions(+), 4 deletions(-) diff --git a/ros_ws/src/roboprobe_ros/setup.py b/ros_ws/src/roboprobe_ros/setup.py index a52c3092..db534b54 100644 --- a/ros_ws/src/roboprobe_ros/setup.py +++ b/ros_ws/src/roboprobe_ros/setup.py @@ -20,15 +20,13 @@ ), ], install_requires=["setuptools"], + extras_require={"recorder": ["pyarrow>=17"]}, entry_points={ "console_scripts": [ "perception_degrader = roboprobe_ros.perception_degrader:main", + "sensor_perturber = roboprobe_ros.sensor_perturber:main", ], }, - extras_require={"recorder": ["pyarrow>=17"]}, - entry_points={ - "console_scripts": ["sensor_perturber = roboprobe_ros.sensor_perturber:main"], - }, zip_safe=False, maintainer="CodewithJha", maintainer_email="155089480+CodewithJha@users.noreply.github.com",