From 4cb01405cf01a153e801b6aaa161520be2dc1442 Mon Sep 17 00:00:00 2001 From: Staaaaaaaaar <2300012435@stu.pku.edu.cn> Date: Mon, 17 Aug 2026 15:49:08 +0800 Subject: [PATCH] feat(runtime): add reusable ROS protocol helpers --- packages/README.md | 4 +- packages/rh_ros/README.md | 63 ++++++++ packages/rh_ros/package.xml | 23 +++ packages/rh_ros/resource/rh_ros | 1 + packages/rh_ros/rh_ros/__init__.py | 57 +++++++ packages/rh_ros/rh_ros/conversions.py | 186 +++++++++++++++++++++++ packages/rh_ros/rh_ros/errors.py | 43 ++++++ packages/rh_ros/rh_ros/py.typed | 1 + packages/rh_ros/rh_ros/qos.py | 45 ++++++ packages/rh_ros/rh_ros/reset_guard.py | 94 ++++++++++++ packages/rh_ros/rh_ros/sequence.py | 62 ++++++++ packages/rh_ros/rh_ros/service.py | 68 +++++++++ packages/rh_ros/rh_ros/status.py | 165 ++++++++++++++++++++ packages/rh_ros/setup.cfg | 5 + packages/rh_ros/setup.py | 21 +++ packages/rh_ros/test/test_conversions.py | 108 +++++++++++++ packages/rh_ros/test/test_qos.py | 42 +++++ packages/rh_ros/test/test_reset_guard.py | 99 ++++++++++++ packages/rh_ros/test/test_sequence.py | 47 ++++++ packages/rh_ros/test/test_service.py | 96 ++++++++++++ packages/rh_ros/test/test_status.py | 105 +++++++++++++ pyproject.toml | 2 +- tests/README.md | 4 + 23 files changed, 1339 insertions(+), 2 deletions(-) create mode 100644 packages/rh_ros/README.md create mode 100644 packages/rh_ros/package.xml create mode 100644 packages/rh_ros/resource/rh_ros create mode 100644 packages/rh_ros/rh_ros/__init__.py create mode 100644 packages/rh_ros/rh_ros/conversions.py create mode 100644 packages/rh_ros/rh_ros/errors.py create mode 100644 packages/rh_ros/rh_ros/py.typed create mode 100644 packages/rh_ros/rh_ros/qos.py create mode 100644 packages/rh_ros/rh_ros/reset_guard.py create mode 100644 packages/rh_ros/rh_ros/sequence.py create mode 100644 packages/rh_ros/rh_ros/service.py create mode 100644 packages/rh_ros/rh_ros/status.py create mode 100644 packages/rh_ros/setup.cfg create mode 100644 packages/rh_ros/setup.py create mode 100644 packages/rh_ros/test/test_conversions.py create mode 100644 packages/rh_ros/test/test_qos.py create mode 100644 packages/rh_ros/test/test_reset_guard.py create mode 100644 packages/rh_ros/test/test_sequence.py create mode 100644 packages/rh_ros/test/test_service.py create mode 100644 packages/rh_ros/test/test_status.py diff --git a/packages/README.md b/packages/README.md index 07fb504..35bc470 100644 --- a/packages/README.md +++ b/packages/README.md @@ -8,5 +8,7 @@ specific simulator, robot, agent, task, or evaluator implementation. ROS wire contract. - [`rh_core`](rh_core/README.md) owns ROS-independent configuration, domain models, lifecycle rules, and termination policy. +- [`rh_ros`](rh_ros/README.md) owns reusable ROS runtime QoS, heartbeat, + idempotency, deadline, sequence-filtering, and model/message adapters. -`rh_ros` and `rh_experiment` are introduced by their roadmap PRs. +`rh_experiment` is introduced by its roadmap PR. diff --git a/packages/rh_ros/README.md b/packages/rh_ros/README.md new file mode 100644 index 0000000..55bb125 --- /dev/null +++ b/packages/rh_ros/README.md @@ -0,0 +1,63 @@ +# `rh_ros` + +`rh_ros` is the reusable ROS 2 runtime protocol layer between the +ROS-independent `rh_core` domain and the `rh_interfaces` wire contract. It +standardizes transport behavior that Environment, Agent, Evaluator, and +Experiment components would otherwise implement differently. + +It is deliberately a thin library: it contains no ROS node executable, owns no +Experiment lifecycle, and has no dependency on a simulator, robot, task, +evaluator, or concrete Agent. + +## Runtime responsibilities + +- Canonical QoS factories for latched control snapshots, velocity commands, and + sensor data. +- Immediate component-status transitions plus periodic 1 Hz heartbeats. +- Steady-clock readiness and stale-heartbeat tracking, independent of `/clock`. +- Thread-safe reset idempotency with duplicate-result replay and conflicting + request-ID rejection. +- Separate service discovery and call deadlines with structured failures. +- Explicit current-Episode selection and monotonically increasing sequence + filtering. +- Validated conversion between core Pose3D/PointNav/lifecycle values and ROS + messages. + +## Contract details + +Status, Episode state, PointNav task, and result snapshots use reliable, +transient-local, keep-last-one QoS. This lets late-joining consumers receive the +most recent authoritative snapshot. A `ComponentStatus.stamp` is committed only +on a transition; heartbeat republishes preserve it. Staleness is measured from +local receipt time with a steady clock because simulation time can be absent or +frozen during startup and failure handling. + +`IdempotentResetGuard` requires the server adapter to construct a hashable +fingerprint from every behaviorally relevant request field. An equivalent +duplicate replays the first result without executing the backend again. Reusing +the same ID with different content is rejected. Completed records are bounded; +capacity should cover the maximum expected retry window. + +`call_service_with_deadline` is intentionally blocking and requires the node's +executor to spin on another thread. It distinguishes service discovery timeout, +call completion timeout, and service failure. It never retries implicitly. + +Episode IDs are opaque strings. `EpisodeSequenceGuard.activate()` must therefore +be called explicitly for each new Episode; the guard never guesses ordering from +an ID. It then accepts only matching messages whose sequence strictly increases. + +## Model boundary + +The conversion module is the only owner of ROS/core mapping. It converts the +complete initial 3D pose to `PoseStamped` with a normalized quaternion and the +PointNav 3D target position to `PointStamped`. PointNav does not gain a target +orientation through this adapter. Incoming quaternions and numeric values are +validated before core values are exposed. + +Concrete components remain responsible for: + +- deciding when they are genuinely READY; +- implementing the actual reset and constructing its request fingerprint; +- mapping structured protocol failures to component-specific responses; +- activating the Episode expected by their business lifecycle; and +- spinning an executor appropriate for their callback concurrency. diff --git a/packages/rh_ros/package.xml b/packages/rh_ros/package.xml new file mode 100644 index 0000000..320e617 --- /dev/null +++ b/packages/rh_ros/package.xml @@ -0,0 +1,23 @@ + + + + rh_ros + 0.0.0 + Reusable ROS 2 runtime protocol helpers for RoboHarness. + Staaaaaaaaar + + NOASSERTION + + ament_python + + geometry_msgs + rclpy + rh_core + rh_interfaces + + python3-pytest + + + ament_python + + diff --git a/packages/rh_ros/resource/rh_ros b/packages/rh_ros/resource/rh_ros new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/packages/rh_ros/resource/rh_ros @@ -0,0 +1 @@ + diff --git a/packages/rh_ros/rh_ros/__init__.py b/packages/rh_ros/rh_ros/__init__.py new file mode 100644 index 0000000..edcbc03 --- /dev/null +++ b/packages/rh_ros/rh_ros/__init__.py @@ -0,0 +1,57 @@ +"""Reusable ROS 2 runtime protocol helpers for RoboHarness.""" + +from rh_ros.conversions import ( + episode_state_to_message, + episode_state_values_from_message, + point_from_message, + point_to_message, + pointnav_task_to_message, + pose_from_message, + pose_to_message, + quaternion_to_rpy, + rpy_to_quaternion, +) +from rh_ros.errors import ( + ConversionError, + InvalidProtocolValueError, + ResetRequestConflictError, + RuntimeProtocolError, + ServiceCallError, + ServiceCallTimeoutError, + ServiceDiscoveryTimeoutError, +) +from rh_ros.qos import command_qos, latched_control_qos, sensor_qos +from rh_ros.reset_guard import IdempotentResetGuard +from rh_ros.sequence import EpisodeIdentity, EpisodeSequenceGuard +from rh_ros.service import call_service_with_deadline +from rh_ros.status import ReceivedStatus, StatusMonitor, StatusPublisher, StatusTracker + +__all__ = [ + "ConversionError", + "EpisodeIdentity", + "EpisodeSequenceGuard", + "IdempotentResetGuard", + "InvalidProtocolValueError", + "ReceivedStatus", + "ResetRequestConflictError", + "RuntimeProtocolError", + "ServiceCallError", + "ServiceCallTimeoutError", + "ServiceDiscoveryTimeoutError", + "StatusMonitor", + "StatusPublisher", + "StatusTracker", + "call_service_with_deadline", + "command_qos", + "episode_state_to_message", + "episode_state_values_from_message", + "latched_control_qos", + "point_from_message", + "point_to_message", + "pointnav_task_to_message", + "pose_from_message", + "pose_to_message", + "quaternion_to_rpy", + "rpy_to_quaternion", + "sensor_qos", +] diff --git a/packages/rh_ros/rh_ros/conversions.py b/packages/rh_ros/rh_ros/conversions.py new file mode 100644 index 0000000..e482137 --- /dev/null +++ b/packages/rh_ros/rh_ros/conversions.py @@ -0,0 +1,186 @@ +"""Conversions at the ROS-independent core / ROS wire boundary.""" + +from __future__ import annotations + +import math +from collections.abc import Iterable + +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PointStamped, PoseStamped, Quaternion +from rh_interfaces.msg import EpisodeState as EpisodeStateMessage +from rh_interfaces.msg import PointNavTask + +from rh_core import ( + EpisodeLifecycle, + EpisodeSpec, + EpisodeState, + Point3D, + Pose3D, + TerminationReason, +) +from rh_ros.errors import ConversionError + +_QUATERNION_NORM_TOLERANCE = 1e-6 + + +def _finite(values: Iterable[float], description: str) -> None: + if not all(math.isfinite(value) for value in values): + raise ConversionError(f"{description} must contain only finite values") + + +def _identifier(value: str, name: str) -> None: + if not isinstance(value, str) or not value.strip(): + raise ConversionError(f"{name} must be a non-empty string") + + +def rpy_to_quaternion(roll: float, pitch: float, yaw: float) -> Quaternion: + """Convert fixed-axis XYZ roll/pitch/yaw to a normalized quaternion.""" + + _finite((roll, pitch, yaw), "roll/pitch/yaw") + cr, sr = math.cos(roll / 2.0), math.sin(roll / 2.0) + cp, sp = math.cos(pitch / 2.0), math.sin(pitch / 2.0) + cy, sy = math.cos(yaw / 2.0), math.sin(yaw / 2.0) + quaternion = Quaternion() + quaternion.x = sr * cp * cy - cr * sp * sy + quaternion.y = cr * sp * cy + sr * cp * sy + quaternion.z = cr * cp * sy - sr * sp * cy + quaternion.w = cr * cp * cy + sr * sp * sy + return quaternion + + +def quaternion_to_rpy(quaternion: Quaternion) -> tuple[float, float, float]: + """Validate a wire quaternion and convert it to fixed-axis XYZ RPY.""" + + x, y, z, w = quaternion.x, quaternion.y, quaternion.z, quaternion.w + _finite((x, y, z, w), "quaternion") + norm = math.sqrt(x * x + y * y + z * z + w * w) + if abs(norm - 1.0) > _QUATERNION_NORM_TOLERANCE: + raise ConversionError("quaternion must be normalized") + + sinr_cosp = 2.0 * (w * x + y * z) + cosr_cosp = 1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(sinr_cosp, cosr_cosp) + sinp = 2.0 * (w * y - z * x) + pitch = math.copysign(math.pi / 2.0, sinp) if abs(sinp) >= 1.0 else math.asin(sinp) + siny_cosp = 2.0 * (w * z + x * y) + cosy_cosp = 1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(siny_cosp, cosy_cosp) + return roll, pitch, yaw + + +def pose_to_message(pose: Pose3D, *, stamp: Time | None = None) -> PoseStamped: + """Convert a complete core pose to a ROS pose snapshot.""" + + _identifier(pose.frame_id, "pose.frame_id") + _finite((pose.x, pose.y, pose.z, pose.roll, pose.pitch, pose.yaw), "pose") + message = PoseStamped() + if stamp is not None: + message.header.stamp = stamp + message.header.frame_id = pose.frame_id + message.pose.position.x = pose.x + message.pose.position.y = pose.y + message.pose.position.z = pose.z + message.pose.orientation = rpy_to_quaternion(pose.roll, pose.pitch, pose.yaw) + return message + + +def pose_from_message(message: PoseStamped) -> Pose3D: + """Convert a ROS pose after finite and quaternion validation.""" + + _identifier(message.header.frame_id, "pose.header.frame_id") + position = message.pose.position + _finite((position.x, position.y, position.z), "pose.position") + roll, pitch, yaw = quaternion_to_rpy(message.pose.orientation) + return Pose3D( + frame_id=message.header.frame_id, + x=position.x, + y=position.y, + z=position.z, + roll=roll, + pitch=pitch, + yaw=yaw, + ) + + +def point_to_message(point: Point3D, *, stamp: Time | None = None) -> PointStamped: + """Convert a core 3D point to a ROS point snapshot.""" + + _identifier(point.frame_id, "point.frame_id") + _finite((point.x, point.y, point.z), "point") + message = PointStamped() + if stamp is not None: + message.header.stamp = stamp + message.header.frame_id = point.frame_id + message.point.x = point.x + message.point.y = point.y + message.point.z = point.z + return message + + +def point_from_message(message: PointStamped) -> Point3D: + """Convert a finite ROS point snapshot to the core representation.""" + + _identifier(message.header.frame_id, "point.header.frame_id") + _finite((message.point.x, message.point.y, message.point.z), "point") + return Point3D( + frame_id=message.header.frame_id, + x=message.point.x, + y=message.point.y, + z=message.point.z, + ) + + +def pointnav_task_to_message( + experiment_id: str, + episode: EpisodeSpec, + *, + stamp: Time | None = None, +) -> PointNavTask: + """Build the immutable PointNav wire snapshot for one Episode.""" + + _identifier(experiment_id, "experiment_id") + _identifier(episode.episode_id, "episode_id") + task = episode.task + _finite((task.success_radius_m, task.timeout_s), "PointNav parameters") + if task.success_radius_m <= 0.0 or task.timeout_s <= 0.0: + raise ConversionError("PointNav radius and timeout must be positive") + message = PointNavTask() + message.experiment_id = experiment_id + message.episode_id = episode.episode_id + message.goal = point_to_message(task.goal, stamp=stamp) + message.success_radius_m = task.success_radius_m + message.timeout_s = task.timeout_s + message.seed = episode.seed + return message + + +def episode_state_to_message( + experiment_id: str, + lifecycle: EpisodeLifecycle, + *, + stamp: Time, + detail: str = "", +) -> EpisodeStateMessage: + """Convert an authoritative core lifecycle snapshot to its wire form.""" + + _identifier(experiment_id, "experiment_id") + message = EpisodeStateMessage() + message.stamp = stamp + message.experiment_id = experiment_id + message.episode_id = lifecycle.episode_id + message.sequence = lifecycle.sequence + message.state = lifecycle.state.value + message.termination_reason = lifecycle.termination_reason.value + message.detail = detail + return message + + +def episode_state_values_from_message( + message: EpisodeStateMessage, +) -> tuple[EpisodeState, TerminationReason]: + """Validate numeric wire values before exposing core enums.""" + + try: + return EpisodeState(message.state), TerminationReason(message.termination_reason) + except ValueError as error: + raise ConversionError("EpisodeState contains an unknown numeric value") from error diff --git a/packages/rh_ros/rh_ros/errors.py b/packages/rh_ros/rh_ros/errors.py new file mode 100644 index 0000000..7302231 --- /dev/null +++ b/packages/rh_ros/rh_ros/errors.py @@ -0,0 +1,43 @@ +"""Structured failures raised by the ROS runtime protocol layer.""" + + +class RuntimeProtocolError(RuntimeError): + """Base class for failures with stable machine-readable codes.""" + + code = "runtime_protocol_error" + + +class InvalidProtocolValueError(RuntimeProtocolError, ValueError): + """A caller supplied a value that cannot satisfy the wire contract.""" + + code = "invalid_protocol_value" + + +class ResetRequestConflictError(RuntimeProtocolError): + """A request ID was reused for a different reset request.""" + + code = "reset_request_conflict" + + +class ServiceDiscoveryTimeoutError(RuntimeProtocolError, TimeoutError): + """A service did not become discoverable before its deadline.""" + + code = "service_discovery_timeout" + + +class ServiceCallTimeoutError(RuntimeProtocolError, TimeoutError): + """A discovered service did not complete before its call deadline.""" + + code = "service_call_timeout" + + +class ServiceCallError(RuntimeProtocolError): + """A service future completed with an exception or without a result.""" + + code = "service_call_error" + + +class ConversionError(RuntimeProtocolError, ValueError): + """A domain object or ROS message cannot be converted safely.""" + + code = "conversion_error" diff --git a/packages/rh_ros/rh_ros/py.typed b/packages/rh_ros/rh_ros/py.typed new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/packages/rh_ros/rh_ros/py.typed @@ -0,0 +1 @@ + diff --git a/packages/rh_ros/rh_ros/qos.py b/packages/rh_ros/rh_ros/qos.py new file mode 100644 index 0000000..56437ae --- /dev/null +++ b/packages/rh_ros/rh_ros/qos.py @@ -0,0 +1,45 @@ +"""Canonical QoS profiles for the RoboHarness runtime contract.""" + +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + LivelinessPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, +) + + +def latched_control_qos() -> QoSProfile: + """Return the profile for status, state, task, and result snapshots.""" + + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + liveliness=LivelinessPolicy.AUTOMATIC, + ) + + +def command_qos() -> QoSProfile: + """Return the low-depth reliable profile for robot velocity commands.""" + + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + liveliness=LivelinessPolicy.AUTOMATIC, + ) + + +def sensor_qos() -> QoSProfile: + """Return an independent copy of the ROS sensor-data profile.""" + + return QoSProfile( + history=qos_profile_sensor_data.history, + depth=qos_profile_sensor_data.depth, + reliability=qos_profile_sensor_data.reliability, + durability=qos_profile_sensor_data.durability, + ) diff --git a/packages/rh_ros/rh_ros/reset_guard.py b/packages/rh_ros/rh_ros/reset_guard.py new file mode 100644 index 0000000..827df58 --- /dev/null +++ b/packages/rh_ros/rh_ros/reset_guard.py @@ -0,0 +1,94 @@ +"""Thread-safe idempotency guard for reset service implementations.""" + +from __future__ import annotations + +from collections import OrderedDict +from collections.abc import Callable, Hashable +from dataclasses import dataclass +from threading import Condition +from typing import Generic, TypeVar + +from rh_ros.errors import InvalidProtocolValueError, ResetRequestConflictError + +FingerprintT = TypeVar("FingerprintT", bound=Hashable) +ResultT = TypeVar("ResultT") + + +@dataclass(slots=True) +class _Record(Generic[FingerprintT, ResultT]): + fingerprint: FingerprintT + complete: bool = False + result: ResultT | None = None + error: BaseException | None = None + + +class IdempotentResetGuard(Generic[FingerprintT, ResultT]): + """Execute one reset per request ID and replay its completed result. + + The caller supplies a hashable fingerprint containing every behaviorally + relevant request field. Reusing an ID with a different fingerprint is a + protocol conflict rather than a second reset. + """ + + def __init__(self, *, capacity: int = 128) -> None: + if isinstance(capacity, bool) or not isinstance(capacity, int) or capacity < 1: + raise InvalidProtocolValueError("capacity must be a positive integer") + self._capacity = capacity + self._records: OrderedDict[str, _Record[FingerprintT, ResultT]] = OrderedDict() + self._condition = Condition() + + def execute( + self, + request_id: str, + fingerprint: FingerprintT, + operation: Callable[[], ResultT], + ) -> ResultT: + """Return the original result for all equivalent duplicate requests.""" + + if not isinstance(request_id, str) or not request_id.strip(): + raise InvalidProtocolValueError("request_id must be a non-empty string") + + with self._condition: + record = self._records.get(request_id) + if record is not None: + if record.fingerprint != fingerprint: + raise ResetRequestConflictError( + f"request_id {request_id!r} was reused with different content" + ) + while not record.complete: + self._condition.wait() + self._records.move_to_end(request_id) + if record.error is not None: + raise record.error + return record.result # type: ignore[return-value] + + record = _Record(fingerprint=fingerprint) + self._records[request_id] = record + + try: + result = operation() + except BaseException as error: + with self._condition: + record.error = error + record.complete = True + self._records.move_to_end(request_id) + self._evict_completed() + self._condition.notify_all() + raise + + with self._condition: + record.result = result + record.complete = True + self._records.move_to_end(request_id) + self._evict_completed() + self._condition.notify_all() + return result + + def _evict_completed(self) -> None: + completed = sum(record.complete for record in self._records.values()) + for request_id in tuple(self._records): + if completed <= self._capacity: + break + if self._records[request_id].complete: + del self._records[request_id] + completed -= 1 diff --git a/packages/rh_ros/rh_ros/sequence.py b/packages/rh_ros/rh_ros/sequence.py new file mode 100644 index 0000000..bfc17e1 --- /dev/null +++ b/packages/rh_ros/rh_ros/sequence.py @@ -0,0 +1,62 @@ +"""Filtering for authoritative Episode snapshots.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from rh_interfaces.msg import EpisodeState + +from rh_ros.errors import InvalidProtocolValueError + + +@dataclass(frozen=True, slots=True) +class EpisodeIdentity: + """Explicit identity of the Episode whose messages may be accepted.""" + + experiment_id: str + episode_id: str + + def __post_init__(self) -> None: + if not self.experiment_id.strip() or not self.episode_id.strip(): + raise InvalidProtocolValueError("experiment_id and episode_id must not be empty") + + +class EpisodeSequenceGuard: + """Reject delayed, duplicate, or out-of-order Episode state messages. + + Episode IDs are opaque, so advancing to another Episode is always explicit. + """ + + def __init__(self) -> None: + self._identity: EpisodeIdentity | None = None + self._last_sequence: int | None = None + + @property + def identity(self) -> EpisodeIdentity | None: + return self._identity + + @property + def last_sequence(self) -> int | None: + return self._last_sequence + + def activate(self, experiment_id: str, episode_id: str) -> None: + """Select a new authoritative Episode and clear sequence history.""" + + self._identity = EpisodeIdentity(experiment_id, episode_id) + self._last_sequence = None + + def accept(self, message: EpisodeState) -> bool: + """Accept a matching snapshot only when its sequence strictly increases.""" + + if self._identity is None: + return False + if ( + message.experiment_id != self._identity.experiment_id + or message.episode_id != self._identity.episode_id + ): + return False + sequence = int(message.sequence) + if self._last_sequence is not None and sequence <= self._last_sequence: + return False + self._last_sequence = sequence + return True diff --git a/packages/rh_ros/rh_ros/service.py b/packages/rh_ros/rh_ros/service.py new file mode 100644 index 0000000..b5b878a --- /dev/null +++ b/packages/rh_ros/rh_ros/service.py @@ -0,0 +1,68 @@ +"""Explicit discovery and call deadlines for short ROS services.""" + +from __future__ import annotations + +import math +from threading import Event +from typing import Any, TypeVar + +from rh_ros.errors import ( + InvalidProtocolValueError, + ServiceCallError, + ServiceCallTimeoutError, + ServiceDiscoveryTimeoutError, +) + +ResponseT = TypeVar("ResponseT") + + +def _duration(value: float, name: str) -> float: + if isinstance(value, bool) or not isinstance(value, int | float): + raise InvalidProtocolValueError(f"{name} must be a finite positive number") + value = float(value) + if not math.isfinite(value) or value <= 0.0: + raise InvalidProtocolValueError(f"{name} must be a finite positive number") + return value + + +def call_service_with_deadline( + client: Any, + request: Any, + *, + discovery_timeout_s: float, + call_timeout_s: float, +) -> ResponseT: + """Make a blocking call with separate discovery and completion deadlines. + + The node executor must be spinning on another thread while this function + waits. Waiting uses OS events and therefore does not depend on simulation + time. Retries are deliberately left to the caller. + """ + + discovery_timeout = _duration(discovery_timeout_s, "discovery_timeout_s") + call_timeout = _duration(call_timeout_s, "call_timeout_s") + + if not client.wait_for_service(timeout_sec=discovery_timeout): + raise ServiceDiscoveryTimeoutError( + f"service was not discovered within {discovery_timeout:g} seconds" + ) + + future = client.call_async(request) + completed = Event() + future.add_done_callback(lambda _: completed.set()) + if not completed.wait(call_timeout): + future.cancel() + raise ServiceCallTimeoutError( + f"service call did not complete within {call_timeout:g} seconds" + ) + + try: + exception = future.exception() + except BaseException as error: + raise ServiceCallError("service future failed") from error + if exception is not None: + raise ServiceCallError("service call failed") from exception + result = future.result() + if result is None: + raise ServiceCallError("service call completed without a response") + return result diff --git a/packages/rh_ros/rh_ros/status.py b/packages/rh_ros/rh_ros/status.py new file mode 100644 index 0000000..cc90d2f --- /dev/null +++ b/packages/rh_ros/rh_ros/status.py @@ -0,0 +1,165 @@ +"""Status heartbeat publication and steady-clock readiness monitoring.""" + +from __future__ import annotations + +import math +import time +from collections.abc import Callable +from dataclasses import dataclass +from threading import Lock + +from rclpy.clock import Clock, ClockType +from rclpy.node import Node +from rh_interfaces.msg import ComponentStatus + +from rh_ros.errors import InvalidProtocolValueError +from rh_ros.qos import latched_control_qos + +_VALID_STATES = frozenset( + { + ComponentStatus.STARTING, + ComponentStatus.RESETTING, + ComponentStatus.READY, + ComponentStatus.ERROR, + } +) + + +def _positive_duration(value: float, name: str) -> float: + if isinstance(value, bool) or not isinstance(value, int | float): + raise InvalidProtocolValueError(f"{name} must be a finite positive number") + value = float(value) + if not math.isfinite(value) or value <= 0.0: + raise InvalidProtocolValueError(f"{name} must be a finite positive number") + return value + + +@dataclass(frozen=True, slots=True) +class ReceivedStatus: + message: ComponentStatus + received_at: float + + +class StatusTracker: + """Store latest status heartbeats and detect staleness using steady time.""" + + def __init__( + self, + *, + stale_timeout_s: float, + monotonic: Callable[[], float] = time.monotonic, + ) -> None: + self._stale_timeout_s = _positive_duration(stale_timeout_s, "stale_timeout_s") + self._monotonic = monotonic + self._statuses: dict[str, ReceivedStatus] = {} + self._lock = Lock() + + def update(self, message: ComponentStatus) -> bool: + """Record a valid heartbeat; malformed messages are ignored.""" + + if not message.component_id.strip() or int(message.state) not in _VALID_STATES: + return False + received = ReceivedStatus(message=message, received_at=self._monotonic()) + with self._lock: + self._statuses[message.component_id] = received + return True + + def latest(self, component_id: str) -> ComponentStatus | None: + with self._lock: + status = self._statuses.get(component_id) + return None if status is None else status.message + + def is_stale(self, component_id: str) -> bool: + now = self._monotonic() + with self._lock: + status = self._statuses.get(component_id) + return status is None or now - status.received_at > self._stale_timeout_s + + def is_ready(self, component_id: str) -> bool: + status = self.latest(component_id) + return ( + status is not None + and int(status.state) == ComponentStatus.READY + and not self.is_stale(component_id) + ) + + +class StatusPublisher: + """Publish immediate transitions and periodic heartbeats on a steady timer.""" + + def __init__( + self, + node: Node, + topic: str, + component_id: str, + *, + heartbeat_period_s: float = 1.0, + ) -> None: + if not component_id.strip(): + raise InvalidProtocolValueError("component_id must not be empty") + heartbeat_period = _positive_duration(heartbeat_period_s, "heartbeat_period_s") + self._node = node + self._component_id = component_id + self._publisher = node.create_publisher( + ComponentStatus, topic, latched_control_qos() + ) + self._message: ComponentStatus | None = None + self._timer = node.create_timer( + heartbeat_period, + self.publish, + clock=Clock(clock_type=ClockType.STEADY_TIME), + ) + self.transition(ComponentStatus.STARTING) + + @property + def message(self) -> ComponentStatus: + if self._message is None: # pragma: no cover - constructor establishes it + raise RuntimeError("status publisher is not initialized") + return self._message + + def transition( + self, + state: int, + *, + error_code: int = 0, + detail: str = "", + restart_required: bool = False, + ) -> None: + """Commit a transition timestamp and immediately publish the snapshot.""" + + if isinstance(state, bool) or state not in _VALID_STATES: + raise InvalidProtocolValueError(f"unsupported component state: {state}") + if ( + isinstance(error_code, bool) + or not isinstance(error_code, int) + or not 0 <= error_code <= 2**32 - 1 + ): + raise InvalidProtocolValueError("error_code must fit in uint32") + message = ComponentStatus() + message.stamp = self._node.get_clock().now().to_msg() + message.component_id = self._component_id + message.state = state + message.error_code = error_code + message.detail = detail + message.restart_required = restart_required + self._message = message + self.publish() + + def publish(self) -> None: + """Publish the unchanged latest snapshot as a heartbeat.""" + + if self._message is not None: + self._publisher.publish(self._message) + + +class StatusMonitor: + """ROS subscription wrapper around :class:`StatusTracker`.""" + + def __init__(self, node: Node, topic: str, *, stale_timeout_s: float) -> None: + self.tracker = StatusTracker(stale_timeout_s=stale_timeout_s) + self.subscription = node.create_subscription( + ComponentStatus, + topic, + self.tracker.update, + latched_control_qos(), + ) diff --git a/packages/rh_ros/setup.cfg b/packages/rh_ros/setup.cfg new file mode 100644 index 0000000..ef330ff --- /dev/null +++ b/packages/rh_ros/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/rh_ros + +[install] +install_scripts=$base/lib/rh_ros diff --git a/packages/rh_ros/setup.py b/packages/rh_ros/setup.py new file mode 100644 index 0000000..3328e46 --- /dev/null +++ b/packages/rh_ros/setup.py @@ -0,0 +1,21 @@ +from setuptools import find_packages, setup + +package_name = "rh_ros" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=("test",)), + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + ], + package_data={package_name: ["py.typed"]}, + install_requires=["setuptools"], + zip_safe=True, + maintainer="Staaaaaaaaar", + maintainer_email="2300012435@stu.pku.edu.cn", + description="Reusable ROS 2 runtime protocol helpers for RoboHarness.", + license="NOASSERTION", + tests_require=["pytest"], +) diff --git a/packages/rh_ros/test/test_conversions.py b/packages/rh_ros/test/test_conversions.py new file mode 100644 index 0000000..fa1b910 --- /dev/null +++ b/packages/rh_ros/test/test_conversions.py @@ -0,0 +1,108 @@ +import math + +import pytest +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PoseStamped, Quaternion + +from rh_core import ( + EpisodeLifecycle, + EpisodeSpec, + EpisodeState, + Point3D, + PointNavTaskSpec, + Pose3D, + TerminationReason, +) +from rh_ros import ( + ConversionError, + episode_state_to_message, + episode_state_values_from_message, + pointnav_task_to_message, + pose_from_message, + pose_to_message, + quaternion_to_rpy, + rpy_to_quaternion, +) + + +def _episode() -> EpisodeSpec: + return EpisodeSpec( + episode_id="episode-1", + scenario="warehouse", + initial_pose=Pose3D("map", 1.0, 2.0, 0.4, 0.1, -0.2, 0.3), + task=PointNavTaskSpec( + goal=Point3D("map", 8.0, 4.0, 0.4), + success_radius_m=0.5, + timeout_s=120.0, + ), + seed=42, + ) + + +def test_full_3d_pose_round_trip() -> None: + pose = _episode().initial_pose + + recovered = pose_from_message(pose_to_message(pose, stamp=Time(sec=3))) + + assert recovered.frame_id == "map" + assert recovered.x == pose.x + assert recovered.y == pose.y + assert recovered.z == pose.z + assert recovered.roll == pytest.approx(pose.roll) + assert recovered.pitch == pytest.approx(pose.pitch) + assert recovered.yaw == pytest.approx(pose.yaw) + + +def test_rpy_conversion_produces_normalized_quaternion() -> None: + quaternion = rpy_to_quaternion(0.4, -0.5, 1.2) + norm = math.sqrt( + quaternion.x**2 + quaternion.y**2 + quaternion.z**2 + quaternion.w**2 + ) + + assert norm == pytest.approx(1.0) + assert quaternion_to_rpy(quaternion) == pytest.approx((0.4, -0.5, 1.2)) + + +def test_non_normalized_quaternion_is_rejected() -> None: + message = PoseStamped() + message.header.frame_id = "map" + message.pose.orientation = Quaternion(w=2.0) + + with pytest.raises(ConversionError, match="normalized"): + pose_from_message(message) + + +def test_pointnav_message_contains_position_but_no_target_orientation() -> None: + message = pointnav_task_to_message("experiment-1", _episode(), stamp=Time(sec=5)) + + assert message.experiment_id == "experiment-1" + assert message.episode_id == "episode-1" + assert message.goal.header.frame_id == "map" + assert (message.goal.point.x, message.goal.point.y, message.goal.point.z) == ( + 8.0, + 4.0, + 0.4, + ) + assert not hasattr(message.goal, "pose") + assert message.seed == 42 + + +def test_lifecycle_values_convert_without_reinterpreting_wire_constants() -> None: + lifecycle = EpisodeLifecycle(episode_id="episode-1").transition(EpisodeState.READY) + message = episode_state_to_message( + "experiment-1", lifecycle, stamp=Time(sec=7), detail="ready" + ) + + state, reason = episode_state_values_from_message(message) + + assert state is EpisodeState.READY + assert reason is TerminationReason.NONE + assert message.sequence == 1 + + +@pytest.mark.parametrize("value", [float("nan"), float("inf")]) +def test_non_finite_pose_is_rejected(value: float) -> None: + pose = Pose3D("map", value, 0.0, 0.0, 0.0, 0.0, 0.0) + + with pytest.raises(ConversionError, match="finite"): + pose_to_message(pose) diff --git a/packages/rh_ros/test/test_qos.py b/packages/rh_ros/test/test_qos.py new file mode 100644 index 0000000..93fc032 --- /dev/null +++ b/packages/rh_ros/test/test_qos.py @@ -0,0 +1,42 @@ +from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSCompatibility, + ReliabilityPolicy, + qos_check_compatible, +) + +from rh_ros import command_qos, latched_control_qos, sensor_qos + + +def test_control_snapshot_profile_is_latched_and_reliable() -> None: + profile = latched_control_qos() + + assert profile.history is HistoryPolicy.KEEP_LAST + assert profile.depth == 1 + assert profile.reliability is ReliabilityPolicy.RELIABLE + assert profile.durability is DurabilityPolicy.TRANSIENT_LOCAL + + +def test_profile_factories_return_independent_values() -> None: + first = latched_control_qos() + second = latched_control_qos() + + first.depth = 7 + + assert second.depth == 1 + + +def test_control_publishers_and_subscribers_are_compatible() -> None: + compatibility, reason = qos_check_compatible( + latched_control_qos(), latched_control_qos() + ) + + assert compatibility == QoSCompatibility.OK + assert reason == "" + + +def test_standard_data_plane_profiles_have_expected_reliability() -> None: + assert command_qos().reliability is ReliabilityPolicy.RELIABLE + assert command_qos().depth == 1 + assert sensor_qos().reliability is ReliabilityPolicy.BEST_EFFORT diff --git a/packages/rh_ros/test/test_reset_guard.py b/packages/rh_ros/test/test_reset_guard.py new file mode 100644 index 0000000..2f06656 --- /dev/null +++ b/packages/rh_ros/test/test_reset_guard.py @@ -0,0 +1,99 @@ +from concurrent.futures import ThreadPoolExecutor +from threading import Barrier, Lock + +import pytest + +from rh_ros import ( + IdempotentResetGuard, + InvalidProtocolValueError, + ResetRequestConflictError, +) + + +def test_duplicate_reset_replays_result_without_reexecution() -> None: + guard: IdempotentResetGuard[tuple[str, str], object] = IdempotentResetGuard() + calls = 0 + expected = object() + + def reset() -> object: + nonlocal calls + calls += 1 + return expected + + first = guard.execute("request-1", ("experiment", "episode"), reset) + duplicate = guard.execute("request-1", ("experiment", "episode"), reset) + + assert first is expected + assert duplicate is expected + assert calls == 1 + + +def test_concurrent_duplicates_execute_only_once() -> None: + guard: IdempotentResetGuard[tuple[str, str], int] = IdempotentResetGuard() + barrier = Barrier(5) + call_lock = Lock() + calls = 0 + + def invoke() -> int: + barrier.wait() + + def reset() -> int: + nonlocal calls + with call_lock: + calls += 1 + return 42 + + return guard.execute("request-1", ("experiment", "episode"), reset) + + with ThreadPoolExecutor(max_workers=5) as executor: + results = list(executor.map(lambda _: invoke(), range(5))) + + assert results == [42] * 5 + assert calls == 1 + + +def test_request_id_reuse_with_different_content_is_rejected() -> None: + guard: IdempotentResetGuard[tuple[str, str], bool] = IdempotentResetGuard() + guard.execute("request-1", ("experiment", "episode-1"), lambda: True) + + with pytest.raises(ResetRequestConflictError): + guard.execute("request-1", ("experiment", "episode-2"), lambda: True) + + +def test_operation_failure_is_replayed_without_an_unsafe_retry() -> None: + guard: IdempotentResetGuard[str, bool] = IdempotentResetGuard() + calls = 0 + + def reset() -> bool: + nonlocal calls + calls += 1 + raise RuntimeError("backend failed after reset began") + + with pytest.raises(RuntimeError, match="backend failed"): + guard.execute("request-1", "fingerprint", reset) + with pytest.raises(RuntimeError, match="backend failed"): + guard.execute("request-1", "fingerprint", reset) + + assert calls == 1 + + +def test_capacity_is_bounded_and_old_completed_ids_can_expire() -> None: + guard: IdempotentResetGuard[str, int] = IdempotentResetGuard(capacity=2) + calls = 0 + + def reset() -> int: + nonlocal calls + calls += 1 + return calls + + guard.execute("request-1", "same", reset) + guard.execute("request-2", "same", reset) + guard.execute("request-3", "same", reset) + + assert guard.execute("request-1", "same", reset) == 4 + + +@pytest.mark.parametrize("capacity", [0, -1, True]) +def test_invalid_capacity_is_rejected(capacity: int) -> None: + with pytest.raises(InvalidProtocolValueError): + IdempotentResetGuard(capacity=capacity) diff --git a/packages/rh_ros/test/test_sequence.py b/packages/rh_ros/test/test_sequence.py new file mode 100644 index 0000000..09b2b5a --- /dev/null +++ b/packages/rh_ros/test/test_sequence.py @@ -0,0 +1,47 @@ +import pytest +from rh_interfaces.msg import EpisodeState + +from rh_ros import EpisodeSequenceGuard, InvalidProtocolValueError + + +def _message(experiment_id: str, episode_id: str, sequence: int) -> EpisodeState: + message = EpisodeState() + message.experiment_id = experiment_id + message.episode_id = episode_id + message.sequence = sequence + return message + + +def test_sequence_guard_requires_explicit_episode_activation() -> None: + guard = EpisodeSequenceGuard() + + assert not guard.accept(_message("experiment", "episode-1", 0)) + + +def test_sequence_guard_rejects_wrong_episode_and_non_increasing_sequence() -> None: + guard = EpisodeSequenceGuard() + guard.activate("experiment", "episode-1") + + assert guard.accept(_message("experiment", "episode-1", 0)) + assert not guard.accept(_message("experiment", "episode-1", 0)) + assert not guard.accept(_message("experiment", "episode-1", 0)) + assert not guard.accept(_message("experiment", "episode-2", 1)) + assert not guard.accept(_message("old-experiment", "episode-1", 1)) + assert guard.accept(_message("experiment", "episode-1", 2)) + + +def test_activating_new_opaque_episode_resets_sequence_tracking() -> None: + guard = EpisodeSequenceGuard() + guard.activate("experiment", "z-last") + assert guard.accept(_message("experiment", "z-last", 9)) + + guard.activate("experiment", "a-next") + + assert guard.last_sequence is None + assert guard.accept(_message("experiment", "a-next", 0)) + + +@pytest.mark.parametrize(("experiment_id", "episode_id"), [("", "episode"), ("exp", " ")]) +def test_activation_rejects_empty_identifiers(experiment_id: str, episode_id: str) -> None: + with pytest.raises(InvalidProtocolValueError): + EpisodeSequenceGuard().activate(experiment_id, episode_id) diff --git a/packages/rh_ros/test/test_service.py b/packages/rh_ros/test/test_service.py new file mode 100644 index 0000000..6f72302 --- /dev/null +++ b/packages/rh_ros/test/test_service.py @@ -0,0 +1,96 @@ +from concurrent.futures import Future +from threading import Timer +from typing import Any + +import pytest + +from rh_ros import ( + InvalidProtocolValueError, + ServiceCallError, + ServiceCallTimeoutError, + ServiceDiscoveryTimeoutError, + call_service_with_deadline, +) + + +class FakeClient: + def __init__(self, *, discovered: bool, future: Future[Any] | None = None) -> None: + self.discovered = discovered + self.future = future or Future() + self.discovery_timeout: float | None = None + + def wait_for_service(self, *, timeout_sec: float) -> bool: + self.discovery_timeout = timeout_sec + return self.discovered + + def call_async(self, request: object) -> Future[Any]: + return self.future + + +def test_service_discovery_timeout_is_distinct() -> None: + client = FakeClient(discovered=False) + + with pytest.raises(ServiceDiscoveryTimeoutError): + call_service_with_deadline( + client, + object(), + discovery_timeout_s=0.01, + call_timeout_s=0.01, + ) + + assert client.discovery_timeout == 0.01 + + +def test_service_call_timeout_cancels_future() -> None: + future: Future[object] = Future() + client = FakeClient(discovered=True, future=future) + + with pytest.raises(ServiceCallTimeoutError): + call_service_with_deadline( + client, + object(), + discovery_timeout_s=0.01, + call_timeout_s=0.01, + ) + + assert future.cancelled() + + +def test_completed_service_response_is_returned() -> None: + future: Future[str] = Future() + Timer(0.01, lambda: future.set_result("response")).start() + + result = call_service_with_deadline( + FakeClient(discovered=True, future=future), + object(), + discovery_timeout_s=0.1, + call_timeout_s=0.2, + ) + + assert result == "response" + + +def test_service_future_exception_is_structured() -> None: + future: Future[object] = Future() + future.set_exception(RuntimeError("server exploded")) + + with pytest.raises(ServiceCallError) as captured: + call_service_with_deadline( + FakeClient(discovered=True, future=future), + object(), + discovery_timeout_s=0.1, + call_timeout_s=0.1, + ) + + assert isinstance(captured.value.__cause__, RuntimeError) + + +@pytest.mark.parametrize("timeout", [0.0, -1.0, float("nan"), True]) +def test_invalid_deadline_is_rejected(timeout: float) -> None: + with pytest.raises(InvalidProtocolValueError): + call_service_with_deadline( + FakeClient(discovered=True), + object(), + discovery_timeout_s=timeout, + call_timeout_s=1.0, + ) diff --git a/packages/rh_ros/test/test_status.py b/packages/rh_ros/test/test_status.py new file mode 100644 index 0000000..1bc72b5 --- /dev/null +++ b/packages/rh_ros/test/test_status.py @@ -0,0 +1,105 @@ +from __future__ import annotations + +import time +from uuid import uuid4 + +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rh_interfaces.msg import ComponentStatus + +from rh_ros import StatusMonitor, StatusPublisher, StatusTracker + + +class FakeSteadyClock: + def __init__(self) -> None: + self.now = 0.0 + + def __call__(self) -> float: + return self.now + + +def _status(component_id: str, state: int) -> ComponentStatus: + message = ComponentStatus() + message.component_id = component_id + message.state = state + return message + + +def test_tracker_detects_missing_and_stale_heartbeats() -> None: + clock = FakeSteadyClock() + tracker = StatusTracker(stale_timeout_s=5.0, monotonic=clock) + + assert tracker.is_stale("env") + assert tracker.update(_status("env", ComponentStatus.READY)) + assert tracker.is_ready("env") + + clock.now = 5.0 + assert not tracker.is_stale("env") + clock.now = 5.001 + assert tracker.is_stale("env") + assert not tracker.is_ready("env") + + +def test_tracker_ignores_malformed_status() -> None: + tracker = StatusTracker(stale_timeout_s=5.0) + + assert not tracker.update(_status("", ComponentStatus.READY)) + assert not tracker.update(_status("env", 255)) + assert tracker.latest("env") is None + + +@pytest.fixture +def ros_context() -> None: + rclpy.init() + try: + yield + finally: + rclpy.shutdown() + + +def test_late_join_monitor_receives_retained_status_and_transition_stamp_is_stable( + ros_context: None, +) -> None: + topic = f"/roboharness/test/status_{uuid4().hex}" + publisher_node = rclpy.create_node("status_publisher_test") + monitor_node = rclpy.create_node("status_monitor_test") + executor = SingleThreadedExecutor() + try: + publisher = StatusPublisher( + publisher_node, + topic, + "env", + heartbeat_period_s=0.05, + ) + publisher.transition(ComponentStatus.READY, detail="ready") + transition_stamp = ( + publisher.message.stamp.sec, + publisher.message.stamp.nanosec, + ) + + # The subscription is intentionally created after the transition publish. + monitor = StatusMonitor(monitor_node, topic, stale_timeout_s=1.0) + executor.add_node(publisher_node) + executor.add_node(monitor_node) + deadline = time.monotonic() + 5.0 + while monitor.tracker.latest("env") is None and time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.05) + + received = monitor.tracker.latest("env") + assert received is not None + assert received.state == ComponentStatus.READY + assert (received.stamp.sec, received.stamp.nanosec) == transition_stamp + + # Let a heartbeat fire; it republishes rather than committing a transition. + heartbeat_deadline = time.monotonic() + 0.2 + while time.monotonic() < heartbeat_deadline: + executor.spin_once(timeout_sec=0.05) + assert ( + publisher.message.stamp.sec, + publisher.message.stamp.nanosec, + ) == transition_stamp + finally: + executor.shutdown() + publisher_node.destroy_node() + monitor_node.destroy_node() diff --git a/pyproject.toml b/pyproject.toml index 3f43382..29b878d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -11,7 +11,7 @@ extend-exclude = [".build"] select = ["E", "F", "I", "UP"] [tool.ruff.lint.isort] -known-first-party = ["rh_core"] +known-first-party = ["rh_core", "rh_ros"] [tool.pytest.ini_options] addopts = "-ra" diff --git a/tests/README.md b/tests/README.md index bbe80dc..726c009 100644 --- a/tests/README.md +++ b/tests/README.md @@ -7,3 +7,7 @@ end-to-end tests. Unit tests stay with their owning package. language bindings and package boundaries. - `fixtures/` will contain deterministic CPU-only mock components in later roadmap PRs. + +Runtime protocol behavior stays with the owning `packages/rh_ros` package, +including its real ROS 2 late-join transport test. Later cross-component tests +belong under `integration_mock` rather than duplicating package-level coverage.