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
9 changes: 7 additions & 2 deletions docs/10-simulator-integration.md
Original file line number Diff line number Diff line change
Expand Up @@ -133,8 +133,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
Expand All @@ -150,6 +151,10 @@ 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`.
The manager writes `episode_done.json` beside `episode.yaml` after the recorder flushes.

## 6. Ground truth & recording

Expand Down
6 changes: 6 additions & 0 deletions docs/27-configuration-reference.md
Original file line number Diff line number Diff line change
Expand Up @@ -377,11 +377,17 @@ 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 |
| `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.
Expand Down
4 changes: 4 additions & 0 deletions ros_ws/src/roboprobe_ros/config/episode_exit_codes.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
completed: 0
startup_timeout: 10
spawn_failed: 11
internal_error: 12
6 changes: 5 additions & 1 deletion ros_ws/src/roboprobe_ros/config/topics.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -50,4 +54,4 @@ frames:
laser: rplidar_link
geometry:
lidar_plane_height_m: 0.194
dropped_pallet_height_m: 0.144
dropped_pallet_height_m: 0.144
14 changes: 11 additions & 3 deletions ros_ws/src/roboprobe_ros/roboprobe_ros/base.py
Original file line number Diff line number Diff line change
Expand Up @@ -63,9 +63,9 @@ 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
topic_config = _read_yaml(topics_path)
self._topics = _mapping(topic_config.get("topics"), "topics")
self.stamped_cmd_vel = _stamped_cmd_vel(topic_config)
self._catalog = _read_yaml(topics_path)
self._topics = _mapping(self._catalog.get("topics"), "topics")
self.stamped_cmd_vel = _stamped_cmd_vel(self._catalog)
self.perturbation = _read_yaml(Path(perturb_path)) if perturb_path else {}
self.rng = Generator(PCG64(seed.get_parameter_value().integer_value))
except Exception:
Expand Down Expand Up @@ -93,6 +93,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 = {
Expand Down
106 changes: 106 additions & 0 deletions ros_ws/src/roboprobe_ros/roboprobe_ros/episode_config.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
"""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


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)


@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)
2 changes: 2 additions & 0 deletions ros_ws/src/roboprobe_ros/test/test_base.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.")
Expand Down
91 changes: 91 additions & 0 deletions ros_ws/src/roboprobe_ros/test/test_episode_config.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,91 @@
"""Verify the materialized episode configuration and installed exit codes."""

# ruff: noqa: S101

from __future__ import annotations

import json
from typing import TYPE_CHECKING

import pytest
import yaml
from roboprobe_ros.episode_config import (
EpisodeConfig,
EpisodeResult,
EpisodeTime,
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,
}


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()
3 changes: 2 additions & 1 deletion schemas/ros_topics.schema.json
Original file line number Diff line number Diff line change
Expand Up @@ -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" }
}
Expand Down Expand Up @@ -91,4 +92,4 @@
}
}
}
}
}
Loading