diff --git a/.github/workflows/ros-package.yml b/.github/workflows/ros-package.yml
index 043c79f..8a76bd7 100644
--- a/.github/workflows/ros-package.yml
+++ b/.github/workflows/ros-package.yml
@@ -35,7 +35,7 @@ jobs:
apt-get update
apt-get install -y --no-install-recommends \
python3-numpy python3-scipy python3-yaml python3-pytest \
- ros-jazzy-launch-testing ros-jazzy-rmw-cyclonedds-cpp
+ ros-jazzy-launch-testing ros-jazzy-rmw-cyclonedds-cpp ros-jazzy-nav2-msgs
- name: Build and test
working-directory: ros_ws
env:
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 3d43721..7bbc4c9 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -28,6 +28,7 @@ versioning: [Semantic Versioning 2.0.0](https://semver.org/) (policy in `docs/33
- 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.
+- Gazebo episode manager (T-094c) starts localization and navigation and records terminal status and timings.
- Opt-in experiment API launch atomically creates the planned campaign matrix and runs it
through the orchestrator and workers (T-084a).
- Casebook and Insights (T-110a/b): filter and edit analyzed failure modes, replay or export
diff --git a/docs/28-traceability-matrix.md b/docs/28-traceability-matrix.md
index 75e22c4..af67e2e 100644
--- a/docs/28-traceability-matrix.md
+++ b/docs/28-traceability-matrix.md
@@ -9,7 +9,7 @@
| PRD | SRS requirements | Design (docs §) | Tasks (22) | Verification (18) |
|-----|------------------|-----------------|-----------|-------------------|
| PR-01 Scenario space | FR-SCN-001…007 | 06 §2–5, 05 §3 | T-020…T-025 | unit/property `tests/unit/scenario`, schema fixtures |
-| PR-02 Autonomous campaigns | FR-RUN-001…006, FR-SIM-001, FR-SIM-003 | 04 §4–6, 05 §5, 10, 11 §4, 14 §2–5 | T-012, T-033, T-035, T-040…T-047, T-091, T-092, T-093a/b, T-096 | integration runner, chaos, backend contract, [warehouse map alignment](evidence/T-092.md), Jazzy package and lidar launch tests, sim map alignment |
+| PR-02 Autonomous campaigns | FR-RUN-001…006, FR-SIM-001, FR-SIM-003 | 04 §4–6, 05 §5, 10, 11 §4, 14 §2–5 | T-012, T-033, T-035, T-040…T-047, T-091, T-092, T-093a/b, T-094c, T-096 | integration runner, chaos, backend contract, [warehouse map alignment](evidence/T-092.md), Jazzy package, lidar and navigation tests, sim map alignment |
| PR-03 Intelligent selection | FR-SEL-002/003/005/006/007/008 | 07 §4–6 | T-055…T-061 | strategy contract, benchmark gates 18 §4.4, EXP-01 |
| PR-04 Baselines | FR-SEL-001 | 07 §3 | T-050, T-051 | strategy contract, design tests |
| PR-05 Failure detection | FR-ORC-001…006 | 08 | T-036…T-039 | golden traces, invariants |
diff --git a/ros_ws/src/roboprobe_ros/package.xml b/ros_ws/src/roboprobe_ros/package.xml
index ee2d52b..73eb822 100644
--- a/ros_ws/src/roboprobe_ros/package.xml
+++ b/ros_ws/src/roboprobe_ros/package.xml
@@ -15,10 +15,14 @@
std_msgs
nav_msgs
nav2_msgs
+ action_msgs
+ lifecycle_msgs
+ std_srvs
python3-numpy
python3-scipy
python3-yaml
python3-pytest
+ rosgraph_msgs
launch_ros
launch_testing
rosgraph_msgs
diff --git a/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_manager.py b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_manager.py
new file mode 100644
index 0000000..d7c2702
--- /dev/null
+++ b/ros_ws/src/roboprobe_ros/roboprobe_ros/episode_manager.py
@@ -0,0 +1,218 @@
+"""Drive one Gazebo/Nav2 episode and report its result (T-094c, FR-SIM-003)."""
+
+from __future__ import annotations
+
+from math import cos, hypot, sin
+from pathlib import Path
+from time import monotonic
+from typing import TYPE_CHECKING, Final, Protocol
+
+import rclpy
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
+from lifecycle_msgs.msg import State
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose
+from rclpy.action import ActionClient
+from std_msgs.msg import String
+from std_srvs.srv import SetBool, Trigger
+
+from roboprobe_ros.base import RoboProbeNode
+from roboprobe_ros.episode_config import (
+ EpisodeConfig,
+ EpisodeResult,
+ EpisodeTime,
+ episode_exit_codes,
+)
+
+if TYPE_CHECKING:
+ from collections.abc import Callable
+
+ from rclpy.client import Client
+ from rclpy.parameter import Parameter as ParameterValue
+
+_POLL_S: Final = 0.05
+_NS_PER_S: Final = 1_000_000_000
+_STATUSES: Final = {
+ GoalStatus.STATUS_SUCCEEDED: "SUCCEEDED",
+ GoalStatus.STATUS_ABORTED: "ABORTED",
+ GoalStatus.STATUS_CANCELED: "CANCELED",
+}
+
+
+class _RecorderResponse(Protocol):
+ success: bool
+
+
+class EpisodeManager(RoboProbeNode):
+ """Wait for localization, navigate, flush the recorder and write episode_done.json."""
+
+ def __init__(self, *, parameter_overrides: list[ParameterValue] | None = None) -> None:
+ """Read the materialized episode and resolve its ROS endpoints."""
+ super().__init__("episode_manager", parameter_overrides=parameter_overrides)
+ try:
+ self._configure()
+ except Exception:
+ self.destroy_node()
+ raise
+
+ def _configure(self) -> None:
+ path = self.declare_parameter("episode_file", "").get_parameter_value().string_value
+ self._destination = Path(path).with_name("episode_done.json")
+ self._episode = EpisodeConfig.read(Path(path))
+ self._pose = self.create_publisher(
+ PoseWithCovarianceStamped, self.topic("initial_pose"), self.qos("initial_pose")
+ )
+ self._localized = False
+ self._map_frame = self.frame("map")
+ self.create_subscription(
+ PoseWithCovarianceStamped,
+ self.topic("amcl_pose"),
+ self._localization,
+ self.qos("amcl_pose"),
+ )
+ self._status = self.create_publisher(String, "/roboprobe/nav_status", 1)
+ self._amcl_state = self.create_client(GetState, "amcl/get_state")
+ self._nav_state = self.create_client(GetState, "bt_navigator/get_state")
+ self._start_recording = self.create_client(Trigger, "episode_recorder/start")
+ self._stop_recording = self.create_client(SetBool, "episode_recorder/stop")
+ self._navigate = ActionClient(self, NavigateToPose, "navigate_to_pose")
+ self._recoveries = 0
+ self._codes = episode_exit_codes()
+
+ def _sim_time(self) -> float:
+ return int(self.get_clock().now().nanoseconds) / _NS_PER_S
+
+ def _wait(self, ready: Callable[[], bool], remaining: Callable[[], float]) -> bool:
+ while not ready():
+ if (seconds := remaining()) <= 0.0:
+ return False
+ rclpy.spin_once(self, timeout_sec=min(seconds, _POLL_S))
+ return True
+
+ def _active(
+ self, client: Client[GetState.Request, GetState.Response], remaining: Callable[[], float]
+ ) -> bool:
+ while self._wait(client.service_is_ready, remaining) and remaining() > 0.0:
+ state = client.call_async(GetState.Request())
+ if not self._wait(state.done, remaining):
+ return False
+ if state.result().current_state.id == State.PRIMARY_STATE_ACTIVE:
+ return True
+ rclpy.spin_once(self, timeout_sec=max(0.0, min(remaining(), _POLL_S)))
+ return False
+
+ def _stamp(self, coordinates: tuple[float, float, float]) -> PoseStamped:
+ x, y, yaw = coordinates
+ pose = PoseStamped()
+ pose.header.frame_id = self._map_frame
+ pose.header.stamp = self.get_clock().now().to_msg()
+ pose.pose.position.x, pose.pose.position.y = x, y
+ pose.pose.orientation.z, pose.pose.orientation.w = sin(yaw / 2.0), cos(yaw / 2.0)
+ return pose
+
+ def _localization(self, message: PoseWithCovarianceStamped) -> None:
+ position = message.pose.pose.position
+ x, y, _ = self._episode.start
+ self._localized = (
+ message.header.frame_id == self._map_frame
+ and hypot(position.x - x, position.y - y) <= self._episode.spawn_tolerance
+ )
+
+ def _spawn(self, remaining: Callable[[], float]) -> bool:
+ if not self._wait(lambda: self._pose.get_subscription_count() > 0, remaining):
+ return False
+ stamped = self._stamp(self._episode.start)
+ initial = PoseWithCovarianceStamped(header=stamped.header)
+ initial.pose.pose = stamped.pose
+ self._localized = False
+ self._pose.publish(initial)
+ return self._wait(lambda: self._localized, remaining)
+
+ def _trigger[Request, Response: _RecorderResponse](
+ self, client: Client[Request, Response], message: Request
+ ) -> None:
+ deadline = monotonic() + self._episode.startup_limit
+
+ def remaining() -> float:
+ return deadline - monotonic()
+
+ if self._wait(client.service_is_ready, remaining):
+ response = client.call_async(message)
+ if self._wait(response.done, remaining) and response.result().success:
+ return
+ msg = f"Recorder service failed: {client.srv_name}"
+ raise RuntimeError(msg)
+
+ def _feedback(self, message: NavigateToPose.Impl.FeedbackMessage) -> None:
+ self._recoveries = message.feedback.number_of_recoveries
+
+ def _drive(self, start: EpisodeTime) -> tuple[str, int | None]:
+ def remaining() -> float:
+ return min(
+ self._episode.wall_limit - (monotonic() - start.wall),
+ self._episode.sim_limit - (self._sim_time() - start.sim),
+ )
+
+ goal = NavigateToPose.Goal(pose=self._stamp(self._episode.goal))
+ pending = self._navigate.send_goal_async(goal, feedback_callback=self._feedback)
+ if not self._wait(pending.done, remaining):
+ return "TIMEOUT", None
+ handle = pending.result()
+ if not handle.accepted:
+ return "ABORTED", GoalStatus.STATUS_ABORTED
+ result = handle.get_result_async()
+ if not self._wait(result.done, remaining):
+ handle.cancel_goal_async()
+ return "TIMEOUT", None
+ code = result.result().status
+ return _STATUSES[code], code
+
+ def run(self) -> int:
+ """Run the episode and return the exit code shared with the backend."""
+ start = EpisodeTime.capture(self._sim_time())
+ code = None
+ finished = None
+
+ def remaining() -> float:
+ return self._episode.startup_limit - (monotonic() - start.wall)
+
+ if not self._active(self._amcl_state, remaining):
+ status = "STARTUP_TIMEOUT"
+ elif not self._spawn(remaining):
+ status = "SPAWN_FAILED"
+ elif not self._active(self._nav_state, remaining) or not self._wait(
+ self._navigate.server_is_ready, remaining
+ ):
+ status = "STARTUP_TIMEOUT"
+ else:
+ self._trigger(self._start_recording, Trigger.Request())
+ start = EpisodeTime.capture(self._sim_time())
+ self._status.publish(String(data="ACTIVE"))
+ complete = False
+ try:
+ status, code = self._drive(start)
+ complete = status != "TIMEOUT"
+ self._status.publish(String(data=status))
+ finally:
+ finished = EpisodeTime.capture(self._sim_time())
+ self._trigger(self._stop_recording, SetBool.Request(data=complete))
+ end = finished or EpisodeTime.capture(self._sim_time())
+ EpisodeResult(status, code, start, end, self._recoveries).write(self._destination)
+ return self._codes.get(status.lower(), self._codes["completed"])
+
+
+def main(args: list[str] | None = None) -> int:
+ """Run and release the episode manager, classifying unexpected failures."""
+ rclpy.init(args=args)
+ try:
+ manager = EpisodeManager()
+ try:
+ return manager.run()
+ finally:
+ manager.destroy_node()
+ except (KeyboardInterrupt, Exception) as exc:
+ rclpy.logging.get_logger("episode_manager").error(str(exc))
+ return episode_exit_codes()["internal_error"]
+ finally:
+ rclpy.try_shutdown()
diff --git a/ros_ws/src/roboprobe_ros/setup.py b/ros_ws/src/roboprobe_ros/setup.py
index 6a01d62..ffdbdf6 100644
--- a/ros_ws/src/roboprobe_ros/setup.py
+++ b/ros_ws/src/roboprobe_ros/setup.py
@@ -24,6 +24,7 @@
entry_points={
"console_scripts": [
"cmd_delay = roboprobe_ros.cmd_delay:main",
+ "episode_manager = roboprobe_ros.episode_manager:main",
"perception_degrader = roboprobe_ros.perception_degrader:main",
"sensor_perturber = roboprobe_ros.sensor_perturber:main",
],
diff --git a/ros_ws/src/roboprobe_ros/test/test_episode_manager.py b/ros_ws/src/roboprobe_ros/test/test_episode_manager.py
new file mode 100644
index 0000000..bdc152f
--- /dev/null
+++ b/ros_ws/src/roboprobe_ros/test/test_episode_manager.py
@@ -0,0 +1,168 @@
+# ruff: noqa: PLR2004, S101
+"""Drive the installed manager against mock Jazzy Nav2 and recorder endpoints."""
+
+from __future__ import annotations
+
+import json
+from threading import Thread
+from typing import TYPE_CHECKING
+
+import pytest
+import rclpy
+import yaml
+from action_msgs.msg import GoalStatus
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from lifecycle_msgs.msg import State
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose
+from rclpy.action import ActionServer, CancelResponse
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.task import Future
+from roboprobe_ros.episode_manager import EpisodeManager, main
+from rosgraph_msgs.msg import Clock
+from std_srvs.srv import SetBool, Trigger
+
+if TYPE_CHECKING:
+ from pathlib import Path
+
+ from rclpy.action.server import ServerGoalHandle
+
+
+@pytest.mark.parametrize(
+ ("case", "status", "nav_code", "exit_code"),
+ [
+ ("success", "SUCCEEDED", GoalStatus.STATUS_SUCCEEDED, 0),
+ ("abort", "ABORTED", GoalStatus.STATUS_ABORTED, 0),
+ ("sim_timeout", "TIMEOUT", None, 0),
+ ("wall_timeout", "TIMEOUT", None, 0),
+ ("startup_timeout", "STARTUP_TIMEOUT", None, 10),
+ ("spawn_failed", "SPAWN_FAILED", None, 11),
+ ],
+)
+def test_episode(
+ tmp_path: Path, case: str, status: str, nav_code: int | None, exit_code: int
+) -> None:
+ """Check terminal status, recorder flushing, and simulated and wall deadlines."""
+ episode = tmp_path / "episode.yaml"
+ pose = {"x": 1.0, "y": 2.0, "yaw": 0.25}
+ config = {
+ "start": pose,
+ "goal": {**pose, "x": 3.0},
+ "time_limit_s": 2.0,
+ "startup_timeout_s": 0.6 if case == "startup_timeout" else 3.0,
+ "wall_timeout_factor": 0.25 if case == "wall_timeout" else 5.0,
+ "spawn_tolerance_m": 0.2,
+ }
+ episode.write_text(yaml.safe_dump(config), encoding="utf-8")
+ events: list[str | tuple[str, bool]] = []
+ rclpy.init()
+ mock = Node("mock_nav2")
+ executor = SingleThreadedExecutor()
+ executor.add_node(mock)
+ server = _serve(mock, case, events)
+ thread = Thread(target=executor.spin)
+ thread.start()
+ try:
+ manager = EpisodeManager(
+ parameter_overrides=[
+ Parameter("episode_file", value=str(episode)),
+ Parameter("use_sim_time", value=True),
+ ]
+ )
+ try:
+ assert manager.run() == exit_code
+ finally:
+ manager.destroy_node()
+ result = json.loads((tmp_path / "episode_done.json").read_text(encoding="utf-8"))
+ assert result["status"] == status
+ assert result["nav_result_code"] == nav_code
+ assert result["t_start"] <= result["t_end"]
+ if exit_code == 0:
+ assert events == ["pose", "start", "goal", ("stop", status != "TIMEOUT")]
+ if case == "sim_timeout":
+ assert result["sim_time_s"] == pytest.approx(2.0, abs=0.25)
+ assert result["wall_time_s"] < 2.0
+ if case == "wall_timeout":
+ assert result["sim_time_s"] == 0.0
+ assert result["wall_time_s"] == pytest.approx(0.5, abs=0.2)
+ finally:
+ executor.shutdown(timeout_sec=5.0)
+ thread.join(timeout=5.0)
+ server.destroy()
+ mock.destroy_node()
+ rclpy.try_shutdown()
+
+
+def _serve(node: Node, case: str, events: list[str | tuple[str, bool]]) -> ActionServer:
+ stop: Future[None] = Future()
+ clock = node.create_publisher(Clock, "/clock", 10)
+ tick = Clock()
+ tick.clock.sec = 1
+ tick.clock.nanosec = 0
+
+ def step() -> None:
+ elapsed, tick.clock.nanosec = divmod(tick.clock.nanosec + 100_000_000, 1_000_000_000)
+ tick.clock.sec += elapsed
+ clock.publish(tick)
+
+ if case != "wall_timeout":
+ node.create_timer(0.03, step)
+ amcl = node.create_publisher(PoseWithCovarianceStamped, "/amcl_pose", 10)
+
+ def localize(pose: PoseWithCovarianceStamped) -> None:
+ events.append("pose")
+ if case == "spawn_failed":
+ pose.pose.pose.position.x += 1.0
+ amcl.publish(pose)
+
+ def start(_request: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
+ events.append("start")
+ response.success = True
+ return response
+
+ def flush(request: SetBool.Request, response: SetBool.Response) -> SetBool.Response:
+ events.append(("stop", request.data))
+ response.success = True
+ return response
+
+ def cancel(_goal: ServerGoalHandle) -> CancelResponse:
+ stop.set_result(None)
+ return CancelResponse.ACCEPT
+
+ async def navigate(goal: ServerGoalHandle) -> NavigateToPose.Result:
+ events.append("goal")
+ goal.publish_feedback(NavigateToPose.Feedback(number_of_recoveries=1))
+ if case.endswith("timeout"):
+ await stop
+ goal.canceled()
+ elif case == "success":
+ goal.succeed()
+ else:
+ goal.abort()
+ return NavigateToPose.Result()
+
+ active = State.PRIMARY_STATE_ACTIVE
+ amcl_id = State.PRIMARY_STATE_INACTIVE if case == "startup_timeout" else active
+ node.create_service(
+ GetState, "amcl/get_state", lambda _, _r: GetState.Response(current_state=State(id=amcl_id))
+ )
+ node.create_service(
+ GetState,
+ "bt_navigator/get_state",
+ lambda _, _r: GetState.Response(
+ current_state=State(id=active if "pose" in events else State.PRIMARY_STATE_INACTIVE)
+ ),
+ )
+ node.create_subscription(PoseWithCovarianceStamped, "/initialpose", localize, 10)
+ node.create_service(Trigger, "episode_recorder/start", start)
+ node.create_service(SetBool, "episode_recorder/stop", flush)
+ return ActionServer(node, NavigateToPose, "navigate_to_pose", navigate, cancel_callback=cancel)
+
+
+def test_missing_episode_is_an_internal_error(tmp_path: Path) -> None:
+ """Classify invalid input before producing an episode result."""
+ missing = tmp_path / "episode.yaml"
+ assert main(["--ros-args", "-p", f"episode_file:={missing}"]) == 12
+ assert not (tmp_path / "episode_done.json").exists()