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
2 changes: 1 addition & 1 deletion .github/workflows/ros-package.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion docs/28-traceability-matrix.md
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down
4 changes: 4 additions & 0 deletions ros_ws/src/roboprobe_ros/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,14 @@
<exec_depend>std_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>nav2_msgs</exec_depend>
<exec_depend>action_msgs</exec_depend>
<exec_depend>lifecycle_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>python3-scipy</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<test_depend>python3-pytest</test_depend>
<test_depend>rosgraph_msgs</test_depend>
<test_depend>launch_ros</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>rosgraph_msgs</test_depend>
Expand Down
218 changes: 218 additions & 0 deletions ros_ws/src/roboprobe_ros/roboprobe_ros/episode_manager.py
Original file line number Diff line number Diff line change
@@ -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()
1 change: 1 addition & 0 deletions ros_ws/src/roboprobe_ros/setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
],
Expand Down
Loading
Loading