From 5df8576000ef03b4c200226182e2ad3e5acd05bc Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:06:20 +0530 Subject: [PATCH 1/2] feat(ros): define Gazebo episode inputs and exit codes (T-094c) --- docs/10-simulator-integration.md | 3 + docs/27-configuration-reference.md | 5 ++ .../config/episode_exit_codes.yaml | 4 ++ ros_ws/src/roboprobe_ros/config/topics.yaml | 6 +- .../src/roboprobe_ros/roboprobe_ros/base.py | 11 +++- .../roboprobe_ros/episode_config.py | 60 +++++++++++++++++ ros_ws/src/roboprobe_ros/test/test_base.py | 2 + .../roboprobe_ros/test/test_episode_config.py | 66 +++++++++++++++++++ schemas/ros_topics.schema.json | 3 +- 9 files changed, 157 insertions(+), 3 deletions(-) create mode 100644 ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml create mode 100644 ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py create mode 100644 ros_ws/src/roboprobe_ros/test/test_episode_config.py diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index e5351fe8..6d4fbfff 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -144,6 +144,9 @@ sequenceDiagram Communication worker ↔ episode: **files + exit code** (`episode_done.json`, `trace.parquet`, `episode.log`), not a live ROS connection — keeps the worker free of rclpy state and makes crashes easy to classify as F-INFRA. +The [shared exit-code table](../ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml) defines +`0` for completed episodes, `10` for startup timeouts, `11` for failed spawn validation, and `12` +for internal errors. A navigation timeout is a completed episode with status `TIMEOUT`. ## 6. Ground truth & recording diff --git a/docs/27-configuration-reference.md b/docs/27-configuration-reference.md index 5af0bf93..58337b88 100644 --- a/docs/27-configuration-reference.md +++ b/docs/27-configuration-reference.md @@ -377,6 +377,11 @@ also fails if a configured group lacks samples or reports failed requests. ### 4.15 Gazebo episode ROS parameters +The worker materializes `episode.yaml` with `start` and `goal` mappings containing +`x` and `y` in metres and `yaw` in radians. The navigation budget is `time_limit_s` +in simulation seconds. Multiplying it by `wall_timeout_factor` gives the wall-clock +safety limit. `startup_timeout_s` and `spawn_tolerance_m` carry the typed settings into ROS. + | ROS parameter | Default | Use | |---------------|---------|-----| | `topics_file` | Installed `roboprobe_ros/config/topics.yaml` | Logical names and QoS profiles for the node's topics | diff --git a/ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml b/ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml new file mode 100644 index 00000000..660f5376 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml @@ -0,0 +1,4 @@ +completed: 0 +startup_timeout: 10 +spawn_failed: 11 +internal_error: 12 diff --git a/ros_ws/src/roboprobe_ros/config/topics.yaml b/ros_ws/src/roboprobe_ros/config/topics.yaml index 989b7735..6faed412 100644 --- a/ros_ws/src/roboprobe_ros/config/topics.yaml +++ b/ros_ws/src/roboprobe_ros/config/topics.yaml @@ -35,6 +35,10 @@ topics: name: /amcl_pose type: geometry_msgs/msg/PoseWithCovarianceStamped qos: default + initial_pose: + name: /initialpose + type: geometry_msgs/msg/PoseWithCovarianceStamped + qos: default ground_truth_pose: name: /ground_truth/pose type: geometry_msgs/msg/PoseStamped @@ -50,4 +54,4 @@ frames: laser: rplidar_link geometry: lidar_plane_height_m: 0.194 - dropped_pallet_height_m: 0.144 \ No newline at end of file + dropped_pallet_height_m: 0.144 diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py index f5606c6f..5bf78a33 100644 --- a/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/base.py @@ -55,7 +55,8 @@ def __init__( seed = self.declare_parameter("seed", 0) topics_path = Path(topic_file.get_parameter_value().string_value) perturb_path = perturb_file.get_parameter_value().string_value - self._topics = _mapping(_read_yaml(topics_path).get("topics"), "topics") + self._catalog = _read_yaml(topics_path) + self._topics = _mapping(self._catalog.get("topics"), "topics") self.perturbation = _read_yaml(Path(perturb_path)) if perturb_path else {} self.rng = Generator(PCG64(seed.get_parameter_value().integer_value)) except Exception: @@ -83,6 +84,14 @@ def qos(self, key: str) -> QoSProfile: msg = f"topics.{key}.qos must name a supported QoS profile" raise ValueError(msg) + def frame(self, key: str) -> str: + """Resolve a logical frame to its configured tf frame identifier.""" + value = _mapping(self._catalog.get("frames"), "frames")[key] + if not isinstance(value, str) or not value: + msg = f"frames.{key} must be a nonempty string" + raise ValueError(msg) + return value + def log_event(self, event: str, **fields: object) -> None: """Write one JSON event with the common backend log fields.""" entry = { diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py new file mode 100644 index 00000000..1e57abf4 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py @@ -0,0 +1,60 @@ +"""Read the episode inputs and exit codes shared by the worker and ROS manager.""" + +from __future__ import annotations + +from dataclasses import dataclass +from math import isfinite +from pathlib import Path + +import yaml +from ament_index_python.packages import get_package_share_directory + + +def episode_exit_codes() -> dict[str, int]: + """Return the process exit classifications distributed with the ROS topic map.""" + share = Path(get_package_share_directory("roboprobe_ros")) + with (share / "config" / "episode_exit_codes.yaml").open(encoding="utf-8") as config: + return {str(key): int(code) for key, code in yaml.safe_load(config).items()} + + +def _float(value: object) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)) or not isfinite(value): + msg = "Episode poses, time limits and tolerance must be finite numbers" + raise ValueError(msg) + return float(value) + + +def _pose(data: object) -> tuple[float, float, float]: + if not isinstance(data, dict): + msg = "Episode start and goal must specify x, y and yaw" + raise TypeError(msg) + return _float(data["x"]), _float(data["y"]), _float(data["yaw"]) + + +@dataclass(frozen=True) +class EpisodeConfig: + """Start, goal and timing limits materialized for one navigation episode.""" + + start: tuple[float, float, float] + goal: tuple[float, float, float] + sim_limit: float + wall_limit: float + startup_limit: float + spawn_tolerance: float + + @classmethod + def read(cls, path: Path) -> EpisodeConfig: + """Read finite poses and positive limits from the worker's episode YAML.""" + with path.open(encoding="utf-8") as stream: + data = yaml.safe_load(stream) + sim_limit = _float(data["time_limit_s"]) + limits = ( + sim_limit, + _float(data["wall_timeout_factor"]) * sim_limit, + _float(data["startup_timeout_s"]), + _float(data["spawn_tolerance_m"]), + ) + if not isfinite(limits[1]) or min(limits) <= 0.0: + msg = "Episode time limits and spawn tolerance must be finite and positive" + raise ValueError(msg) + return cls(_pose(data["start"]), _pose(data["goal"]), *limits) diff --git a/ros_ws/src/roboprobe_ros/test/test_base.py b/ros_ws/src/roboprobe_ros/test/test_base.py index d1ad764f..720ba776 100644 --- a/ros_ws/src/roboprobe_ros/test/test_base.py +++ b/ros_ws/src/roboprobe_ros/test/test_base.py @@ -41,6 +41,8 @@ def test_installed_topics_and_vendored_kernels(ros_context: Context) -> None: assert node.topic("scan_raw").startswith("/") assert node.qos("scan_raw").reliability is QoSReliabilityPolicy.BEST_EFFORT assert node.qos("cmd_vel").reliability is QoSReliabilityPolicy.RELIABLE + assert node.topic("initial_pose") == "/initialpose" + assert node.frame("map") == "map" assert CircleFootprint.__module__.startswith("roboprobe_ros._vendor.") assert OccupancyGrid.__module__.startswith("roboprobe_ros._vendor.") assert CommandDelayBuffer.__module__.startswith("roboprobe_ros._vendor.") diff --git a/ros_ws/src/roboprobe_ros/test/test_episode_config.py b/ros_ws/src/roboprobe_ros/test/test_episode_config.py new file mode 100644 index 00000000..3ee25238 --- /dev/null +++ b/ros_ws/src/roboprobe_ros/test/test_episode_config.py @@ -0,0 +1,66 @@ +"""Verify the materialized episode configuration and installed exit codes.""" + +# ruff: noqa: S101 + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import pytest +import yaml +from roboprobe_ros.episode_config import EpisodeConfig, episode_exit_codes + +if TYPE_CHECKING: + from pathlib import Path + + +@pytest.fixture +def inputs() -> dict[str, object]: + """Return the worker's episode YAML fields.""" + return { + "start": {"x": 1.0, "y": 2.0, "yaw": -0.3}, + "goal": {"x": 4.5, "y": 6.0, "yaw": 0.5}, + "time_limit_s": 3.0, + "wall_timeout_factor": 4.0, + "startup_timeout_s": 2.0, + "spawn_tolerance_m": 0.15, + } + + +def test_reads_episode_limits(tmp_path: Path, inputs: dict[str, object]) -> None: + """Convert the materialized factor to a wall-clock budget.""" + path = tmp_path / "episode.yaml" + path.write_text(yaml.safe_dump(inputs), encoding="utf-8") + assert EpisodeConfig.read(path) == EpisodeConfig( + (1.0, 2.0, -0.3), (4.5, 6.0, 0.5), 3, 12, 2, 0.15 + ) + + +@pytest.mark.parametrize( + ("key", "value", "error"), + [ + ("start", [0, 0, 0], TypeError), + ("startup_timeout_s", 0, ValueError), + ("wall_timeout_factor", float("inf"), ValueError), + ("time_limit_s", False, ValueError), + ], +) +def test_rejects_invalid_inputs( + tmp_path: Path, inputs: dict[str, object], key: str, value: object, error: type[Exception] +) -> None: + """Reject malformed poses, nonnumeric values, and invalid limits before starting ROS.""" + path = tmp_path / "episode.yaml" + inputs[key] = value + path.write_text(yaml.safe_dump(inputs), encoding="utf-8") + with pytest.raises(error): + EpisodeConfig.read(path) + + +def test_exit_codes_are_packaged() -> None: + """Keep process outcomes stable for the worker reading the same YAML.""" + assert episode_exit_codes() == { + "completed": 0, + "startup_timeout": 10, + "spawn_failed": 11, + "internal_error": 12, + } diff --git a/schemas/ros_topics.schema.json b/schemas/ros_topics.schema.json index dc1654b8..d5889e32 100644 --- a/schemas/ros_topics.schema.json +++ b/schemas/ros_topics.schema.json @@ -34,6 +34,7 @@ "cmd_vel": { "$ref": "#/$defs/ros_topic" }, "odom": { "$ref": "#/$defs/ros_topic" }, "amcl_pose": { "$ref": "#/$defs/ros_topic" }, + "initial_pose": { "$ref": "#/$defs/ros_topic" }, "ground_truth_pose": { "$ref": "#/$defs/ros_topic" }, "bt_log": { "$ref": "#/$defs/ros_topic" } } @@ -91,4 +92,4 @@ } } } -} \ No newline at end of file +} From b97e45f55fe451ff764fb2b0b6b2cc1569541151 Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:41:12 +0530 Subject: [PATCH 2/2] feat(ros): serialize Gazebo episode outcomes atomically (T-094c) --- docs/10-simulator-integration.md | 6 ++- docs/27-configuration-reference.md | 1 + .../roboprobe_ros/episode_config.py | 48 ++++++++++++++++++- .../roboprobe_ros/test/test_episode_config.py | 27 ++++++++++- 4 files changed, 78 insertions(+), 4 deletions(-) diff --git a/docs/10-simulator-integration.md b/docs/10-simulator-integration.md index 6d4fbfff..0303be72 100644 --- a/docs/10-simulator-integration.md +++ b/docs/10-simulator-integration.md @@ -127,8 +127,9 @@ sequenceDiagram L->>G: gz sim -s -r --headless-rendering world.sdf L->>N: nav2 bringup with nav2_params.yaml L->>E: start nodes (perturber, degrader, delay, gt, recorder, manager) - E->>N: wait lifecycle ACTIVE (timeout startup_timeout_s) - E->>N: publish /initialpose (start pose) + E->>N: wait for AMCL lifecycle ACTIVE (startup_timeout_s) + E->>N: publish /initialpose and verify localization + E->>N: wait for BT navigator lifecycle ACTIVE E->>R: start recording E->>N: NavigateToPose(goal) loop until result, T_max, or wall timeout @@ -147,6 +148,7 @@ not a live ROS connection — keeps the worker free of rclpy state and makes cra The [shared exit-code table](../ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml) defines `0` for completed episodes, `10` for startup timeouts, `11` for failed spawn validation, and `12` for internal errors. A navigation timeout is a completed episode with status `TIMEOUT`. +The manager writes `episode_done.json` beside `episode.yaml` after the recorder flushes. ## 6. Ground truth & recording diff --git a/docs/27-configuration-reference.md b/docs/27-configuration-reference.md index 58337b88..b51e78cb 100644 --- a/docs/27-configuration-reference.md +++ b/docs/27-configuration-reference.md @@ -387,6 +387,7 @@ safety limit. `startup_timeout_s` and `spawn_tolerance_m` carry the typed settin | `topics_file` | Installed `roboprobe_ros/config/topics.yaml` | Logical names and QoS profiles for the node's topics | | `perturb_file` | Empty | YAML parameters written into the episode working directory | | `seed` | `0` | Initial PCG64 state for the node's random stream | +| `episode_file` | Required by `episode_manager` | Path to the materialized `episode.yaml` | The lidar node reads a nonnegative episode `seed` from the `perturb_file`. `lidar.sigma_m` is the finite, nonnegative Gaussian standard deviation in metres. diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py index 1e57abf4..0f4c4603 100644 --- a/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py +++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py @@ -1,10 +1,13 @@ -"""Read the episode inputs and exit codes shared by the worker and ROS manager.""" +"""Define episode inputs, results and exit codes shared by the worker and manager.""" from __future__ import annotations +import json from dataclasses import dataclass +from datetime import UTC, datetime from math import isfinite from pathlib import Path +from time import monotonic import yaml from ament_index_python.packages import get_package_share_directory @@ -58,3 +61,46 @@ def read(cls, path: Path) -> EpisodeConfig: msg = "Episode time limits and spawn tolerance must be finite and positive" raise ValueError(msg) return cls(_pose(data["start"]), _pose(data["goal"]), *limits) + + +@dataclass(frozen=True) +class EpisodeTime: + """Capture the simulation, monotonic, and UTC clocks for a lifecycle event.""" + + sim: float + wall: float + utc: str + + @classmethod + def capture(cls, sim_time: float) -> EpisodeTime: + """Record the current monotonic and UTC instants at a simulator time.""" + return cls(sim_time, monotonic(), datetime.now(UTC).isoformat()) + + +@dataclass(frozen=True) +class EpisodeResult: + """A terminal navigation outcome and its measured interval.""" + + status: str + nav_code: int | None + start: EpisodeTime + end: EpisodeTime + recoveries: int + + def write(self, path: Path) -> None: + """Publish a complete result atomically after the recorder has flushed.""" + sim_time = max(0.0, self.end.sim - self.start.sim) + wall_time = max(0.0, self.end.wall - self.start.wall) + details = { + "status": self.status, + "nav_result_code": self.nav_code, + "t_start": self.start.utc, + "t_end": self.end.utc, + "sim_time_s": sim_time, + "wall_time_s": wall_time, + "rtf_mean": sim_time / wall_time if wall_time else 0.0, + "recoveries": self.recoveries, + } + temporary = path.with_suffix(".tmp") + temporary.write_text(json.dumps(details, allow_nan=False) + "\n", encoding="utf-8") + temporary.replace(path) diff --git a/ros_ws/src/roboprobe_ros/test/test_episode_config.py b/ros_ws/src/roboprobe_ros/test/test_episode_config.py index 3ee25238..2c847431 100644 --- a/ros_ws/src/roboprobe_ros/test/test_episode_config.py +++ b/ros_ws/src/roboprobe_ros/test/test_episode_config.py @@ -4,11 +4,17 @@ from __future__ import annotations +import json from typing import TYPE_CHECKING import pytest import yaml -from roboprobe_ros.episode_config import EpisodeConfig, episode_exit_codes +from roboprobe_ros.episode_config import ( + EpisodeConfig, + EpisodeResult, + EpisodeTime, + episode_exit_codes, +) if TYPE_CHECKING: from pathlib import Path @@ -64,3 +70,22 @@ def test_exit_codes_are_packaged() -> None: "spawn_failed": 11, "internal_error": 12, } + + +def test_writes_worker_result(tmp_path: Path) -> None: + """Publish one complete document with elapsed times across clock origins.""" + result = tmp_path / "episode_done.json" + start = EpisodeTime(10.0, 20.0, "2026-11-22T20:00:00+00:00") + end = EpisodeTime(12.0, 21.0, "2026-11-22T20:00:01+00:00") + EpisodeResult("TIMEOUT", None, start, end, 2).write(result) + assert json.loads(result.read_text(encoding="utf-8")) == { + "status": "TIMEOUT", + "nav_result_code": None, + "t_start": start.utc, + "t_end": end.utc, + "sim_time_s": 2.0, + "wall_time_s": 1.0, + "rtf_mean": 2.0, + "recoveries": 2, + } + assert not result.with_suffix(".tmp").exists()