From 450b8f083a7d4054c7c5b3b0ddc374bc72908d59 Mon Sep 17 00:00:00 2001 From: CodewithJha <155089480+CodewithJha@users.noreply.github.com> Date: Mon, 23 Nov 2026 02:43:32 +0530 Subject: [PATCH] feat(ros): orchestrate timed Gazebo navigation episodes (T-094c) --- .github/workflows/ros-package.yml | 2 +- CHANGELOG.md | 1 + docs/28-traceability-matrix.md | 2 +- ros_ws/src/roboprobe_ros/package.xml | 4 + .../roboprobe_ros/episode_manager.py | 218 ++++++++++++++++++ ros_ws/src/roboprobe_ros/setup.py | 1 + .../test/test_episode_manager.py | 168 ++++++++++++++ 7 files changed, 394 insertions(+), 2 deletions(-) create mode 100644 ros_ws/src/roboprobe_ros/roboprobe_ros/episode_manager.py create mode 100644 ros_ws/src/roboprobe_ros/test/test_episode_manager.py 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()