feat(runtime): add reusable ROS protocol helpers

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.

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.

packages/rh_ros/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rh_ros</name>
+  <version>0.0.0</version>
+  <description>Reusable ROS 2 runtime protocol helpers for RoboHarness.</description>
+  <maintainer email="2300012435@stu.pku.edu.cn">Staaaaaaaaar</maintainer>
+  <!-- Project licensing and release versioning are finalized by roadmap PR 18. -->
+  <license>NOASSERTION</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>rh_core</depend>
+  <depend>rh_interfaces</depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

packages/rh_ros/resource/rh_ros

@@ -0,0 +1 @@
+

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",
+]

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

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"

packages/rh_ros/rh_ros/py.typed

@@ -0,0 +1 @@
+