From e015720693ea51f1ff4d8f6aab66c81d24b7ff6c Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:54:43 +0530 Subject: [PATCH 1/2] =?UTF-8?q?feat(gazebo):=20generate=20deterministic=20?= =?UTF-8?q?episode=20inputs=20for=20P1=E2=80=93P8=20(T-096a)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CHANGELOG.md | 1 + .../backends/gazebo_nav2/__init__.py | 1 + .../backends/gazebo_nav2/materialize.py | 367 ++++++++++++++++ .../backends/gazebo_nav2/nav2_params.py | 196 +++++++++ .../src/roboprobe/backends/gazebo_nav2/sdf.py | 128 ++++++ .../fixtures/gazebo_materialize/bridge.yaml | 45 ++ .../fixtures/gazebo_materialize/episode.yaml | 30 ++ .../gazebo_materialize/nav2_params.yaml | 34 ++ .../gazebo_materialize/perturb_params.yaml | 12 + .../fixtures/gazebo_materialize/world.sdf | 14 + .../unit/backends/gazebo/test_materialize.py | 411 ++++++++++++++++++ docs/10-simulator-integration.md | 18 + docs/28-traceability-matrix.md | 4 + 13 files changed, 1261 insertions(+) create mode 100644 backend/src/roboprobe/backends/gazebo_nav2/__init__.py create mode 100644 backend/src/roboprobe/backends/gazebo_nav2/materialize.py create mode 100644 backend/src/roboprobe/backends/gazebo_nav2/nav2_params.py create mode 100644 backend/src/roboprobe/backends/gazebo_nav2/sdf.py create mode 100644 backend/tests/fixtures/gazebo_materialize/bridge.yaml create mode 100644 backend/tests/fixtures/gazebo_materialize/episode.yaml create mode 100644 backend/tests/fixtures/gazebo_materialize/nav2_params.yaml create mode 100644 backend/tests/fixtures/gazebo_materialize/perturb_params.yaml create mode 100644 backend/tests/fixtures/gazebo_materialize/world.sdf create mode 100644 backend/tests/unit/backends/gazebo/test_materialize.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 3d43721e..4f159230 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33 ### Added - The verified warehouse occupancy map, aligned kinematic map, and Nav2 nominal path for cross-backend pallet placement (T-092). +- Deterministic Gazebo episode materialization for the warehouse world, Nav2, sensor perturbations, and bridge configuration (T-096a). - Verified TB4 Nav2 topic map (`ros_ws/src/roboprobe_ros/config/topics.yaml`), JSON Schema, and evidence capture script (T-091). - Playwright E2E harness and journeys (T-117, T-118, T-114): compose-backed global setup with seeded warehouse campaign, axe helper and page objects, journey specs, `e2e.yml`, Lighthouse diff --git a/backend/src/roboprobe/backends/gazebo_nav2/__init__.py b/backend/src/roboprobe/backends/gazebo_nav2/__init__.py new file mode 100644 index 00000000..631a3928 --- /dev/null +++ b/backend/src/roboprobe/backends/gazebo_nav2/__init__.py @@ -0,0 +1 @@ +"""Gazebo Harmonic and ROS 2 navigation backend adapters.""" diff --git a/backend/src/roboprobe/backends/gazebo_nav2/materialize.py b/backend/src/roboprobe/backends/gazebo_nav2/materialize.py new file mode 100644 index 00000000..8452be2c --- /dev/null +++ b/backend/src/roboprobe/backends/gazebo_nav2/materialize.py @@ -0,0 +1,367 @@ +"""Create the deterministic Gazebo episode inputs (T-096a, FR-SIM-003, FR-SCN-005).""" + +from __future__ import annotations + +import importlib +import math +from collections.abc import Callable +from dataclasses import dataclass +from pathlib import Path + +import yaml +from pydantic import BaseModel +from pydantic import ValidationError as PydanticValidationError + +from roboprobe.backends.base import MaterializedScenario +from roboprobe.backends.gazebo_nav2.nav2_params import Nav2ParamOverlay, PatchedNav2Params +from roboprobe.backends.gazebo_nav2.sdf import PalletPlacement, SdfPatcher +from roboprobe.core.errors import NotFoundError, ValidationError +from roboprobe.core.seeding import PERCEPTION, SENSOR, SIMULATOR, derive_seed +from roboprobe.core.settings.backends import GazeboNav2Settings +from roboprobe.core.settings.oracles import InfraThresholds +from roboprobe.core.settings.worker import WorkerSettings +from roboprobe.core.types import Scenario, SutProfile, TaskSpec +from roboprobe.perturbation.perception import PerceptionModel +from roboprobe.scenario._documents import load_yaml_document, read_document +from roboprobe.scenario.materialize import place_obstacle, render_template, side_sign +from roboprobe.scenario.repo_root import discover_repo_root +from roboprobe.scenario.sut_profile import GazeboNav2ProfileConfig +from roboprobe.scenario.task import load_nominal_path, polyline_length_m, time_limit_s + +__all__ = ["EpisodeSeeds", "GazeboMaterializer", "MaterializedEpisode"] + +MaterializedEpisode = MaterializedScenario +_BACKEND = "gazebo_nav2" +_STOCK_PARAMS = GazeboNav2Settings().base_nav2_params +_MAP_YAML = "map.yaml" +_MAP_IMAGE = "map.pgm" +_PERCEPTION_FILE = "perception_model.yaml" +_WORLD_ROBOT = "turtlebot4" +_MIN_PACKAGE_PATH_PARTS = 2 +_GZ_TO_ROS = "GZ_TO_ROS" +_ROS_TO_GZ = "ROS_TO_GZ" +_STOCK_BRIDGES = ( + ("clock", "/clock", "rosgraph_msgs/msg/Clock", "gz.msgs.Clock", _GZ_TO_ROS), + ("joint_states", "joint_states", "sensor_msgs/msg/JointState", "gz.msgs.Model", _GZ_TO_ROS), + ("tf", "/tf", "tf2_msgs/msg/TFMessage", "gz.msgs.Pose_V", _GZ_TO_ROS), + ("imu", "/imu", "sensor_msgs/msg/Imu", "gz.msgs.IMU", _GZ_TO_ROS), +) +_BRIDGE_TEMPLATE = """{% for ros_name, gz_name, ros_type, gz_type, direction in entries -%} +- ros_topic_name: {{ ros_name | tojson }} + gz_topic_name: {{ gz_name | tojson }} + ros_type_name: {{ ros_type | tojson }} + gz_type_name: {{ gz_type | tojson }} + direction: {{ direction }} +{% endfor -%} +""" + + +@dataclass(frozen=True, slots=True) +class EpisodeSeeds: + """Root run seed for deterministic Gazebo and ROS node streams.""" + + run: int + + +class _Topic(BaseModel): + name: str + type: str + + +class _Topics(BaseModel): + stamped_cmd_vel: bool + topics: dict[str, _Topic] + frames: dict[str, str] + + def entry(self, name: str) -> _Topic: + """Return a required topic and its message type.""" + try: + return self.topics[name] + except KeyError as exc: + msg = f"Gazebo topics catalog is missing {name}" + raise ValidationError(msg) from exc + + +def _find_package(name: str) -> Path: + try: + index = importlib.import_module("ament_index_python.packages") + return Path(index.get_package_share_directory(name)) + except (ImportError, LookupError) as exc: + msg = f"cannot locate ROS package {name}; source the ROS environment first" + raise NotFoundError(msg, context={"package": name}) from exc + + +def _package_path(reference: str, resolver: Callable[[str], Path]) -> Path: + path = Path(reference) + if path.is_absolute(): + return path + if len(path.parts) < _MIN_PACKAGE_PATH_PARTS or ".." in path.parts: + msg = f"expected an absolute path or ROS package-relative file, got {reference}" + raise ValidationError(msg) + package, *relative = path.parts + share = resolver(package).resolve() + resolved = share.joinpath(*relative).resolve() + if not resolved.is_relative_to(share): + msg = f"ROS package asset escapes its package: {reference}" + raise ValidationError(msg) + return resolved + + +def _number(scenario: Scenario, key: str) -> float: + value = scenario.values.get(key) + if isinstance(value, bool) or not isinstance(value, int | float) or not math.isfinite(value): + msg = f"Gazebo scenario requires a finite number for {key}" + raise ValidationError(msg) + return float(value) + + +def _read_topics(raw: bytes, path: Path) -> _Topics: + try: + topics = _Topics.model_validate(load_yaml_document(raw, source=str(path))) + except PydanticValidationError as exc: + msg = f"{path}: invalid Gazebo topics catalog" + raise ValidationError(msg) from exc + for raw_name, processed_name in ( + ("scan_raw", "scan"), + ("depth_points_raw", "depth_points"), + ("cmd_vel", "cmd_vel_nav"), + ): + upstream, downstream = topics.entry(raw_name), topics.entry(processed_name) + if upstream.name == downstream.name or upstream.type != downstream.type: + msg = ( + f"Gazebo {raw_name} and {processed_name} require distinct names and matching types" + ) + raise ValidationError(msg) + twist = "TwistStamped" if topics.stamped_cmd_vel else "Twist" + if not topics.frames.get("map"): + msg = "Gazebo topics catalog requires frames.map" + raise ValidationError(msg) + for name, expected in ( + ("scan_raw", "sensor_msgs/msg/LaserScan"), + ("depth_points_raw", "sensor_msgs/msg/PointCloud2"), + ("cmd_vel", f"geometry_msgs/msg/{twist}"), + ("odom", "nav_msgs/msg/Odometry"), + ("initial_pose", "geometry_msgs/msg/PoseWithCovarianceStamped"), + ("ground_truth_pose", "geometry_msgs/msg/PoseStamped"), + ): + if topics.entry(name).type != expected: + msg = f"Gazebo {name} requires the ROS message type {expected}" + raise ValidationError(msg) + return topics + + +def _bridge(topics: _Topics, world: str) -> str: + bindings = ( + ("odom", "/odom", "gz.msgs.Odometry", _GZ_TO_ROS), + ("scan_raw", "/scan", "gz.msgs.LaserScan", _GZ_TO_ROS), + ("depth_points_raw", "/rgbd_camera/points", "gz.msgs.PointCloudPacked", _GZ_TO_ROS), + ("cmd_vel", "/cmd_vel", "gz.msgs.Twist", _ROS_TO_GZ), + ( + "ground_truth_pose", + f"/world/{world}/model/{_WORLD_ROBOT}/pose", + "gz.msgs.Pose", + _GZ_TO_ROS, + ), + ) + entries = list(_STOCK_BRIDGES) + for key, gz_topic, gz_type, direction in bindings: + topic = topics.entry(key) + entries.append((topic.name, gz_topic, topic.type, gz_type, direction)) + return render_template(_BRIDGE_TEMPLATE, {"entries": entries}) + + +def _map_assets(root: Path, reference: str) -> tuple[bytes, bytes]: + path = (root / reference).resolve() + if not path.is_file(): + path = (root / "tasks" / reference).resolve() + if not path.is_relative_to(root): + msg = "Gazebo task map must stay inside the repository" + raise ValidationError(msg) + map_config = load_yaml_document(read_document(path), source=str(path)) + if not isinstance(map_config, dict) or not isinstance(map_config.get("image"), str): + msg = f"{path}: Gazebo task map requires a PGM image" + raise ValidationError(msg) + image = (path.parent / map_config["image"]).resolve() + if not image.is_relative_to(path.parent): + msg = f"{path}: Gazebo map image must stay inside the map directory" + raise ValidationError(msg) + map_config["image"] = _MAP_IMAGE + map_image = read_document(image) + if not map_image.startswith(b"P5"): + msg = f"{path}: Gazebo task map requires a binary PGM image" + raise ValidationError(msg) + return yaml.safe_dump(map_config, sort_keys=True).encode(), map_image + + +class GazeboMaterializer: + """Materialize launch files so the ROS side only needs the episode workdir.""" + + def __init__( # noqa: PLR0913 - typed episode-adapter boundary plus test/package overrides + self, + settings: GazeboNav2Settings, + sut: SutProfile, + task: TaskSpec, + *, + resolve_package: Callable[[str], Path] = _find_package, + repo_root: Path | None = None, + wall_timeout_s: float | None = None, + spawn_tolerance_m: float | None = None, + ) -> None: + """Validate configuration and retain host asset location for deterministic generation.""" + if sut.backend != _BACKEND: + msg = "Gazebo materializer requires a gazebo_nav2 SUT profile" + raise ValidationError(msg) + if task.provisional: + msg = "Gazebo materializer requires a verified task with no provisional values" + raise ValidationError(msg) + try: + self._profile = GazeboNav2ProfileConfig.model_validate(sut.config) + except PydanticValidationError as exc: + msg = "Gazebo materializer requires valid Nav2 SUT parameters" + raise ValidationError(msg) from exc + self._settings = settings + self._task = task + self._root = (repo_root or discover_repo_root()).resolve() + self._resolve_package = resolve_package + self._wall_timeout_s = ( + WorkerSettings().default_wall_timeout_s if wall_timeout_s is None else wall_timeout_s + ) + self._spawn_tolerance_m = ( + InfraThresholds().spawn_tolerance_m if spawn_tolerance_m is None else spawn_tolerance_m + ) + + def materialize( + self, scenario: Scenario, workdir: Path, seeds: EpisodeSeeds + ) -> MaterializedEpisode: + """Resolve scenario mechanisms, then write deterministic launch files.""" + settings = self._settings + task_root = self._root / "tasks" + task_file = (task_root / f"{self._task.id}.yaml").resolve() + if task_file.parent != task_root: + msg = "Gazebo task id must resolve inside the task directory" + raise ValidationError(msg) + nominal = load_nominal_path(task_file, self._task) + pallets = tuple( + PalletPlacement( + obstacle.model, + place_obstacle( + nominal, + _number(scenario, obstacle.placement.along), + _number(scenario, obstacle.placement.lateral), + side_sign(obstacle.placement.side), + ), + obstacle.size[2], + ) + for obstacle in self._task.injected_obstacles + ) + model_path = self._root / settings.perception_model_file + model = PerceptionModel.from_yaml(model_path) + illuminance = _number(scenario, "env.illuminance") + speed = _number(scenario, "robot.max_speed") + world_path = _package_path(settings.base_world, self._resolve_package) + world = SdfPatcher(world_path).patch( + lighting_scale=min(1.0, illuminance / model.l_ref_lux), + floor_friction=_number(scenario, "env.floor_friction"), + real_time_factor=settings.physics.real_time_factor, + pallets=pallets, + ) + topics_path = self._root / settings.topics_file + topics_raw = read_document(topics_path) + topics = _read_topics(topics_raw, topics_path) + params = self._nav2(topics, speed) + map_yaml, map_image = _map_assets(self._root, self._task.map) + time_limit = time_limit_s(self._task, speed, polyline_length_m(nominal)) + simulator_seed = derive_seed(seeds.run, SIMULATOR) + files = { + "world.sdf": world.content, + "nav2_params.yaml": params.content.encode(), + "perturb_params.yaml": self._perturb( + scenario, seeds, illuminance, params.controller_frequency_hz + ).encode(), + "bridge.yaml": _bridge(topics, world.name).encode(), + "episode.yaml": self._episode( + pallets, params, time_limit, simulator_seed, world.name + ).encode(), + "topics.yaml": topics_raw, + _MAP_YAML: map_yaml, + _MAP_IMAGE: map_image, + _PERCEPTION_FILE: read_document(model_path), + } + workdir.mkdir(parents=True, exist_ok=True) + written: dict[str, Path] = {} + for filename, content in files.items(): + output = workdir / filename + output.write_bytes(content) + written[filename] = output + return MaterializedEpisode( + files=written, payload={"time_limit_s": time_limit, "simulator_seed": simulator_seed} + ) + + def _nav2(self, topics: _Topics, speed: float) -> PatchedNav2Params: + reference = self._profile.base_nav2_params + if reference == _STOCK_PARAMS: + reference = self._settings.base_nav2_params + return Nav2ParamOverlay( + _package_path(reference, self._resolve_package), self._profile + ).patch( + speed_mps=speed, + depth_topic=topics.entry("depth_points").name, + command_output_topic=topics.entry("cmd_vel_nav").name, + stamped_cmd_vel=topics.stamped_cmd_vel, + ) + + def _perturb( + self, scenario: Scenario, seeds: EpisodeSeeds, illuminance: float, rate: float + ) -> str: + sensor_seed = derive_seed(seeds.run, SENSOR) + return yaml.safe_dump( + { + "illuminance_lux": illuminance, + "perception_model_file": _PERCEPTION_FILE, + "lidar": { + "sigma_m": _number(scenario, "sensor.lidar_noise_std"), + "dropout_p": _number(scenario, "sensor.lidar_dropout"), + }, + "cmd_delay_ms": _number(scenario, "comms.cmd_delay_ms"), + "rate_hz": rate, + "seed": sensor_seed, + "seeds": { + "sensor_perturber": sensor_seed, + "perception_degrader": derive_seed(seeds.run, PERCEPTION), + "cmd_delay": derive_seed(seeds.run, SIMULATOR, "cmd_delay"), + }, + }, + sort_keys=True, + ) + + def _episode( + self, + pallets: tuple[PalletPlacement, ...], + params: PatchedNav2Params, + time_limit: float, + simulator_seed: int, + world_name: str, + ) -> str: + return yaml.safe_dump( + { + "start": self._task.start.model_dump(mode="json"), + "goal": self._task.goal.model_dump(mode="json"), + "time_limit_s": time_limit, + "sample_hz": self._settings.sample_hz, + "record_bag": self._settings.record_bag, + "startup_timeout_s": self._settings.startup_timeout_s, + "teardown_grace_s": self._settings.teardown_grace_s, + "spawn_tolerance_m": self._spawn_tolerance_m, + "wall_timeout_factor": self._wall_timeout_s / time_limit, + "map_file": _MAP_YAML, + "world_name": world_name, + "robot_name": _WORLD_ROBOT, + "footprint": params.footprint, + "simulator_seed": simulator_seed, + "pallets": [ + {"pose": pallet.pose.model_dump(mode="json"), "size": list(obstacle.size)} + for pallet, obstacle in zip(pallets, self._task.injected_obstacles, strict=True) + ], + }, + sort_keys=True, + ) diff --git a/backend/src/roboprobe/backends/gazebo_nav2/nav2_params.py b/backend/src/roboprobe/backends/gazebo_nav2/nav2_params.py new file mode 100644 index 00000000..d4d39d1d --- /dev/null +++ b/backend/src/roboprobe/backends/gazebo_nav2/nav2_params.py @@ -0,0 +1,196 @@ +"""Overlay a verified base Nav2 configuration (T-096a, FR-SIM-003).""" + +from __future__ import annotations + +import json +import math +import re +from copy import deepcopy +from dataclasses import dataclass +from pathlib import Path +from typing import cast + +import numpy as np +import yaml +from pydantic import TypeAdapter +from pydantic import ValidationError as PydanticValidationError + +from roboprobe.core.errors import ValidationError +from roboprobe.core.types import JsonValue +from roboprobe.geometry import PolygonFootprint +from roboprobe.scenario._documents import load_yaml_document, read_document +from roboprobe.scenario.sut_profile import GazeboNav2ProfileConfig + +__all__ = ["Nav2ParamOverlay", "PatchedNav2Params"] + +_SEGMENT = re.compile(r"([^\[\].]+)(?:\[([0-9]+)\])?") +_PARAMETERS = "ros__parameters" +_DOCUMENT = TypeAdapter(dict[str, JsonValue]) + + +@dataclass(frozen=True, slots=True) +class PatchedNav2Params: + """Generated parameter YAML and actual controller rate for the delay node.""" + + content: str + controller_frequency_hz: float + footprint: dict[str, JsonValue] + + +def _merge(destination: dict[str, JsonValue], overlay: dict[str, JsonValue]) -> None: + for key, value in overlay.items(): + existing = destination.get(key) + if isinstance(existing, dict) and isinstance(value, dict): + _merge(existing, value) + else: + destination[key] = deepcopy(value) + + +def _segments(path: str) -> list[str | int]: + parts: list[str | int] = [] + for component in path.split("."): + matched = _SEGMENT.fullmatch(component) + if matched is None: + msg = f"invalid Nav2 speed parameter path: {path}" + raise ValidationError(msg) + parts.append(matched[1]) + if matched[2] is not None: + parts.append(int(matched[2])) + return parts + + +def _set_speed(params: dict[str, JsonValue], path: str, speed: float) -> None: + *parents, leaf = _segments(path) + branch: JsonValue = params + for part in parents: + match branch, part: + case dict(), str(): + branch = branch.get(part) + case list(), int() if part < len(branch): + branch = branch[part] + case _: + branch = None + break + match branch, leaf: + case dict(), str() if type(branch.get(leaf)) in (float, int): + branch[leaf] = speed + case list(), int() if leaf < len(branch) and type(branch[leaf]) in (float, int): + branch[leaf] = speed + case _: + msg = f"Nav2 speed parameter does not point to a numeric value: {path}" + raise ValidationError(msg) + + +def _map_at(params: dict[str, JsonValue], *keys: str) -> dict[str, JsonValue]: + branch = params + for key in keys: + value = branch.get(key) + if not isinstance(value, dict): + msg = f"Nav2 parameters require a mapping at {'.'.join(keys)}" + raise ValidationError(msg) + branch = value + return branch + + +def _add_depth(params: dict[str, JsonValue], topic: str) -> None: + costmap = _map_at(params, "local_costmap", "local_costmap", _PARAMETERS) + voxel = _map_at(costmap, "voxel_layer") + plugins = costmap.get("plugins") + sources = voxel.get("observation_sources") + if ( + not isinstance(plugins, list) + or "voxel_layer" not in plugins + or not isinstance(sources, str) + ): + msg = "Nav2 local costmap must configure voxel_layer with observation_sources" + raise ValidationError(msg) + voxel["enabled"] = True + voxel["observation_sources"] = " ".join(dict.fromkeys([*sources.split(), "depth"])) + _merge( + voxel, + { + "depth": { + "data_type": "PointCloud2", + "topic": topic, + "marking": True, + "clearing": False, + } + }, + ) + + +def _positive_number(value: JsonValue, name: str) -> float: + if ( + isinstance(value, bool) + or not isinstance(value, int | float) + or not (math.isfinite(value) and value > 0) + ): + msg = f"Nav2 {name} must be a finite positive value" + raise ValidationError(msg) + return float(value) + + +def _footprint(params: dict[str, JsonValue]) -> dict[str, JsonValue]: + costmap = _map_at(params, "local_costmap", "local_costmap", _PARAMETERS) + footprint = costmap.get("footprint") + if footprint and footprint != "[]": + try: + points = json.loads(footprint) if isinstance(footprint, str) else footprint + polygon = PolygonFootprint(np.asarray(points, dtype=np.float64)) + except (ValueError, TypeError) as exc: + msg = "Nav2 footprint must contain a valid robot polygon" + raise ValidationError(msg) from exc + return {"vertices_m": cast("list[JsonValue]", polygon.vertices.tolist())} + return {"radius_m": _positive_number(costmap.get("robot_radius"), "robot_radius")} + + +def _route_velocity(params: dict[str, JsonValue], topic: str, stamped: bool) -> None: + for node in ( + "controller_server", + "behavior_server", + "velocity_smoother", + "collision_monitor", + "docking_server", + ): + if node in params: + _map_at(params, node, _PARAMETERS)["enable_stamped_cmd_vel"] = stamped + _map_at(params, "collision_monitor", _PARAMETERS)["cmd_vel_out_topic"] = topic + + +class Nav2ParamOverlay: + """Deep-merge the profile, apply speed limits, and route the processed depth cloud.""" + + def __init__(self, source: Path, profile: GazeboNav2ProfileConfig) -> None: + """Store the base path and validated overlays.""" + self._source = source + self._profile = profile + + def patch( + self, + *, + speed_mps: float, + depth_topic: str, + command_output_topic: str | None = None, + stamped_cmd_vel: bool = False, + ) -> PatchedNav2Params: + """Generate sorted YAML and fail if the selected profile disagrees with the base.""" + document = load_yaml_document(read_document(self._source), source=str(self._source)) + try: + params = _DOCUMENT.validate_python(document) + except PydanticValidationError as exc: + msg = f"{self._source}: invalid Nav2 parameter mapping" + raise ValidationError(msg) from exc + _merge(params, self._profile.nav2_overrides) + for path in self._profile.speed_param_paths: + _set_speed(params, path, speed_mps) + _add_depth(params, depth_topic) + if command_output_topic is not None: + _route_velocity(params, command_output_topic, stamped_cmd_vel) + controller = _map_at(params, "controller_server", _PARAMETERS) + return PatchedNav2Params( + content=yaml.safe_dump(params, sort_keys=True), + controller_frequency_hz=_positive_number( + controller.get("controller_frequency"), "controller_frequency" + ), + footprint=_footprint(params), + ) diff --git a/backend/src/roboprobe/backends/gazebo_nav2/sdf.py b/backend/src/roboprobe/backends/gazebo_nav2/sdf.py new file mode 100644 index 00000000..fe22a6c5 --- /dev/null +++ b/backend/src/roboprobe/backends/gazebo_nav2/sdf.py @@ -0,0 +1,128 @@ +"""Deterministic SDF lighting, friction, and pallet patches (T-096a, FR-SIM-003).""" + +from __future__ import annotations + +import math +import re +from collections.abc import Sequence +from dataclasses import dataclass +from pathlib import Path +from typing import cast + +from lxml import etree # type: ignore[import-untyped] + +from roboprobe.core.errors import ValidationError +from roboprobe.core.types import Pose2D +from roboprobe.scenario._documents import read_document + +__all__ = ["PalletPlacement", "PatchedWorld", "SdfPatcher"] + +_COLOR_COMPONENTS = 4 +_MODEL_NAME = re.compile(r"[a-zA-Z0-9_][a-zA-Z0-9_.-]*") +_GROUND_COLLISION = "./model[@name='ground_plane']/link/collision[geometry/plane]" +_LIGHTING = "./scene/ambient | ./light[@type='directional' or @type='point']/diffuse" + + +@dataclass(frozen=True, slots=True) +class PalletPlacement: + """One resolved model name, planar pose and box height.""" + + model: str + pose: Pose2D + height_m: float + + +@dataclass(frozen=True, slots=True) +class PatchedWorld: + """Generated world bytes and the world name used for the ROS bridge.""" + + content: bytes + name: str + + +def _number(value: float) -> str: + return f"{value:.15g}" + + +def _set_text(parent: etree._Element, name: str, text: str) -> None: + child = _child(parent, name) + child.text = text + + +def _child(parent: etree._Element, name: str) -> etree._Element: + child = parent.find(name) + return child if child is not None else etree.SubElement(parent, name) + + +def _scale_color(node: etree._Element, scale: float) -> None: + msg = f"SDF {node.tag} must contain four finite RGBA components" + try: + values = [float(part) for part in (node.text or "").split()] + except ValueError as exc: + raise ValidationError(msg) from exc + if len(values) != _COLOR_COMPONENTS or not all(map(math.isfinite, values)): + raise ValidationError(msg) + red, green, blue, alpha = values + node.text = " ".join(map(_number, (red * scale, green * scale, blue * scale, alpha))) + + +def _append_pallet(world: etree._Element, pallet: PalletPlacement, index: int) -> None: + if not _MODEL_NAME.fullmatch(pallet.model): + msg = "SDF pallet model must be a local model name" + raise ValidationError(msg, context={"model": pallet.model}) + include = etree.SubElement(world, "include") + _set_text(include, "uri", f"model://{pallet.model}") + _set_text(include, "name", f"{pallet.model}_{index}") + x, y, yaw = pallet.pose.x, pallet.pose.y, pallet.pose.yaw + _set_text(include, "pose", " ".join(map(_number, (x, y, pallet.height_m / 2, 0, 0, yaw)))) + + +class SdfPatcher: + """Patch an operator-selected world without expanding XML entities or loading DTDs.""" + + def __init__(self, source: Path) -> None: + """Store the path to the unmodified source SDF.""" + self._source = source + + def patch( + self, + *, + lighting_scale: float, + floor_friction: float, + real_time_factor: float, + pallets: Sequence[PalletPlacement], + ) -> PatchedWorld: + """Generate a world from the same base on every call.""" + raw = read_document(self._source) + # DTD loading, entity expansion, and parser network access are disabled before parse. + parser = etree.XMLParser(resolve_entities=False, no_network=True, load_dtd=False) + try: + root = etree.fromstring(raw, parser=parser) + except etree.XMLSyntaxError as exc: + msg = f"{self._source}: invalid SDF" + raise ValidationError(msg) from exc + if root.getroottree().docinfo.doctype: + msg = f"{self._source}: SDF DOCTYPE declarations are forbidden" + raise ValidationError(msg) + worlds = root.findall("./world") if root.tag == "sdf" else [] + if len(worlds) != 1 or not worlds[0].get("name"): + msg = f"{self._source}: SDF must contain exactly one named world" + raise ValidationError(msg) + world = worlds[0] + ground = world.xpath(_GROUND_COLLISION) + if not ground: + msg = f"{self._source}: SDF has no ground plane collision" + raise ValidationError(msg) + for collision in ground: + friction = _child(_child(_child(collision, "surface"), "friction"), "ode") + _set_text(friction, "mu", _number(floor_friction)) + _set_text(friction, "mu2", _number(floor_friction)) + for color in world.xpath(_LIGHTING): + _scale_color(color, lighting_scale) + _set_text(_child(world, "physics"), "real_time_factor", _number(real_time_factor)) + for index, pallet in enumerate(pallets): + _append_pallet(world, pallet, index) + return PatchedWorld( + content=cast("bytes", etree.tostring(root, encoding="UTF-8", xml_declaration=True)), + name=cast("str", world.get("name")), + ) diff --git a/backend/tests/fixtures/gazebo_materialize/bridge.yaml b/backend/tests/fixtures/gazebo_materialize/bridge.yaml new file mode 100644 index 00000000..cb3c1cc4 --- /dev/null +++ b/backend/tests/fixtures/gazebo_materialize/bridge.yaml @@ -0,0 +1,45 @@ +- ros_topic_name: "clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS +- ros_topic_name: "tf" + gz_topic_name: "/tf" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS +- ros_topic_name: "imu" + gz_topic_name: "/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + direction: GZ_TO_ROS +- ros_topic_name: "/odom" + gz_topic_name: "/odom" + ros_type_name: "nav_msgs/msg/Odometry" + gz_type_name: "gz.msgs.Odometry" + direction: GZ_TO_ROS +- ros_topic_name: "/scan_raw" + gz_topic_name: "/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS +- ros_topic_name: "/rgbd_camera/points_raw" + gz_topic_name: "/rgbd_camera/points" + ros_type_name: "sensor_msgs/msg/PointCloud2" + gz_type_name: "gz.msgs.PointCloudPacked" + direction: GZ_TO_ROS +- ros_topic_name: "/cmd_vel" + gz_topic_name: "/cmd_vel" + ros_type_name: "geometry_msgs/msg/Twist" + gz_type_name: "gz.msgs.Twist" + direction: ROS_TO_GZ +- ros_topic_name: "/ground_truth_pose" + gz_topic_name: "/world/warehouse/model/turtlebot4/pose" + ros_type_name: "geometry_msgs/msg/PoseStamped" + gz_type_name: "gz.msgs.Pose" + direction: GZ_TO_ROS diff --git a/backend/tests/fixtures/gazebo_materialize/episode.yaml b/backend/tests/fixtures/gazebo_materialize/episode.yaml new file mode 100644 index 00000000..560a6562 --- /dev/null +++ b/backend/tests/fixtures/gazebo_materialize/episode.yaml @@ -0,0 +1,30 @@ +footprint: + radius_m: 0.22 +goal: + x: 10.0 + y: 0.0 + yaw: 0.0 +map_file: map.yaml +pallets: +- pose: + x: 5.0 + y: 0.5 + yaw: 0.0 + size: + - 0.8 + - 0.6 + - 0.135 +record_bag: always +robot_name: turtlebot4 +sample_hz: 15.0 +simulator_seed: 2047656344537034507 +spawn_tolerance_m: 0.25 +start: + x: 0.0 + y: 0.0 + yaw: 0.0 +startup_timeout_s: 60.0 +teardown_grace_s: 10.0 +time_limit_s: 55.0 +wall_timeout_factor: 4.0 +world_name: warehouse diff --git a/backend/tests/fixtures/gazebo_materialize/nav2_params.yaml b/backend/tests/fixtures/gazebo_materialize/nav2_params.yaml new file mode 100644 index 00000000..114b8d73 --- /dev/null +++ b/backend/tests/fixtures/gazebo_materialize/nav2_params.yaml @@ -0,0 +1,34 @@ +collision_monitor: + ros__parameters: + cmd_vel_out_topic: /roboprobe/cmd_vel_nav + enable_stamped_cmd_vel: false +controller_server: + ros__parameters: + FollowPath: + vx_max: 0.4 + controller_frequency: 20.0 + enable_stamped_cmd_vel: false +local_costmap: + local_costmap: + ros__parameters: + plugins: + - voxel_layer + - inflation_layer + robot_radius: 0.22 + voxel_layer: + depth: + clearing: false + data_type: PointCloud2 + marking: true + topic: /rgbd_camera/points + enabled: true + observation_sources: scan depth + scan: + topic: /scan +velocity_smoother: + ros__parameters: + enable_stamped_cmd_vel: false + max_velocity: + - 0.4 + - 0.0 + - 2.0 diff --git a/backend/tests/fixtures/gazebo_materialize/perturb_params.yaml b/backend/tests/fixtures/gazebo_materialize/perturb_params.yaml new file mode 100644 index 00000000..a9935674 --- /dev/null +++ b/backend/tests/fixtures/gazebo_materialize/perturb_params.yaml @@ -0,0 +1,12 @@ +cmd_delay_ms: 50.0 +illuminance_lux: 150.0 +lidar: + dropout_p: 0.04 + sigma_m: 0.01 +perception_model_file: perception_model.yaml +rate_hz: 20.0 +seed: 5957242366569722094 +seeds: + cmd_delay: 3889710175154448791 + perception_degrader: 610100211397079758 + sensor_perturber: 5957242366569722094 diff --git a/backend/tests/fixtures/gazebo_materialize/world.sdf b/backend/tests/fixtures/gazebo_materialize/world.sdf new file mode 100644 index 00000000..ab04b89f --- /dev/null +++ b/backend/tests/fixtures/gazebo_materialize/world.sdf @@ -0,0 +1,14 @@ + + + + 1 + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + 0.5 0.5 0.5 0.8 + + + 0.50.5 + + + model://dropped_palletdropped_pallet_05 0.5 0.0675 0 0 0 + \ No newline at end of file diff --git a/backend/tests/unit/backends/gazebo/test_materialize.py b/backend/tests/unit/backends/gazebo/test_materialize.py new file mode 100644 index 00000000..2d972d7d --- /dev/null +++ b/backend/tests/unit/backends/gazebo/test_materialize.py @@ -0,0 +1,411 @@ +"""Gazebo launch-file generation and input validation (T-096a).""" + +from pathlib import Path +from xml.etree import ElementTree + +import pytest +import yaml + +from roboprobe.backends.gazebo_nav2.materialize import EpisodeSeeds, GazeboMaterializer +from roboprobe.backends.gazebo_nav2.nav2_params import Nav2ParamOverlay +from roboprobe.backends.gazebo_nav2.sdf import PalletPlacement, SdfPatcher +from roboprobe.core.errors import ValidationError +from roboprobe.core.settings.backends import GazeboNav2Settings +from roboprobe.core.types import Pose2D, Scenario, TaskSpec +from roboprobe.scenario.sut_profile import GazeboNav2ProfileConfig, load_sut_profile + +pytestmark = pytest.mark.unit + +BASE_WORLD = """ + + 1 + 0.8 0.6 0.4 1 + 1 0.8 0.6 1 + 1 1 1 0.8 + + + + + + + +""" + +BASE_PARAMS = { + "controller_server": { + "ros__parameters": {"controller_frequency": 20.0, "FollowPath": {"vx_max": 0.5}} + }, + "velocity_smoother": {"ros__parameters": {"max_velocity": [0.5, 0.0, 2.0]}}, + "local_costmap": { + "local_costmap": { + "ros__parameters": { + "robot_radius": 0.22, + "plugins": ["voxel_layer", "inflation_layer"], + "voxel_layer": { + "enabled": False, + "observation_sources": "scan", + "scan": {"topic": "/scan"}, + }, + } + } + }, + "collision_monitor": {"ros__parameters": {"cmd_vel_out_topic": "/cmd_vel"}}, +} +SPEED_PATHS = ( + "controller_server.ros__parameters.FollowPath.vx_max", + "velocity_smoother.ros__parameters.max_velocity[0]", +) +ROOT = Path(__file__).resolve().parents[5] +FIXTURES = ROOT / "backend/tests/fixtures/gazebo_materialize" +VALUES = { + "robot.max_speed": 0.4, + "obstacle.clearance": 0.5, + "env.illuminance": 150.0, + "sensor.lidar_noise_std": 0.01, + "env.floor_friction": 0.5, + "comms.cmd_delay_ms": 50.0, + "obstacle.along_path": 0.5, + "sensor.lidar_dropout": 0.04, +} + + +@pytest.fixture +def source_tree(tmp_path: Path) -> Path: + root = tmp_path / "inputs" + for folder in ("tasks/paths", "map", "nav2", "sim"): + (root / folder).mkdir(parents=True) + (root / "map/model.pgm").write_bytes(b"P5\n2 2\n255\n" + bytes([0, 255, 255, 0])) + (root / "map/model.yaml").write_text("image: model.pgm\nresolution: 0.05\n", encoding="utf-8") + (root / "tasks/paths/nominal.csv").write_text("x,y\n0,0\n10,0\n", encoding="utf-8") + (root / "sim/world.sdf").write_text(BASE_WORLD, encoding="utf-8") + (root / "nav2/params.yaml").write_text(yaml.safe_dump(BASE_PARAMS), encoding="utf-8") + topics = { + "scan_raw": ("/scan_raw", "sensor_msgs/msg/LaserScan"), + "scan": ("/scan", "sensor_msgs/msg/LaserScan"), + "depth_points_raw": ("/rgbd_camera/points_raw", "sensor_msgs/msg/PointCloud2"), + "depth_points": ("/rgbd_camera/points", "sensor_msgs/msg/PointCloud2"), + "odom": ("/odom", "nav_msgs/msg/Odometry"), + "initial_pose": ("/initialpose", "geometry_msgs/msg/PoseWithCovarianceStamped"), + "cmd_vel": ("/cmd_vel", "geometry_msgs/msg/Twist"), + "cmd_vel_nav": ("/roboprobe/cmd_vel_nav", "geometry_msgs/msg/Twist"), + "ground_truth_pose": ("/ground_truth_pose", "geometry_msgs/msg/PoseStamped"), + } + catalog = { + "stamped_cmd_vel": False, + "frames": {"map": "map"}, + "topics": {key: {"name": name, "type": kind} for key, (name, kind) in topics.items()}, + } + (root / "topics.yaml").write_text(yaml.safe_dump(catalog), encoding="utf-8") + return root + + +@pytest.fixture +def materializer(source_tree: Path) -> GazeboMaterializer: + task = TaskSpec.model_validate( + { + "id": "warehouse", + "schema_version": 1, + "world": {"gazebo": "sim/world.sdf"}, + "map": "map/model.yaml", + "start": {"x": 0, "y": 0, "yaw": 0}, + "goal": {"x": 10, "y": 0, "yaw": 0}, + "goal_tolerance": {"xy": 0.25, "yaw": 0.5}, + "nominal_path": "paths/nominal.csv", + "time_limit": {"factor": 2, "cap_s": 100, "offset_s": 5}, + "injected_obstacles": [ + { + "model": "dropped_pallet", + "size": [0.8, 0.6, 0.135], + "placement": { + "along": "obstacle.along_path", + "lateral": "obstacle.clearance", + "side": "left", + }, + }, + ], + } + ) + sut = load_sut_profile(ROOT / "configs/sut_profiles/tb4_nav2_default.yaml", strict=True) + return GazeboMaterializer( + GazeboNav2Settings( + base_world="sim/world.sdf", + base_nav2_params="nav2/params.yaml", + topics_file=Path("topics.yaml"), + perception_model_file=ROOT / "configs/perception/oakd_lowlight_v1.yaml", + sample_hz=15, + record_bag="always", + ), + sut, + task, + repo_root=source_tree, + resolve_package=lambda package: source_tree / package, + wall_timeout_s=220, + spawn_tolerance_m=0.25, + ) + + +def _scenario(**overrides: float) -> Scenario: + return Scenario( + space_id="warehouse", + space_version="0123456789ab", + scenario_hash="0123456789abcdef", + values={**VALUES, **overrides}, + ) + + +def test_materializes_golden_launch_files( + materializer: GazeboMaterializer, source_tree: Path, tmp_path: Path +) -> None: + episode = materializer.materialize(_scenario(), tmp_path / "first", EpisodeSeeds(run=51)) + repeated = materializer.materialize(_scenario(), tmp_path / "again", EpisodeSeeds(run=51)) + for name in ( + "world.sdf", + "episode.yaml", + "perturb_params.yaml", + "nav2_params.yaml", + "bridge.yaml", + ): + assert episode.files[name].read_bytes() == (FIXTURES / name).read_bytes() + assert repeated.files[name].read_bytes() == episode.files[name].read_bytes() + assert episode.files["map.pgm"].read_bytes() == (source_tree / "map/model.pgm").read_bytes() + assert ( + yaml.safe_load(episode.files["map.yaml"].read_text(encoding="utf-8"))["image"] == "map.pgm" + ) + assert episode.payload["time_limit_s"] == 55.0 + + +def _changed_yaml_keys(left: object, right: object, prefix: str) -> set[str]: + if isinstance(left, dict) and isinstance(right, dict): + return { + changed + for key in left.keys() | right.keys() + for changed in _changed_yaml_keys(left.get(key), right.get(key), f"{prefix}.{key}") + } + return set() if left == right else {prefix} + + +def _changed_fields(name: str, first: Path, second: Path) -> set[str]: + if first.read_bytes() == second.read_bytes(): + return set() + if name == "world.sdf": + left = ElementTree.parse(first) # noqa: S314 - generated by the restricted SDF parser + right = ElementTree.parse(second) # noqa: S314 - generated by the restricted SDF parser + pairs = zip(left.iter(), right.iter(), strict=True) + return { + f"{name}.{before.tag}" + for before, after in pairs + if before.text != after.text or before.attrib != after.attrib + } + assert name.endswith(".yaml"), name + return _changed_yaml_keys( + yaml.safe_load(first.read_bytes()), yaml.safe_load(second.read_bytes()), name + ) + + +@pytest.mark.parametrize( + ("parameter", "value", "expected"), + [ + ( + "robot.max_speed", + 0.5, + { + "nav2_params.yaml.controller_server.ros__parameters.FollowPath.vx_max", + "nav2_params.yaml.velocity_smoother.ros__parameters.max_velocity", + "episode.yaml.time_limit_s", + "episode.yaml.wall_timeout_factor", + }, + ), + ("obstacle.clearance", 0.7, {"world.sdf.pose", "episode.yaml.pallets"}), + ( + "env.illuminance", + 350.0, + {"world.sdf.ambient", "world.sdf.diffuse", "perturb_params.yaml.illuminance_lux"}, + ), + ("sensor.lidar_noise_std", 0.02, {"perturb_params.yaml.lidar.sigma_m"}), + ("env.floor_friction", 0.8, {"world.sdf.mu", "world.sdf.mu2"}), + ("comms.cmd_delay_ms", 90.0, {"perturb_params.yaml.cmd_delay_ms"}), + ("obstacle.along_path", 0.6, {"world.sdf.pose", "episode.yaml.pallets"}), + ("sensor.lidar_dropout", 0.08, {"perturb_params.yaml.lidar.dropout_p"}), + ], +) +def test_parameter_changes_are_isolated( + materializer: GazeboMaterializer, + tmp_path: Path, + parameter: str, + value: float, + expected: set[str], +) -> None: + before = materializer.materialize(_scenario(), tmp_path / "before", EpisodeSeeds(run=51)) + after = materializer.materialize( + _scenario(**{parameter: value}), tmp_path / "after", EpisodeSeeds(run=51) + ) + assert { + field + for name, path in before.files.items() + for field in _changed_fields(name, path, after.files[name]) + } == expected + + +@pytest.mark.parametrize( + "case", + [ + ("topics.yaml", "topics: {}", "invalid Gazebo topics catalog"), + ( + "topics.yaml", + "stamped_cmd_vel: false\nframes: {map: map}\ntopics: {}", + "missing scan_raw", + ), + ("map/model.yaml", "image: ../elsewhere.pgm", "stay inside the map directory"), + ("map/model.yaml", "resolution: 0.05", "requires a PGM image"), + ("map/model.pgm", "invalid image", "requires a binary PGM image"), + ], +) +def test_invalid_inputs_do_not_create_launch_files( + materializer: GazeboMaterializer, + source_tree: Path, + tmp_path: Path, + case: tuple[str, str, str], +) -> None: + filename, contents, problem = case + (source_tree / filename).write_text(contents, encoding="utf-8") + output = tmp_path / "failed" + with pytest.raises(ValidationError, match=problem): + materializer.materialize(_scenario(), output, EpisodeSeeds(run=51)) + assert not output.exists() + + +def test_velocity_bridge_supports_consistent_stamped_mode( + materializer: GazeboMaterializer, source_tree: Path, tmp_path: Path +) -> None: + catalog_file = source_tree / "topics.yaml" + catalog = yaml.safe_load(catalog_file.read_bytes()) + catalog["stamped_cmd_vel"] = True + for role in ("cmd_vel", "cmd_vel_nav"): + catalog["topics"][role]["type"] = "geometry_msgs/msg/TwistStamped" + catalog_file.write_bytes(yaml.safe_dump(catalog).encode()) + episode = materializer.materialize(_scenario(), tmp_path / "stamped", EpisodeSeeds(run=51)) + bridge = yaml.safe_load(episode.files["bridge.yaml"].read_bytes()) + command = next(route for route in bridge if route["direction"] == "ROS_TO_GZ") + assert command["ros_type_name"] == "geometry_msgs/msg/TwistStamped" + nav2 = yaml.safe_load(episode.files["nav2_params.yaml"].read_bytes()) + collision = nav2["collision_monitor"]["ros__parameters"] + assert collision["enable_stamped_cmd_vel"] is True + assert collision["cmd_vel_out_topic"] == "/roboprobe/cmd_vel_nav" + + +def test_depth_topic_must_use_verified_cloud_type( + materializer: GazeboMaterializer, source_tree: Path, tmp_path: Path +) -> None: + catalog_file = source_tree / "topics.yaml" + catalog = yaml.safe_load(catalog_file.read_bytes()) + for role in ("depth_points", "depth_points_raw"): + catalog["topics"][role]["type"] = "sensor_msgs/msg/Image" + catalog_file.write_bytes(yaml.safe_dump(catalog).encode()) + with pytest.raises(ValidationError, match=r"requires the ROS message type .*PointCloud2"): + materializer.materialize(_scenario(), tmp_path / "invalid", EpisodeSeeds(run=51)) + + +def test_nav2_overlays_speed_values_and_keeps_scan(tmp_path: Path) -> None: + source = tmp_path / "nav2.yaml" + source.write_text(yaml.safe_dump(BASE_PARAMS), encoding="utf-8") + profile = GazeboNav2ProfileConfig( + base_nav2_params="nav2/params.yaml", + speed_param_paths=SPEED_PATHS, + nav2_overrides={"controller_server": {"ros__parameters": {"controller_frequency": 25.0}}}, + ) + overlay = Nav2ParamOverlay(source, profile).patch(speed_mps=0.3, depth_topic="/processed/depth") + assert overlay.controller_frequency_hz == 25.0 + params = yaml.safe_load(overlay.content) + assert params["controller_server"]["ros__parameters"] == { + "FollowPath": {"vx_max": 0.3}, + "controller_frequency": 25.0, + } + assert params["velocity_smoother"]["ros__parameters"]["max_velocity"] == [0.3, 0.0, 2.0] + assert params["local_costmap"]["local_costmap"]["ros__parameters"]["voxel_layer"] == { + "enabled": True, + "observation_sources": "scan depth", + "scan": {"topic": "/scan"}, + "depth": { + "data_type": "PointCloud2", + "topic": "/processed/depth", + "marking": True, + "clearing": False, + }, + } + assert yaml.safe_load(source.read_text(encoding="utf-8")) == BASE_PARAMS + + +def test_nav2_reads_a_profile_polygon_for_ground_truth(tmp_path: Path) -> None: + source = tmp_path / "nav2.yaml" + source.write_text(yaml.safe_dump(BASE_PARAMS), encoding="utf-8") + vertices = [[0.1, 0.2], [0.1, -0.2], [-0.1, 0.0]] + profile = GazeboNav2ProfileConfig( + base_nav2_params="nav2/params.yaml", + speed_param_paths=SPEED_PATHS, + nav2_overrides={ + "local_costmap": {"local_costmap": {"ros__parameters": {"footprint": str(vertices)}}} + }, + ) + overlay = Nav2ParamOverlay(source, profile).patch(speed_mps=0.3, depth_topic="/processed/depth") + assert overlay.footprint == {"vertices_m": vertices} + + +@pytest.mark.parametrize( + "path", + ( + "missing.vx_max", + "velocity_smoother.ros__parameters.max_velocity[4]", + "controller_server.ros__parameters.FollowPath.", + ), +) +def test_nav2_rejects_nonexistent_speed_parameter(tmp_path: Path, path: str) -> None: + source = tmp_path / "nav2.yaml" + source.write_text(yaml.safe_dump(BASE_PARAMS), encoding="utf-8") + profile = GazeboNav2ProfileConfig( + base_nav2_params="nav2/params.yaml", speed_param_paths=(path,) + ) + with pytest.raises(ValidationError, match="speed parameter"): + Nav2ParamOverlay(source, profile).patch(speed_mps=0.3, depth_topic="/processed/depth") + + +def test_world_patches_lighting_ground_and_pallet(tmp_path: Path) -> None: + source = tmp_path / "world.sdf" + source.write_text(BASE_WORLD, encoding="utf-8") + pallet = PalletPlacement("dropped_pallet", Pose2D(x=1, y=2, yaw=0.5), 0.135) + world = SdfPatcher(source).patch( + lighting_scale=0.5, floor_friction=0.35, real_time_factor=0.75, pallets=(pallet,) + ) + + root = ElementTree.fromstring(world.content) # noqa: S314 - produced by the safe parser + assert world.name == "warehouse" + assert root.findtext(".//scene/ambient") == "0.4 0.3 0.2 1" + assert [light.text for light in root.findall(".//light/diffuse")] == [ + "0.5 0.4 0.3 1", + "0.5 0.5 0.5 0.8", + ] + assert root.findtext(".//collision/surface/friction/ode/mu") == "0.35" + assert root.findtext(".//collision/surface/friction/ode/mu2") == "0.35" + assert root.findtext(".//physics/real_time_factor") == "0.75" + assert root.findtext(".//include/uri") == "model://dropped_pallet" + assert root.findtext(".//include/name") == "dropped_pallet_0" + assert root.findtext(".//include/pose") == "1 2 0.0675 0 0 0.5" + assert source.read_text(encoding="utf-8") == BASE_WORLD + + +@pytest.mark.parametrize( + ("document", "message"), + [ + ("\n" + BASE_WORLD, "DOCTYPE"), + ("", "invalid SDF"), + ("", "exactly one named world"), + (BASE_WORLD.replace("ground_plane", "another_model"), "ground plane"), + (BASE_WORLD.replace("1 0.8 0.6 1", "invalid"), "RGBA"), + ], +) +def test_world_rejects_invalid_input(tmp_path: Path, document: str, message: str) -> None: + source = tmp_path / "world.sdf" + source.write_text(document, encoding="utf-8") + with pytest.raises(ValidationError, match=message): + SdfPatcher(source).patch( + lighting_scale=0.5, floor_friction=0.5, real_time_factor=1, pallets=() + ) diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index b22f7a82..af193f80 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -87,6 +87,7 @@ Materialisation writes into `workdir//`: | `perturb_params.yaml` | `PerturbationConfig` | P3 model constants, P4, P8, P6, per-node seeds | | `bridge.yaml` | template | lidar to `/scan_raw`, depth to `…/points_raw`, ground-truth pose topic | | `episode.yaml` | `EpisodeConfig` | start, goal, time limit, sample rate, record_bag | +| `topics.yaml`, `map.yaml`, `map.pgm`, `perception_model.yaml` | `GazeboMaterializer` | Workdir-local copies of the configured topics, map and perception inputs | | `manifest.json` | `ManifestBuilder` | all hashes and versions (21 §4) | | Param | Mechanism | Detail | @@ -117,6 +118,19 @@ for each processed cloud. The installed OAK-D simulation publishes 320×240 poin Friction and lighting are world properties ⇒ a fresh world load per run (no warm reset in MVP; see §8). +The generated `episode.yaml` stores planar start and goal poses as `{x, y, yaw}`, the world and +robot names, map filename, robot footprint, and the resolved pallet pose and dimensions. The +footprint comes from the Nav2 local costmap profile and contains `vertices_m` or `radius_m`. +`time_limit_s` is derived from the task’s nominal path and P1. `wall_timeout_factor` expresses +the worker’s wall-time budget as a multiple of that limit. Startup, teardown and spawn tolerances +come from the typed backend and oracle settings. + +The perturbation file uses `illuminance_lux`, `perception_model_file`, `lidar.sigma_m`, +`lidar.dropout_p`, `cmd_delay_ms` and `rate_hz`. `seeds` contains distinct derived streams +named `sensor_perturber`, `perception_degrader` and `cmd_delay`. The lidar also reads its seed +from the top-level `seed` field. The bridge keeps the stock clock, joint state, TF, IMU and odometry +routes while using the verified topic catalog for the perturbation and ground-truth edges. + **SUT profiles** (`configs/sut_profiles/.yaml`, schema `schemas/sut_profile.schema.json`, loader `roboprobe.scenario.sut_profile.load_sut_profile`, T-016) select the robot stack: for `gazebo_nav2` the base `base_nav2_params`, the `speed_param_paths` that receive P1 and a nested `nav2_overrides` overlay; for `kinematic2d` the @@ -124,6 +138,10 @@ Friction and lighting are world properties ⇒ a fresh world load per run (no wa config, so only behaviour-relevant edits change it (docs/33 comparability rule). Values awaiting verification are listed under `provisional` and rejected by strict loading. +The standard Gazebo SUT profile uses the worker’s `backends.gazebo_nav2.base_nav2_params` setting +as its base. A profile that names a different parameter file uses that file before merging +`nav2_overrides` and applying the episode speed and topic routing. + ## 5. Episode lifecycle (Gazebo) ```mermaid diff --git a/docs/28-traceability-matrix.md b/docs/28-traceability-matrix.md index 75e22c40..b22218b9 100644 --- a/docs/28-traceability-matrix.md +++ b/docs/28-traceability-matrix.md @@ -61,6 +61,10 @@ Creation durability and scale follow-up (#147–#149): 200 ms p95 target at 10,000 runs. `tests/unit/runner/test_campaign_space_loader.py` covers stored uploaded spaces and pins; factory tests verify effective selection snapshot restoration. +Gazebo materialization tests in `tests/unit/backends/gazebo/test_materialize.py` cover stable +generated files, the individual effects of P1–P8, XML document restrictions and ROS topic and +parameter contracts (T-096a, FR-SIM-003). + ## 2. Non-functional requirements → verification | NFR | Design | Verification | Gate | From 28ce3c916f6a5e41ce02efd0f8700e2a0114b641 Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:59:13 +0530 Subject: [PATCH 2/2] fix(gazebo): bound generated seeds to Harmonic CLI range (T-096a) --- backend/src/roboprobe/backends/gazebo_nav2/materialize.py | 4 +++- backend/tests/fixtures/gazebo_materialize/episode.yaml | 2 +- backend/tests/unit/backends/gazebo/test_materialize.py | 3 +++ docs/10-simulator-integration.md | 2 ++ 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/backend/src/roboprobe/backends/gazebo_nav2/materialize.py b/backend/src/roboprobe/backends/gazebo_nav2/materialize.py index 8452be2c..a22a0ef1 100644 --- a/backend/src/roboprobe/backends/gazebo_nav2/materialize.py +++ b/backend/src/roboprobe/backends/gazebo_nav2/materialize.py @@ -38,6 +38,8 @@ _PERCEPTION_FILE = "perception_model.yaml" _WORLD_ROBOT = "turtlebot4" _MIN_PACKAGE_PATH_PARTS = 2 +# Gazebo Harmonic's CLI passes the seed through a signed C int. +_GAZEBO_CLI_MAX_SEED = (1 << 31) - 1 _GZ_TO_ROS = "GZ_TO_ROS" _ROS_TO_GZ = "ROS_TO_GZ" _STOCK_BRIDGES = ( @@ -271,7 +273,7 @@ def materialize( params = self._nav2(topics, speed) map_yaml, map_image = _map_assets(self._root, self._task.map) time_limit = time_limit_s(self._task, speed, polyline_length_m(nominal)) - simulator_seed = derive_seed(seeds.run, SIMULATOR) + simulator_seed = derive_seed(seeds.run, SIMULATOR) & _GAZEBO_CLI_MAX_SEED files = { "world.sdf": world.content, "nav2_params.yaml": params.content.encode(), diff --git a/backend/tests/fixtures/gazebo_materialize/episode.yaml b/backend/tests/fixtures/gazebo_materialize/episode.yaml index 560a6562..cdf4e434 100644 --- a/backend/tests/fixtures/gazebo_materialize/episode.yaml +++ b/backend/tests/fixtures/gazebo_materialize/episode.yaml @@ -17,7 +17,7 @@ pallets: record_bag: always robot_name: turtlebot4 sample_hz: 15.0 -simulator_seed: 2047656344537034507 +simulator_seed: 775188235 spawn_tolerance_m: 0.25 start: x: 0.0 diff --git a/backend/tests/unit/backends/gazebo/test_materialize.py b/backend/tests/unit/backends/gazebo/test_materialize.py index 2d972d7d..ca216616 100644 --- a/backend/tests/unit/backends/gazebo/test_materialize.py +++ b/backend/tests/unit/backends/gazebo/test_materialize.py @@ -172,6 +172,9 @@ def test_materializes_golden_launch_files( yaml.safe_load(episode.files["map.yaml"].read_text(encoding="utf-8"))["image"] == "map.pgm" ) assert episode.payload["time_limit_s"] == 55.0 + captured = yaml.safe_load(episode.files["episode.yaml"].read_bytes()) + assert captured["simulator_seed"] == episode.payload["simulator_seed"] + assert 0 <= captured["simulator_seed"] <= (1 << 31) - 1 def _changed_yaml_keys(left: object, right: object, prefix: str) -> set[str]: diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index af193f80..84e76074 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -130,6 +130,8 @@ The perturbation file uses `illuminance_lux`, `perception_model_file`, `lidar.si named `sensor_perturber`, `perception_degrader` and `cmd_delay`. The lidar also reads its seed from the top-level `seed` field. The bridge keeps the stock clock, joint state, TF, IMU and odometry routes while using the verified topic catalog for the perturbation and ground-truth edges. +The Gazebo CLI uses the low 31 bits of the simulator seed to fit Harmonic's signed integer +argument. ROS node seeds retain their full derived precision. **SUT profiles** (`configs/sut_profiles/.yaml`, schema `schemas/sut_profile.schema.json`, loader `roboprobe.scenario.sut_profile.load_sut_profile`, T-016) select the robot stack: for `gazebo_nav2` the base