Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 6 additions & 0 deletions docs/10-simulator-integration.md
Original file line number Diff line number Diff line change
Expand Up @@ -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/<id>.yaml`, schema `schemas/sut_profile.schema.json`, loader
Expand Down
4 changes: 4 additions & 0 deletions docs/28-traceability-matrix.md
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
99 changes: 99 additions & 0 deletions ros_ws/src/roboprobe_ros/roboprobe_ros/perception_degrader.py
Original file line number Diff line number Diff line change
@@ -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()
1 change: 1 addition & 0 deletions ros_ws/src/roboprobe_ros/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
],
},
Expand Down
146 changes: 146 additions & 0 deletions ros_ws/src/roboprobe_ros/test/test_perception_degrader.py
Original file line number Diff line number Diff line change
@@ -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)
)
Loading