From d886d9b11d89ee3d6b30118277b1bde12a6bf58b Mon Sep 17 00:00:00 2001 From: Staaaaaaaaar <2300012435@stu.pku.edu.cn> Date: Mon, 17 Aug 2026 15:23:55 +0800 Subject: [PATCH] feat(core): implement validated experiment domain models --- Makefile | 2 +- configs/README.md | 8 +- configs/experiments/mvp.yaml | 25 + docs/architecture-and-development-plan.md | 32 +- packages/README.md | 9 +- packages/rh_core/README.md | 99 ++++ packages/rh_core/package.xml | 20 + packages/rh_core/resource/rh_core | 1 + packages/rh_core/rh_core/__init__.py | 41 ++ packages/rh_core/rh_core/config.py | 459 ++++++++++++++++++ packages/rh_core/rh_core/errors.py | 65 +++ packages/rh_core/rh_core/lifecycle.py | 197 ++++++++ packages/rh_core/rh_core/models.py | 107 ++++ packages/rh_core/rh_core/py.typed | 1 + packages/rh_core/rh_core/termination.py | 38 ++ packages/rh_core/setup.cfg | 5 + packages/rh_core/setup.py | 21 + packages/rh_core/test/conftest.py | 38 ++ packages/rh_core/test/test_config.py | 212 ++++++++ packages/rh_core/test/test_lifecycle.py | 177 +++++++ packages/rh_core/test/test_termination.py | 39 ++ packages/rh_interfaces/README.md | 11 +- packages/rh_interfaces/msg/PointNavTask.msg | 11 +- packages/rh_interfaces/srv/ResetEnv.srv | 5 +- pyproject.toml | 3 + tests/contracts/README.md | 5 + .../rh_core_contract_tests/CMakeLists.txt | 11 + .../rh_core_contract_tests/package.xml | 21 + .../test/test_core_interface_alignment.py | 136 ++++++ .../test/test_cpp_typesupport.cpp | 13 +- .../test/test_python_contract.py | 5 +- 31 files changed, 1784 insertions(+), 33 deletions(-) create mode 100644 configs/experiments/mvp.yaml create mode 100644 packages/rh_core/README.md create mode 100644 packages/rh_core/package.xml create mode 100644 packages/rh_core/resource/rh_core create mode 100644 packages/rh_core/rh_core/__init__.py create mode 100644 packages/rh_core/rh_core/config.py create mode 100644 packages/rh_core/rh_core/errors.py create mode 100644 packages/rh_core/rh_core/lifecycle.py create mode 100644 packages/rh_core/rh_core/models.py create mode 100644 packages/rh_core/rh_core/py.typed create mode 100644 packages/rh_core/rh_core/termination.py create mode 100644 packages/rh_core/setup.cfg create mode 100644 packages/rh_core/setup.py create mode 100644 packages/rh_core/test/conftest.py create mode 100644 packages/rh_core/test/test_config.py create mode 100644 packages/rh_core/test/test_lifecycle.py create mode 100644 packages/rh_core/test/test_termination.py create mode 100644 tests/contracts/rh_core_contract_tests/CMakeLists.txt create mode 100644 tests/contracts/rh_core_contract_tests/package.xml create mode 100644 tests/contracts/rh_core_contract_tests/test/test_core_interface_alignment.py diff --git a/Makefile b/Makefile index 8b2ba76..15fcf95 100644 --- a/Makefile +++ b/Makefile @@ -57,6 +57,6 @@ test-local: build-local lint-local: python3 tools/validation/check_repository.py - ruff check tools tests + ruff check tools tests packages check-local: lint-local list-local test-local diff --git a/configs/README.md b/configs/README.md index 1096310..3e386d2 100644 --- a/configs/README.md +++ b/configs/README.md @@ -1,5 +1,9 @@ # Configuration Versioned experiment, scenario, agent, task, and simulator configuration belongs -in this domain. Configuration schemas and the MVP examples are introduced with -the components that can validate them; PR 01 does not add speculative examples. +in this domain. [`experiments/mvp.yaml`](experiments/mvp.yaml) is the canonical +schema-version 1 PointNav example validated by `rh_core`. + +Episode initialization uses a complete 3D pose (`x/y/z` in metres and +`roll/pitch/yaw` in radians). PointNav goals are 3D positions without target +orientation semantics. diff --git a/configs/experiments/mvp.yaml b/configs/experiments/mvp.yaml new file mode 100644 index 0000000..ef985da --- /dev/null +++ b/configs/experiments/mvp.yaml @@ -0,0 +1,25 @@ +schema_version: 1 +experiment: + name: go2_keyboard_pointnav + execution_mode: manual + episodes: + - episode_id: "0000" + scenario: warehouse_default + initial_pose: + frame_id: map + x: 1.0 + y: 2.0 + z: 0.4 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 + task: + type: pointnav + goal: + frame_id: map + x: 8.0 + y: 4.0 + z: 0.4 + success_radius_m: 0.5 + timeout_s: 120.0 + seed: 42 diff --git a/docs/architecture-and-development-plan.md b/docs/architecture-and-development-plan.md index ef0f718..c3d3752 100644 --- a/docs/architecture-and-development-plan.md +++ b/docs/architecture-and-development-plan.md @@ -97,7 +97,7 @@ Simulator、Robot、Agent、Task、Evaluation 均通过 ROS contract、配置约 | Agent | 消费标准观测、执行策略、输出命令、agent reset、任务局部状态；MVP 处理键盘输入 | simulator reset、spawn、Episode 调度、评估和持久化 | | Experiment container | control/evaluation plane 的部署边界 | 高频导航数据转发、simulator 或 policy 实现 | | Experiment Orchestrator | Experiment/Episode 权威状态、readiness 等待、reset 顺序、task 分配、start/abort、终止协调、失败策略 | 机器人实时控制、指标公式细节 | -| Task Manager | 加载并校验 EpisodeSpec、构造 PointNav task、start/goal/timeout/success 条件 | 控制 robot、判定组件 readiness、保存结果 | +| Task Manager | 加载并校验 EpisodeSpec、构造 PointNav goal/timeout/success 条件 | 控制 robot、判定组件 readiness、保存结果 | | Evaluator | 订阅 ground truth/episode/task，维护轨迹并计算 metrics、提出终止候选 | 发布 `cmd_vel`、改变 world、决定调度策略 | | Result Recorder | 原子写入 config、metadata、episode spec、events、trajectory、metrics 和 summary | 计算控制或拥有生命周期状态 | | Simulator Backend | Isaac app/extension、timeline、stage、physics、world、原生 ROS 2 Bridge 配置与 backend readiness | Agent、Task/Eval、跨 simulator 的机器人声明 | @@ -218,8 +218,8 @@ Core contract 由 ROS interfaces、配置 schema 和行为测试共同定义；P - `ComponentStatus.msg`：`builtin_interfaces/Time stamp`、`string component_id`、`uint8 state`、`uint32 error_code`、`string detail`、`bool restart_required`；常量 `STARTING/RESETTING/READY/ERROR`。 - `EpisodeState.msg`：`stamp`、`experiment_id`、`episode_id`、`uint64 sequence`、`uint8 state`、`uint8 termination_reason`、`string detail`；定义生命周期和终止原因常量。 -- `PointNavTask.msg`：`experiment_id`、`episode_id`、`geometry_msgs/PoseStamped start`、`goal`、`float64 success_radius_m`、`float64 timeout_s`、`int64 seed`。frame 必须相同且 MVP 为 `map`。 -- `ResetEnv.srv` request：`request_id`、`experiment_id`、`episode_id`、`start`、`seed`；response：`success`、`error_code`、`detail`。 +- `PointNavTask.msg`：`experiment_id`、`episode_id`、`geometry_msgs/PointStamped goal`、`float64 success_radius_m`、`float64 timeout_s`、`int64 seed`。PointNav goal 是 `map` frame 中的 3D position，不表达目标朝向。 +- `ResetEnv.srv` request：`request_id`、`experiment_id`、`episode_id`、`geometry_msgs/PoseStamped initial_pose`、`seed`；initial pose 完整表达 3D position 和 orientation，四元数必须有限且归一化；response：`success`、`error_code`、`detail`。 - `ResetAgent.srv` request：`request_id`、`experiment_id`、`episode_id`；response 同上。 - `StartEpisode.srv` request：`experiment_id`、`episode_id`；response：`accepted`、`detail`。 - `AbortEpisode.srv` request：IDs、`reason`；response：`accepted`、`detail`。 @@ -256,7 +256,8 @@ Experiment └─ ordered Episode instances ├─ immutable EpisodeSpec │ ├─ Scenario/world reference - │ ├─ Task(type=pointnav, start, goal) + │ ├─ Initial pose(x, y, z, roll, pitch, yaw) + │ ├─ Task(type=pointnav, 3D goal) │ ├─ timeout/success radius │ └─ seed ├─ lifecycle state @@ -280,16 +281,23 @@ experiment: episodes: - episode_id: "0000" scenario: warehouse_default + initial_pose: + frame_id: map + x: 1.0 + y: 2.0 + z: 0.4 + roll: 0.0 + pitch: 0.0 + yaw: 0.0 task: type: pointnav - start: {frame_id: map, x: 1.0, y: 2.0, yaw: 0.0} - goal: {frame_id: map, x: 8.0, y: 4.0, yaw: 0.0} + goal: {frame_id: map, x: 8.0, y: 4.0, z: 0.4} success_radius_m: 0.5 timeout_s: 120.0 seed: 42 ``` -配置加载后转换为 typed dataclass，并一次性完成不依赖环境的静态验证：唯一 ID、有限数值、正 timeout/radius 和 frame 一致。静态无效配置直接拒绝启动 Experiment；依赖已加载 world 的检查（例如 start/goal 是否处于有效区域）在对应 Episode 的 PREPARING 阶段执行，失败时以 `INVALID_TASK` 结束该局且不允许机器人运动。 +配置加载后转换为 typed dataclass，并一次性完成不依赖环境的静态验证：唯一 ID、完整且有限的 initial pose、有限的 3D goal、正 timeout/radius 和 frame 一致。配置中的 position 单位为米，roll/pitch/yaw 单位为弧度；RPY 采用绕固定 X/Y/Z 轴的旋转，等价旋转矩阵为 `Rz(yaw) * Ry(pitch) * Rx(roll)`。二维数据源必须在适配层显式补齐缺失的 `z/roll/pitch`（通常置零），再转换成完整 pose。静态无效配置直接拒绝启动 Experiment；依赖已加载 world 的检查（例如 initial pose/goal 是否处于有效区域）在对应 Episode 的 PREPARING 阶段执行，失败时以 `INVALID_TASK` 结束该局且不允许机器人运动。 --- @@ -635,16 +643,16 @@ Env 只有在 Isaac、stage、Go2、physics、ROS bridge、clock、required topi ## Part XIII — PointNav MVP -PointNav EpisodeSpec 必须包含唯一 ID、`map` frame 中的 start/goal、positive success radius、positive timeout、seed 和 execution mode。yaw 用于初始姿态，MVP success 只判断平面位置距离，不要求目标朝向。 +EpisodeSpec 必须包含唯一 ID、`map` frame 中的完整 3D initial pose、PointNav 3D goal、positive success radius、positive timeout、seed 和 execution mode。initial pose 的 orientation 属于环境初始化条件，不属于 PointNav 目标语义；PointNav 不包含目标 orientation，MVP success 判断机器人 tracking point（默认 `base_link` 原点）到 goal 的 3D 欧氏距离。未来若需要目标朝向，应新增语义明确的 PoseNav task/model/interface，而不是重新解释 PointNav 字段。 1. Task Manager 在运动前验证 schema 和有限数值。 -2. Env reset 到 start 并确认 robot 静止；Agent reset；task 直接发布到 Env/Agent/Evaluator。 +2. Env reset 到 initial pose 并确认 robot 静止；Agent reset；task 直接发布到 Env/Agent/Evaluator。 3. Episode 进入 READY，manual 模式等待 `/episode/start`。 4. RUNNING 后 Keyboard Agent 才能驱动；Evaluator 计算 goal distance。 5. 距离首次 `<= success_radius_m` 为 `SUCCESS`；simulation elapsed `>= timeout_s` 为 `TIMEOUT`。 6. 同时发生时优先级为 runtime safety error、user abort、success、timeout；最终 reason 只提交一次，其他候选作为事件保存。 -MVP 不包含 SPL、语义目标、动态场景、复杂碰撞惩罚或 start/goal 自动采样。 +MVP 不包含 SPL、语义目标、动态场景、复杂碰撞惩罚或 initial pose/goal 自动采样。 --- @@ -733,7 +741,7 @@ CPU CI 至少执行 `colcon build`、lint/type checks、unit/interface tests、m **Out of Scope:** physics realism、Gazebo、Isaac compatibility layer。 **Files / Modules:** `tests/fixtures/mock_env/`、integration tests。 **ROS Interfaces:** Env status/reset、Episode state、`/clock`、`/robot/cmd_vel`、odom/TF。 -**Tests:** readiness、idempotent reset、start pose、non-RUNNING zero gate、freeze/reset failure injection。 +**Tests:** readiness、idempotent reset、完整 initial pose、non-RUNNING zero gate、freeze/reset failure injection。 **Acceptance Criteria:** headless 测试 < 30 s 且 deterministic；fixture 明确标注不可作为 simulator 产品实现。 **Dependencies:** PR 04。 **Risks:** mock 与真实 contract 偏离；接口测试共享同一 black-box suite。 @@ -772,7 +780,7 @@ CPU CI 至少执行 `colcon build`、lint/type checks、unit/interface tests、m **Out of Scope:** evaluator aggregation、自动 goal sampling、其他 Task。 **Files / Modules:** `tasks/pointnav/`、configs、tests。 **ROS Interfaces:** publish `PointNavTask`，验证 direct subscribers 和 transient-local behavior。 -**Tests:** start/goal/frame/radius/timeout、late subscriber、episode mismatch。 +**Tests:** 3D goal/frame/radius/timeout、late subscriber、episode mismatch。 **Acceptance Criteria:** Env/Agent/Evaluator 可直接获得同一不可变 task；非法 goal 在运动前拒绝。 **Dependencies:** PR 03、PR 04。 **Risks:** Task 与 Eval 耦合；只共享 typed spec/termination predicate，不共享 recorder。 diff --git a/packages/README.md b/packages/README.md index 3aeaf3d..07fb504 100644 --- a/packages/README.md +++ b/packages/README.md @@ -4,6 +4,9 @@ This domain contains shared RoboHarness platform and communication packages. Packages must depend toward stable layers and must not statically depend on a specific simulator, robot, agent, task, or evaluator implementation. -The first platform package is [`rh_interfaces`](rh_interfaces/README.md), which -owns the implementation-independent ROS wire contract. `rh_core`, `rh_ros`, -and `rh_experiment` are introduced by their roadmap PRs. +- [`rh_interfaces`](rh_interfaces/README.md) owns the implementation-independent + ROS wire contract. +- [`rh_core`](rh_core/README.md) owns ROS-independent configuration, domain + models, lifecycle rules, and termination policy. + +`rh_ros` and `rh_experiment` are introduced by their roadmap PRs. diff --git a/packages/rh_core/README.md b/packages/rh_core/README.md new file mode 100644 index 0000000..00ff8e9 --- /dev/null +++ b/packages/rh_core/README.md @@ -0,0 +1,99 @@ +# `rh_core` + +`rh_core` is the ROS-independent domain layer of RoboHarness. It defines what +an Experiment means, which static configuration is valid, which lifecycle +transitions are legal, and how competing termination candidates are resolved. +It does not communicate with ROS or execute an Experiment. + +## Responsibilities + +- Immutable typed models for versioned Experiment, Episode, and PointNav input. +- Strict YAML decoding with unknown-field and duplicate-key rejection. +- Static validation that does not require a loaded simulator world. +- Pure Experiment and Episode lifecycle transition guards. +- Deterministic Episode termination priority. +- Structured errors containing a machine-readable code, field path, and + human-readable message. + +The package must not import `rclpy`, `rh_interfaces`, Isaac Sim, or any concrete +Simulator, Agent, Task, or Evaluator implementation. ROS message conversion +belongs to the future `rh_ros` package. + +## Configuration schema version 1 + +The canonical example is +[`configs/experiments/mvp.yaml`](../../configs/experiments/mvp.yaml). A document +contains an Experiment name, execution mode, and a non-empty ordered list of +Episode specifications. Each MVP Episode references a scenario and contains an +immutable PointNav task plus a signed 64-bit reproducibility seed. + +The loader validates: + +- exact known fields and `schema_version: 1`; +- non-empty identifiers, scenario references, and Experiment names; +- unique Episode IDs; +- `manual` or `automatic` execution mode; +- PointNav as the only MVP task type; +- finite initial-pose position/orientation, goal, radius, and timeout values; +- positive radius and timeout; +- matching `map` initial-pose/goal frames; and +- an integer seed in the ROS `int64` range. + +The Episode initial state is a full 3D robot pose: position is expressed in +metres and roll, pitch, and yaw in radians. RPY means fixed-axis rotations about +X, Y, and Z, with the equivalent rotation matrix +`Rz(yaw) * Ry(pitch) * Rx(roll)`. A source that only provides planar data must +explicitly adapt the absent `z`, roll, and pitch dimensions, normally to zero. +The ROS adapter converts this representation to a normalized `PoseStamped` +quaternion. + +PointNav contains only a 3D goal position. It has no desired target orientation, +and success uses the 3D Euclidean distance from the configured robot tracking +point (MVP: the `base_link` origin) to the goal. PoseNav, if later required, will +be a separate task model and interface rather than an extension hidden inside +these PointNav fields. + +Checks that require a loaded world, such as collision-free or reachable poses, +remain an Environment/Task concern during Episode `PREPARING`. + +```python +from rh_core import ConfigError, load_experiment_config + +try: + config = load_experiment_config("configs/experiments/mvp.yaml") +except ConfigError as error: + for issue in error.issues: + print(issue.as_dict()) +``` + +## Lifecycle rules + +Experiment progression is: + +```text +CREATED -> STARTING -> RUNNING -> FINALIZING -> FINISHED + \---------- errors ----------> FAILED +``` + +Episode progression is: + +```text +PREPARING -> READY -> RUNNING -> TERMINATING -> FINISHED + | | | + +----------+---------+---- early termination +``` + +Entering `TERMINATING` requires a non-`NONE` reason. The reason is committed +once, retained through `FINISHED`, and cannot be replaced. Every accepted +Episode transition returns a new immutable snapshot with `sequence + 1`. + +Termination candidates use this priority: + +```text +ENV_ERROR > AGENT_ERROR > FAILURE > INVALID_TASK + > ABORTED > SUCCESS > TIMEOUT +``` + +This refines the architectural rule “runtime safety error, user abort, success, +timeout” with deterministic ordering inside the failure tier. The Environment +wins ties because it is the final motion-safety boundary. diff --git a/packages/rh_core/package.xml b/packages/rh_core/package.xml new file mode 100644 index 0000000..bd302fa --- /dev/null +++ b/packages/rh_core/package.xml @@ -0,0 +1,20 @@ + + + + rh_core + 0.0.0 + ROS-independent domain models and rules for RoboHarness. + Staaaaaaaaar + + NOASSERTION + + ament_python + + python3-yaml + + python3-pytest + + + ament_python + + diff --git a/packages/rh_core/resource/rh_core b/packages/rh_core/resource/rh_core new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/packages/rh_core/resource/rh_core @@ -0,0 +1 @@ + diff --git a/packages/rh_core/rh_core/__init__.py b/packages/rh_core/rh_core/__init__.py new file mode 100644 index 0000000..2115439 --- /dev/null +++ b/packages/rh_core/rh_core/__init__.py @@ -0,0 +1,41 @@ +"""ROS-independent RoboHarness domain models and rules.""" + +from rh_core.config import load_experiment_config, parse_experiment_config +from rh_core.errors import ConfigError, ErrorCode, LifecycleError, ValidationIssue +from rh_core.lifecycle import EpisodeLifecycle, ExperimentLifecycle +from rh_core.models import ( + EpisodeSpec, + EpisodeState, + ExecutionMode, + ExperimentConfig, + ExperimentSpec, + ExperimentState, + Point3D, + PointNavTaskSpec, + Pose3D, + TerminationReason, +) +from rh_core.termination import TERMINATION_PRIORITY, resolve_termination_reason + +__all__ = [ + "ConfigError", + "EpisodeLifecycle", + "EpisodeSpec", + "EpisodeState", + "ErrorCode", + "ExecutionMode", + "ExperimentConfig", + "ExperimentLifecycle", + "ExperimentSpec", + "ExperimentState", + "LifecycleError", + "Point3D", + "PointNavTaskSpec", + "Pose3D", + "TERMINATION_PRIORITY", + "TerminationReason", + "ValidationIssue", + "load_experiment_config", + "parse_experiment_config", + "resolve_termination_reason", +] diff --git a/packages/rh_core/rh_core/config.py b/packages/rh_core/rh_core/config.py new file mode 100644 index 0000000..9289465 --- /dev/null +++ b/packages/rh_core/rh_core/config.py @@ -0,0 +1,459 @@ +"""Strict YAML loading and static validation for Experiment configuration.""" + +from __future__ import annotations + +import math +from collections.abc import Mapping +from pathlib import Path +from typing import Any + +import yaml +from yaml.nodes import MappingNode + +from rh_core.errors import ConfigError, ErrorCode, ValidationIssue +from rh_core.models import ( + EpisodeSpec, + ExecutionMode, + ExperimentConfig, + ExperimentSpec, + Point3D, + PointNavTaskSpec, + Pose3D, +) + +SUPPORTED_SCHEMA_VERSION = 1 +MVP_FRAME_ID = "map" +_INT64_MIN = -(2**63) +_INT64_MAX = 2**63 - 1 + + +class _DuplicateKeyError(yaml.YAMLError): + def __init__(self, key: object) -> None: + self.key = key + super().__init__(f"duplicate YAML mapping key: {key!r}") + + +class _UniqueKeySafeLoader(yaml.SafeLoader): + """SafeLoader variant that rejects silent mapping-key replacement.""" + + +def _construct_unique_mapping( + loader: _UniqueKeySafeLoader, + node: MappingNode, + deep: bool = False, +) -> dict[object, object]: + loader.flatten_mapping(node) + result: dict[object, object] = {} + for key_node, value_node in node.value: + key = loader.construct_object(key_node, deep=deep) + try: + duplicate = key in result + except TypeError as exc: + raise yaml.constructor.ConstructorError( + "while constructing a mapping", + node.start_mark, + "found an unhashable key", + key_node.start_mark, + ) from exc + if duplicate: + raise _DuplicateKeyError(key) + result[key] = loader.construct_object(value_node, deep=deep) + return result + + +_UniqueKeySafeLoader.add_constructor( + yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, + _construct_unique_mapping, +) + + +class _Validator: + def __init__(self) -> None: + self.issues: list[ValidationIssue] = [] + + def add(self, code: ErrorCode, path: str, message: str) -> None: + self.issues.append(ValidationIssue(code=code, path=path, message=message)) + + def mapping( + self, + value: object, + path: str, + *, + required: frozenset[str], + allowed: frozenset[str], + ) -> Mapping[str, Any] | None: + if not isinstance(value, Mapping): + self.add(ErrorCode.TYPE_MISMATCH, path, "expected a mapping") + return None + + valid_keys: set[str] = set() + for key in value: + if not isinstance(key, str): + self.add( + ErrorCode.TYPE_MISMATCH, + path, + f"mapping keys must be strings, got {type(key).__name__}", + ) + continue + valid_keys.add(key) + if key not in allowed: + self.add(ErrorCode.UNKNOWN_FIELD, f"{path}.{key}", "unknown field") + + for key in sorted(required - valid_keys): + self.add(ErrorCode.MISSING_FIELD, f"{path}.{key}", "required field is missing") + return value + + def string(self, value: object, path: str) -> str | None: + if not isinstance(value, str): + self.add(ErrorCode.TYPE_MISMATCH, path, "expected a string") + return None + if not value.strip(): + self.add(ErrorCode.INVALID_VALUE, path, "must not be empty") + return None + return value + + def number(self, value: object, path: str) -> float | None: + if isinstance(value, bool) or not isinstance(value, int | float): + self.add(ErrorCode.TYPE_MISMATCH, path, "expected a number") + return None + try: + converted = float(value) + except OverflowError: + self.add(ErrorCode.INVALID_VALUE, path, "must be finite") + return None + if not math.isfinite(converted): + self.add(ErrorCode.INVALID_VALUE, path, "must be finite") + return None + return converted + + def int64(self, value: object, path: str) -> int | None: + if isinstance(value, bool) or not isinstance(value, int): + self.add(ErrorCode.TYPE_MISMATCH, path, "expected an integer") + return None + if not _INT64_MIN <= value <= _INT64_MAX: + self.add(ErrorCode.INVALID_VALUE, path, "must fit in a signed 64-bit integer") + return None + return value + + +def _parse_point(value: object, path: str, validator: _Validator) -> Point3D | None: + issue_count = len(validator.issues) + data = validator.mapping( + value, + path, + required=frozenset({"frame_id", "x", "y", "z"}), + allowed=frozenset({"frame_id", "x", "y", "z"}), + ) + if data is None: + return None + + frame_id = ( + validator.string(data["frame_id"], f"{path}.frame_id") + if "frame_id" in data + else None + ) + x = validator.number(data["x"], f"{path}.x") if "x" in data else None + y = validator.number(data["y"], f"{path}.y") if "y" in data else None + z = validator.number(data["z"], f"{path}.z") if "z" in data else None + + if len(validator.issues) != issue_count: + return None + assert frame_id is not None and x is not None and y is not None and z is not None + return Point3D(frame_id=frame_id, x=x, y=y, z=z) + + +def _parse_pose(value: object, path: str, validator: _Validator) -> Pose3D | None: + issue_count = len(validator.issues) + fields = frozenset({"frame_id", "x", "y", "z", "roll", "pitch", "yaw"}) + data = validator.mapping(value, path, required=fields, allowed=fields) + if data is None: + return None + + frame_id = ( + validator.string(data["frame_id"], f"{path}.frame_id") + if "frame_id" in data + else None + ) + x = validator.number(data["x"], f"{path}.x") if "x" in data else None + y = validator.number(data["y"], f"{path}.y") if "y" in data else None + z = validator.number(data["z"], f"{path}.z") if "z" in data else None + roll = validator.number(data["roll"], f"{path}.roll") if "roll" in data else None + pitch = ( + validator.number(data["pitch"], f"{path}.pitch") if "pitch" in data else None + ) + yaw = validator.number(data["yaw"], f"{path}.yaw") if "yaw" in data else None + + if len(validator.issues) != issue_count: + return None + assert ( + frame_id is not None + and x is not None + and y is not None + and z is not None + and roll is not None + and pitch is not None + and yaw is not None + ) + return Pose3D( + frame_id=frame_id, + x=x, + y=y, + z=z, + roll=roll, + pitch=pitch, + yaw=yaw, + ) + + +def _parse_task( + value: object, + path: str, + validator: _Validator, +) -> PointNavTaskSpec | None: + issue_count = len(validator.issues) + fields = frozenset({"type", "goal", "success_radius_m", "timeout_s"}) + data = validator.mapping(value, path, required=fields, allowed=fields) + if data is None: + return None + + task_type = validator.string(data["type"], f"{path}.type") if "type" in data else None + if task_type is not None and task_type != "pointnav": + validator.add(ErrorCode.INVALID_VALUE, f"{path}.type", "only 'pointnav' is supported") + + goal = _parse_point(data["goal"], f"{path}.goal", validator) if "goal" in data else None + radius = ( + validator.number(data["success_radius_m"], f"{path}.success_radius_m") + if "success_radius_m" in data + else None + ) + timeout = ( + validator.number(data["timeout_s"], f"{path}.timeout_s") + if "timeout_s" in data + else None + ) + + if radius is not None and radius <= 0.0: + validator.add( + ErrorCode.INVALID_VALUE, + f"{path}.success_radius_m", + "must be greater than zero", + ) + if timeout is not None and timeout <= 0.0: + validator.add(ErrorCode.INVALID_VALUE, f"{path}.timeout_s", "must be greater than zero") + + if goal is not None: + if goal.frame_id != MVP_FRAME_ID: + validator.add( + ErrorCode.UNSUPPORTED_FRAME, + f"{path}.goal.frame_id", + f"MVP requires frame_id '{MVP_FRAME_ID}'", + ) + + if len(validator.issues) != issue_count: + return None + assert goal is not None and radius is not None and timeout is not None + return PointNavTaskSpec( + goal=goal, + success_radius_m=radius, + timeout_s=timeout, + ) + + +def _parse_episode(value: object, index: int, validator: _Validator) -> EpisodeSpec | None: + path = f"experiment.episodes[{index}]" + issue_count = len(validator.issues) + fields = frozenset({"episode_id", "scenario", "initial_pose", "task", "seed"}) + data = validator.mapping(value, path, required=fields, allowed=fields) + if data is None: + return None + + episode_id = ( + validator.string(data["episode_id"], f"{path}.episode_id") + if "episode_id" in data + else None + ) + scenario = ( + validator.string(data["scenario"], f"{path}.scenario") + if "scenario" in data + else None + ) + initial_pose = ( + _parse_pose(data["initial_pose"], f"{path}.initial_pose", validator) + if "initial_pose" in data + else None + ) + task = _parse_task(data["task"], f"{path}.task", validator) if "task" in data else None + seed = validator.int64(data["seed"], f"{path}.seed") if "seed" in data else None + + if len(validator.issues) != issue_count: + return None + if initial_pose is not None: + if initial_pose.frame_id != MVP_FRAME_ID: + validator.add( + ErrorCode.UNSUPPORTED_FRAME, + f"{path}.initial_pose.frame_id", + f"MVP requires frame_id '{MVP_FRAME_ID}'", + ) + if task is not None and initial_pose.frame_id != task.goal.frame_id: + validator.add( + ErrorCode.FRAME_MISMATCH, + path, + "initial_pose and task goal frame_id values must match", + ) + + if len(validator.issues) != issue_count: + return None + assert ( + episode_id is not None + and scenario is not None + and initial_pose is not None + and task is not None + and seed is not None + ) + return EpisodeSpec( + episode_id=episode_id, + scenario=scenario, + initial_pose=initial_pose, + task=task, + seed=seed, + ) + + +def _parse_experiment(value: object, validator: _Validator) -> ExperimentSpec | None: + path = "experiment" + issue_count = len(validator.issues) + fields = frozenset({"name", "execution_mode", "episodes"}) + data = validator.mapping(value, path, required=fields, allowed=fields) + if data is None: + return None + + name = validator.string(data["name"], f"{path}.name") if "name" in data else None + + mode_value = ( + validator.string(data["execution_mode"], f"{path}.execution_mode") + if "execution_mode" in data + else None + ) + mode: ExecutionMode | None = None + if mode_value is not None: + try: + mode = ExecutionMode(mode_value) + except ValueError: + validator.add( + ErrorCode.INVALID_VALUE, + f"{path}.execution_mode", + "must be 'manual' or 'automatic'", + ) + + indexed_episodes: list[tuple[int, EpisodeSpec]] = [] + raw_episodes = data.get("episodes") + if "episodes" in data: + if not isinstance(raw_episodes, list): + validator.add(ErrorCode.TYPE_MISMATCH, f"{path}.episodes", "expected a list") + elif not raw_episodes: + validator.add(ErrorCode.INVALID_VALUE, f"{path}.episodes", "must not be empty") + else: + for index, raw_episode in enumerate(raw_episodes): + episode = _parse_episode(raw_episode, index, validator) + if episode is not None: + indexed_episodes.append((index, episode)) + + first_index_by_id: dict[str, int] = {} + for source_index, episode in indexed_episodes: + first_index = first_index_by_id.setdefault(episode.episode_id, source_index) + if first_index != source_index: + validator.add( + ErrorCode.DUPLICATE_EPISODE_ID, + f"{path}.episodes[{source_index}].episode_id", + f"duplicates experiment.episodes[{first_index}].episode_id", + ) + + if len(validator.issues) != issue_count: + return None + assert name is not None and mode is not None and indexed_episodes + episodes = tuple(episode for _, episode in indexed_episodes) + return ExperimentSpec(name=name, execution_mode=mode, episodes=episodes) + + +def parse_experiment_config(document: object) -> ExperimentConfig: + """Validate a decoded YAML document and return immutable typed models.""" + + validator = _Validator() + fields = frozenset({"schema_version", "experiment"}) + root = validator.mapping(document, "$", required=fields, allowed=fields) + if root is None: + raise ConfigError(validator.issues) + + schema_version = ( + validator.int64(root["schema_version"], "schema_version") + if "schema_version" in root + else None + ) + if schema_version is not None and schema_version != SUPPORTED_SCHEMA_VERSION: + validator.add( + ErrorCode.UNSUPPORTED_SCHEMA_VERSION, + "schema_version", + f"supported version is {SUPPORTED_SCHEMA_VERSION}", + ) + + experiment = ( + _parse_experiment(root["experiment"], validator) if "experiment" in root else None + ) + + if validator.issues: + raise ConfigError(validator.issues) + assert schema_version is not None and experiment is not None + return ExperimentConfig(schema_version=schema_version, experiment=experiment) + + +def load_experiment_config(path: str | Path) -> ExperimentConfig: + """Load one UTF-8 YAML file and return a validated immutable configuration.""" + + config_path = Path(path) + try: + text = config_path.read_text(encoding="utf-8") + except FileNotFoundError as exc: + raise ConfigError( + ( + ValidationIssue( + code=ErrorCode.FILE_NOT_FOUND, + path=str(config_path), + message="configuration file does not exist", + ), + ) + ) from exc + except OSError as exc: + raise ConfigError( + ( + ValidationIssue( + code=ErrorCode.FILE_READ_ERROR, + path=str(config_path), + message=str(exc), + ), + ) + ) from exc + + try: + document = yaml.load(text, Loader=_UniqueKeySafeLoader) + except _DuplicateKeyError as exc: + raise ConfigError( + ( + ValidationIssue( + code=ErrorCode.YAML_DUPLICATE_KEY, + path=str(config_path), + message=f"duplicate mapping key {exc.key!r}", + ), + ) + ) from exc + except yaml.YAMLError as exc: + raise ConfigError( + ( + ValidationIssue( + code=ErrorCode.YAML_SYNTAX, + path=str(config_path), + message=str(exc), + ), + ) + ) from exc + + return parse_experiment_config(document) diff --git a/packages/rh_core/rh_core/errors.py b/packages/rh_core/rh_core/errors.py new file mode 100644 index 0000000..00346fd --- /dev/null +++ b/packages/rh_core/rh_core/errors.py @@ -0,0 +1,65 @@ +"""Structured errors returned by RoboHarness core rules.""" + +from __future__ import annotations + +from collections.abc import Iterable +from dataclasses import dataclass +from enum import Enum + + +class ErrorCode(str, Enum): + """Stable machine-readable categories for core failures.""" + + FILE_NOT_FOUND = "file_not_found" + FILE_READ_ERROR = "file_read_error" + YAML_SYNTAX = "yaml_syntax" + YAML_DUPLICATE_KEY = "yaml_duplicate_key" + TYPE_MISMATCH = "type_mismatch" + MISSING_FIELD = "missing_field" + UNKNOWN_FIELD = "unknown_field" + UNSUPPORTED_SCHEMA_VERSION = "unsupported_schema_version" + INVALID_VALUE = "invalid_value" + DUPLICATE_EPISODE_ID = "duplicate_episode_id" + FRAME_MISMATCH = "frame_mismatch" + UNSUPPORTED_FRAME = "unsupported_frame" + INVALID_TRANSITION = "invalid_transition" + TERMINATION_REQUIRED = "termination_required" + TERMINATION_ALREADY_COMMITTED = "termination_already_committed" + UNEXPECTED_TERMINATION_REASON = "unexpected_termination_reason" + + +@dataclass(frozen=True, slots=True) +class ValidationIssue: + """One precise validation or lifecycle failure.""" + + code: ErrorCode + path: str + message: str + + def as_dict(self) -> dict[str, str]: + """Return a serialization-friendly representation.""" + + return {"code": self.code.value, "path": self.path, "message": self.message} + + +class CoreError(ValueError): + """Base exception carrying one or more structured issues.""" + + def __init__(self, issues: Iterable[ValidationIssue]) -> None: + self.issues = tuple(issues) + if not self.issues: + raise ValueError("CoreError requires at least one issue") + super().__init__(self._format_issues()) + + def _format_issues(self) -> str: + return "; ".join( + f"{issue.code.value} at {issue.path}: {issue.message}" for issue in self.issues + ) + + +class ConfigError(CoreError): + """Configuration could not be loaded or statically validated.""" + + +class LifecycleError(CoreError): + """A requested domain-state transition violated the lifecycle contract.""" diff --git a/packages/rh_core/rh_core/lifecycle.py b/packages/rh_core/rh_core/lifecycle.py new file mode 100644 index 0000000..9b34aa4 --- /dev/null +++ b/packages/rh_core/rh_core/lifecycle.py @@ -0,0 +1,197 @@ +"""Pure lifecycle transition guards for Experiments and Episodes.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace + +from rh_core.errors import ErrorCode, LifecycleError, ValidationIssue +from rh_core.models import EpisodeState, ExperimentState, TerminationReason + +_EXPERIMENT_TRANSITIONS: dict[ExperimentState, frozenset[ExperimentState]] = { + ExperimentState.CREATED: frozenset({ExperimentState.STARTING}), + ExperimentState.STARTING: frozenset({ExperimentState.RUNNING, ExperimentState.FAILED}), + ExperimentState.RUNNING: frozenset({ExperimentState.FINALIZING, ExperimentState.FAILED}), + ExperimentState.FINALIZING: frozenset( + {ExperimentState.FINISHED, ExperimentState.FAILED} + ), + ExperimentState.FINISHED: frozenset(), + ExperimentState.FAILED: frozenset(), +} + +_EPISODE_TRANSITIONS: dict[EpisodeState, frozenset[EpisodeState]] = { + EpisodeState.PREPARING: frozenset({EpisodeState.READY, EpisodeState.TERMINATING}), + EpisodeState.READY: frozenset({EpisodeState.RUNNING, EpisodeState.TERMINATING}), + EpisodeState.RUNNING: frozenset({EpisodeState.TERMINATING}), + EpisodeState.TERMINATING: frozenset({EpisodeState.FINISHED}), + EpisodeState.FINISHED: frozenset(), +} +_UINT64_MAX = 2**64 - 1 + + +def _lifecycle_error(code: ErrorCode, path: str, message: str) -> LifecycleError: + return LifecycleError((ValidationIssue(code=code, path=path, message=message),)) + + +@dataclass(frozen=True, slots=True) +class ExperimentLifecycle: + """Immutable snapshot of Experiment lifecycle state.""" + + state: ExperimentState = ExperimentState.CREATED + + def __post_init__(self) -> None: + if not isinstance(self.state, ExperimentState): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + "experiment.state", + "state must be an ExperimentState", + ) + + def transition(self, target: ExperimentState) -> ExperimentLifecycle: + """Return the next snapshot or reject an illegal transition.""" + + if not isinstance(target, ExperimentState): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + "experiment.state", + "target must be an ExperimentState", + ) + if target not in _EXPERIMENT_TRANSITIONS[self.state]: + raise _lifecycle_error( + ErrorCode.INVALID_TRANSITION, + "experiment.state", + f"cannot transition from {self.state.name} to {target.name}", + ) + return replace(self, state=target) + + +@dataclass(frozen=True, slots=True) +class EpisodeLifecycle: + """Immutable Episode state with a monotonic transition sequence.""" + + episode_id: str + state: EpisodeState = EpisodeState.PREPARING + termination_reason: TerminationReason = TerminationReason.NONE + sequence: int = 0 + + def __post_init__(self) -> None: + path = f"episode[{self.episode_id}]" + if not isinstance(self.episode_id, str): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + "episode.id", + "episode_id must be a string", + ) + if not self.episode_id.strip(): + raise _lifecycle_error( + ErrorCode.INVALID_VALUE, + "episode.id", + "episode_id must not be empty", + ) + if not isinstance(self.state, EpisodeState): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + f"{path}.state", + "state must be an EpisodeState", + ) + if not isinstance(self.termination_reason, TerminationReason): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + f"{path}.termination_reason", + "termination_reason must be a TerminationReason", + ) + if isinstance(self.sequence, bool) or not isinstance(self.sequence, int): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + f"{path}.sequence", + "sequence must be an integer", + ) + if not 0 <= self.sequence <= _UINT64_MAX: + raise _lifecycle_error( + ErrorCode.INVALID_VALUE, + f"{path}.sequence", + "sequence must fit in an unsigned 64-bit integer", + ) + + reason_is_committed = self.termination_reason is not TerminationReason.NONE + state_is_terminal = self.state in {EpisodeState.TERMINATING, EpisodeState.FINISHED} + if state_is_terminal and not reason_is_committed: + raise _lifecycle_error( + ErrorCode.TERMINATION_REQUIRED, + f"{path}.termination_reason", + f"state {self.state.name} requires a non-NONE reason", + ) + if not state_is_terminal and reason_is_committed: + raise _lifecycle_error( + ErrorCode.UNEXPECTED_TERMINATION_REASON, + f"{path}.termination_reason", + f"state {self.state.name} cannot carry a committed reason", + ) + + def transition( + self, + target: EpisodeState, + *, + termination_reason: TerminationReason | None = None, + ) -> EpisodeLifecycle: + """Return the next snapshot while enforcing one-time termination commit.""" + + path = f"episode[{self.episode_id}].state" + if not isinstance(target, EpisodeState): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + path, + "target must be an EpisodeState", + ) + if termination_reason is not None and not isinstance( + termination_reason, TerminationReason + ): + raise _lifecycle_error( + ErrorCode.TYPE_MISMATCH, + f"episode[{self.episode_id}].termination_reason", + "termination_reason must be a TerminationReason", + ) + if target not in _EPISODE_TRANSITIONS[self.state]: + raise _lifecycle_error( + ErrorCode.INVALID_TRANSITION, + path, + f"cannot transition from {self.state.name} to {target.name}", + ) + + if self.termination_reason is not TerminationReason.NONE: + if termination_reason is not None: + raise _lifecycle_error( + ErrorCode.TERMINATION_ALREADY_COMMITTED, + f"episode[{self.episode_id}].termination_reason", + f"termination reason is already {self.termination_reason.name}", + ) + next_reason = self.termination_reason + elif target is EpisodeState.TERMINATING: + if termination_reason in (None, TerminationReason.NONE): + raise _lifecycle_error( + ErrorCode.TERMINATION_REQUIRED, + f"episode[{self.episode_id}].termination_reason", + "entering TERMINATING requires a non-NONE reason", + ) + next_reason = termination_reason + elif termination_reason is not None: + raise _lifecycle_error( + ErrorCode.UNEXPECTED_TERMINATION_REASON, + f"episode[{self.episode_id}].termination_reason", + "a reason can only be committed when entering TERMINATING", + ) + else: + next_reason = TerminationReason.NONE + + if self.sequence == _UINT64_MAX: + raise _lifecycle_error( + ErrorCode.INVALID_VALUE, + f"episode[{self.episode_id}].sequence", + "sequence cannot be incremented beyond uint64", + ) + + return replace( + self, + state=target, + termination_reason=next_reason, + sequence=self.sequence + 1, + ) diff --git a/packages/rh_core/rh_core/models.py b/packages/rh_core/rh_core/models.py new file mode 100644 index 0000000..7e01cd8 --- /dev/null +++ b/packages/rh_core/rh_core/models.py @@ -0,0 +1,107 @@ +"""Typed domain models shared by RoboHarness core behavior.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum, IntEnum + + +class ExecutionMode(str, Enum): + """How a READY Episode is triggered.""" + + MANUAL = "manual" + AUTOMATIC = "automatic" + + +class ExperimentState(IntEnum): + """Authoritative lifecycle of one Experiment run.""" + + CREATED = 0 + STARTING = 1 + RUNNING = 2 + FINALIZING = 3 + FINISHED = 4 + FAILED = 5 + + +class EpisodeState(IntEnum): + """Authoritative lifecycle of one Episode.""" + + PREPARING = 0 + READY = 1 + RUNNING = 2 + TERMINATING = 3 + FINISHED = 4 + + +class TerminationReason(IntEnum): + """Why an Episode stopped; orthogonal to its lifecycle state.""" + + NONE = 0 + SUCCESS = 1 + TIMEOUT = 2 + ABORTED = 3 + FAILURE = 4 + ENV_ERROR = 5 + AGENT_ERROR = 6 + INVALID_TASK = 7 + + +@dataclass(frozen=True, slots=True) +class Point3D: + """Three-dimensional point expressed in a named coordinate frame.""" + + frame_id: str + x: float + y: float + z: float + + +@dataclass(frozen=True, slots=True) +class Pose3D: + """Robot pose using metres and fixed-axis X-Y-Z roll/pitch/yaw radians.""" + + frame_id: str + x: float + y: float + z: float + roll: float + pitch: float + yaw: float + + +@dataclass(frozen=True, slots=True) +class PointNavTaskSpec: + """Immutable static input for a PointNav Episode.""" + + goal: Point3D + success_radius_m: float + timeout_s: float + + +@dataclass(frozen=True, slots=True) +class EpisodeSpec: + """Validated and immutable description of one Episode.""" + + episode_id: str + scenario: str + initial_pose: Pose3D + task: PointNavTaskSpec + seed: int + + +@dataclass(frozen=True, slots=True) +class ExperimentSpec: + """Ordered collection of Episode specifications.""" + + name: str + execution_mode: ExecutionMode + episodes: tuple[EpisodeSpec, ...] + + +@dataclass(frozen=True, slots=True) +class ExperimentConfig: + """Versioned, validated root configuration document.""" + + schema_version: int + experiment: ExperimentSpec diff --git a/packages/rh_core/rh_core/py.typed b/packages/rh_core/rh_core/py.typed new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/packages/rh_core/rh_core/py.typed @@ -0,0 +1 @@ + diff --git a/packages/rh_core/rh_core/termination.py b/packages/rh_core/rh_core/termination.py new file mode 100644 index 0000000..065848d --- /dev/null +++ b/packages/rh_core/rh_core/termination.py @@ -0,0 +1,38 @@ +"""Deterministic policy for competing Episode termination candidates.""" + +from __future__ import annotations + +from collections.abc import Iterable + +from rh_core.models import TerminationReason + +# Safety/infrastructure failures outrank operator abort, success, and timeout. +# Within the failure tier, the Environment wins because it is the final motion +# safety boundary. Existing order is domain behavior and must be changed with care. +TERMINATION_PRIORITY: tuple[TerminationReason, ...] = ( + TerminationReason.ENV_ERROR, + TerminationReason.AGENT_ERROR, + TerminationReason.FAILURE, + TerminationReason.INVALID_TASK, + TerminationReason.ABORTED, + TerminationReason.SUCCESS, + TerminationReason.TIMEOUT, +) + +_PRIORITY_RANK = {reason: rank for rank, reason in enumerate(TERMINATION_PRIORITY)} + + +def resolve_termination_reason( + candidates: Iterable[TerminationReason], +) -> TerminationReason: + """Select one authoritative reason, or NONE when no candidate exists.""" + + effective: set[TerminationReason] = set() + for reason in candidates: + if not isinstance(reason, TerminationReason): + raise TypeError("termination candidates must be TerminationReason values") + if reason is not TerminationReason.NONE: + effective.add(reason) + if not effective: + return TerminationReason.NONE + return min(effective, key=_PRIORITY_RANK.__getitem__) diff --git a/packages/rh_core/setup.cfg b/packages/rh_core/setup.cfg new file mode 100644 index 0000000..62f1a2e --- /dev/null +++ b/packages/rh_core/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/rh_core + +[install] +install_scripts=$base/lib/rh_core diff --git a/packages/rh_core/setup.py b/packages/rh_core/setup.py new file mode 100644 index 0000000..0e91101 --- /dev/null +++ b/packages/rh_core/setup.py @@ -0,0 +1,21 @@ +from setuptools import find_packages, setup + +package_name = "rh_core" + +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=["PyYAML", "setuptools"], + zip_safe=True, + maintainer="Staaaaaaaaar", + maintainer_email="2300012435@stu.pku.edu.cn", + description="ROS-independent domain models and rules for RoboHarness.", + license="NOASSERTION", + tests_require=["pytest"], +) diff --git a/packages/rh_core/test/conftest.py b/packages/rh_core/test/conftest.py new file mode 100644 index 0000000..5fffbfc --- /dev/null +++ b/packages/rh_core/test/conftest.py @@ -0,0 +1,38 @@ +from __future__ import annotations + +from typing import Any + +import pytest + + +@pytest.fixture +def valid_document() -> dict[str, Any]: + return { + "schema_version": 1, + "experiment": { + "name": "go2_keyboard_pointnav", + "execution_mode": "manual", + "episodes": [ + { + "episode_id": "0000", + "scenario": "warehouse_default", + "initial_pose": { + "frame_id": "map", + "x": 1.0, + "y": 2.0, + "z": 0.4, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + }, + "task": { + "type": "pointnav", + "goal": {"frame_id": "map", "x": 8.0, "y": 4.0, "z": 0.4}, + "success_radius_m": 0.5, + "timeout_s": 120.0, + }, + "seed": 42, + } + ], + }, + } diff --git a/packages/rh_core/test/test_config.py b/packages/rh_core/test/test_config.py new file mode 100644 index 0000000..7b437ca --- /dev/null +++ b/packages/rh_core/test/test_config.py @@ -0,0 +1,212 @@ +from __future__ import annotations + +from copy import deepcopy +from dataclasses import FrozenInstanceError +from pathlib import Path +from typing import Any + +import pytest + +from rh_core import ConfigError, ErrorCode, ExecutionMode +from rh_core.config import load_experiment_config, parse_experiment_config + + +def issue_codes(error: ConfigError) -> set[ErrorCode]: + return {issue.code for issue in error.issues} + + +def test_canonical_mvp_config_loads_to_immutable_models() -> None: + repository_root = Path(__file__).resolve().parents[3] + config = load_experiment_config(repository_root / "configs/experiments/mvp.yaml") + + assert config.schema_version == 1 + assert config.experiment.name == "go2_keyboard_pointnav" + assert config.experiment.execution_mode is ExecutionMode.MANUAL + assert isinstance(config.experiment.episodes, tuple) + assert config.experiment.episodes[0].initial_pose.frame_id == "map" + assert config.experiment.episodes[0].initial_pose.yaw == 0.0 + assert config.experiment.episodes[0].task.goal.z == 0.4 + assert config.experiment.episodes[0].seed == 42 + + with pytest.raises(FrozenInstanceError): + config.experiment.name = "changed" # type: ignore[misc] + + +def test_automatic_execution_mode_is_supported(valid_document: dict[str, Any]) -> None: + valid_document["experiment"]["execution_mode"] = "automatic" + config = parse_experiment_config(valid_document) + assert config.experiment.execution_mode is ExecutionMode.AUTOMATIC + + +@pytest.mark.parametrize("schema_version", [0, 2, -1]) +def test_unknown_schema_version_is_rejected( + valid_document: dict[str, Any], schema_version: int +) -> None: + valid_document["schema_version"] = schema_version + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.UNSUPPORTED_SCHEMA_VERSION in issue_codes(caught.value) + + +def test_missing_and_unknown_fields_are_reported_together(valid_document: dict[str, Any]) -> None: + del valid_document["experiment"]["name"] + valid_document["experiment"]["typo"] = True + + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + + issues = {(issue.code, issue.path) for issue in caught.value.issues} + assert (ErrorCode.MISSING_FIELD, "experiment.name") in issues + assert (ErrorCode.UNKNOWN_FIELD, "experiment.typo") in issues + + +def test_duplicate_episode_ids_are_rejected(valid_document: dict[str, Any]) -> None: + duplicate = deepcopy(valid_document["experiment"]["episodes"][0]) + valid_document["experiment"]["episodes"].append(duplicate) + + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + + issue = next( + issue for issue in caught.value.issues if issue.code is ErrorCode.DUPLICATE_EPISODE_ID + ) + assert issue.path == "experiment.episodes[1].episode_id" + + +@pytest.mark.parametrize("value", [float("nan"), float("inf"), float("-inf")]) +@pytest.mark.parametrize("field", ["x", "y", "z"]) +def test_non_finite_goal_values_are_rejected( + valid_document: dict[str, Any], field: str, value: float +) -> None: + valid_document["experiment"]["episodes"][0]["task"]["goal"][field] = value + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.INVALID_VALUE in issue_codes(caught.value) + + +def test_unrepresentably_large_number_is_structured(valid_document: dict[str, Any]) -> None: + valid_document["experiment"]["episodes"][0]["task"]["goal"]["x"] = 10**10000 + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.INVALID_VALUE in issue_codes(caught.value) + + +@pytest.mark.parametrize("field", ["success_radius_m", "timeout_s"]) +@pytest.mark.parametrize("value", [0, -1.0, float("nan"), float("inf")]) +def test_positive_finite_task_limits_are_required( + valid_document: dict[str, Any], field: str, value: float +) -> None: + valid_document["experiment"]["episodes"][0]["task"][field] = value + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.INVALID_VALUE in issue_codes(caught.value) + + +def test_frame_mismatch_and_non_map_frames_are_reported(valid_document: dict[str, Any]) -> None: + episode = valid_document["experiment"]["episodes"][0] + episode["initial_pose"]["frame_id"] = "odom" + + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + + assert ErrorCode.FRAME_MISMATCH in issue_codes(caught.value) + assert ErrorCode.UNSUPPORTED_FRAME in issue_codes(caught.value) + + +@pytest.mark.parametrize("value", [True, 1.5, 2**63, -(2**63) - 1]) +def test_seed_must_be_int64(valid_document: dict[str, Any], value: object) -> None: + valid_document["experiment"]["episodes"][0]["seed"] = value + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert issue_codes(caught.value) & {ErrorCode.TYPE_MISMATCH, ErrorCode.INVALID_VALUE} + + +@pytest.mark.parametrize("path", ["success_radius_m", "timeout_s"]) +def test_boolean_is_not_accepted_as_numeric_value( + valid_document: dict[str, Any], path: str +) -> None: + valid_document["experiment"]["episodes"][0]["task"][path] = True + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.TYPE_MISMATCH in issue_codes(caught.value) + + +def test_unsupported_task_type_is_rejected(valid_document: dict[str, Any]) -> None: + valid_document["experiment"]["episodes"][0]["task"]["type"] = "coverage" + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.INVALID_VALUE in issue_codes(caught.value) + + +def test_orientation_field_is_rejected_for_pointnav( + valid_document: dict[str, Any], +) -> None: + valid_document["experiment"]["episodes"][0]["task"]["goal"]["yaw"] = 1.0 + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert (ErrorCode.UNKNOWN_FIELD, "experiment.episodes[0].task.goal.yaw") in { + (issue.code, issue.path) for issue in caught.value.issues + } + + +@pytest.mark.parametrize("field", ["x", "y", "z", "roll", "pitch", "yaw"]) +@pytest.mark.parametrize("value", [float("nan"), float("inf"), float("-inf")]) +def test_initial_pose_requires_finite_position_and_orientation( + valid_document: dict[str, Any], field: str, value: float +) -> None: + valid_document["experiment"]["episodes"][0]["initial_pose"][field] = value + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + assert ErrorCode.INVALID_VALUE in issue_codes(caught.value) + + +def test_root_must_be_a_mapping() -> None: + with pytest.raises(ConfigError) as caught: + parse_experiment_config([]) + assert caught.value.issues[0].code is ErrorCode.TYPE_MISMATCH + assert caught.value.issues[0].path == "$" + + +def test_yaml_duplicate_key_is_rejected(tmp_path: Path) -> None: + path = tmp_path / "duplicate.yaml" + path.write_text("schema_version: 1\nschema_version: 1\n", encoding="utf-8") + with pytest.raises(ConfigError) as caught: + load_experiment_config(path) + assert caught.value.issues[0].code is ErrorCode.YAML_DUPLICATE_KEY + + +def test_invalid_yaml_is_structured(tmp_path: Path) -> None: + path = tmp_path / "invalid.yaml" + path.write_text("experiment: [\n", encoding="utf-8") + with pytest.raises(ConfigError) as caught: + load_experiment_config(path) + assert caught.value.issues[0].code is ErrorCode.YAML_SYNTAX + + +def test_missing_file_is_structured(tmp_path: Path) -> None: + path = tmp_path / "missing.yaml" + with pytest.raises(ConfigError) as caught: + load_experiment_config(path) + assert caught.value.issues[0].as_dict() == { + "code": "file_not_found", + "path": str(path), + "message": "configuration file does not exist", + } + + +def test_duplicate_id_path_keeps_original_index_when_prior_episode_is_invalid( + valid_document: dict[str, Any], +) -> None: + first = deepcopy(valid_document["experiment"]["episodes"][0]) + invalid = deepcopy(first) + invalid["episode_id"] = "" + duplicate = deepcopy(first) + valid_document["experiment"]["episodes"] = [first, invalid, duplicate] + + with pytest.raises(ConfigError) as caught: + parse_experiment_config(valid_document) + + duplicate_issue = next( + issue for issue in caught.value.issues if issue.code is ErrorCode.DUPLICATE_EPISODE_ID + ) + assert duplicate_issue.path == "experiment.episodes[2].episode_id" diff --git a/packages/rh_core/test/test_lifecycle.py b/packages/rh_core/test/test_lifecycle.py new file mode 100644 index 0000000..90bb815 --- /dev/null +++ b/packages/rh_core/test/test_lifecycle.py @@ -0,0 +1,177 @@ +from __future__ import annotations + +from dataclasses import FrozenInstanceError + +import pytest + +from rh_core import ( + EpisodeLifecycle, + EpisodeState, + ErrorCode, + ExperimentLifecycle, + ExperimentState, + LifecycleError, + TerminationReason, +) + + +def assert_error_code(caught: pytest.ExceptionInfo[LifecycleError], code: ErrorCode) -> None: + assert caught.value.issues[0].code is code + + +def test_experiment_happy_path() -> None: + lifecycle = ExperimentLifecycle() + for target in ( + ExperimentState.STARTING, + ExperimentState.RUNNING, + ExperimentState.FINALIZING, + ExperimentState.FINISHED, + ): + lifecycle = lifecycle.transition(target) + assert lifecycle.state is ExperimentState.FINISHED + + +@pytest.mark.parametrize( + ("initial", "target"), + [ + (ExperimentState.CREATED, ExperimentState.RUNNING), + (ExperimentState.STARTING, ExperimentState.FINISHED), + (ExperimentState.RUNNING, ExperimentState.FINISHED), + (ExperimentState.FINALIZING, ExperimentState.RUNNING), + (ExperimentState.FINISHED, ExperimentState.STARTING), + (ExperimentState.FAILED, ExperimentState.STARTING), + ], +) +def test_illegal_experiment_transitions_are_rejected( + initial: ExperimentState, target: ExperimentState +) -> None: + with pytest.raises(LifecycleError) as caught: + ExperimentLifecycle(initial).transition(target) + assert_error_code(caught, ErrorCode.INVALID_TRANSITION) + + +@pytest.mark.parametrize( + "initial", + [ExperimentState.STARTING, ExperimentState.RUNNING, ExperimentState.FINALIZING], +) +def test_active_experiment_can_fail(initial: ExperimentState) -> None: + lifecycle = ExperimentLifecycle(initial).transition(ExperimentState.FAILED) + assert lifecycle.state is ExperimentState.FAILED + + +def test_episode_happy_path_increments_sequence() -> None: + lifecycle = EpisodeLifecycle("0000") + lifecycle = lifecycle.transition(EpisodeState.READY) + lifecycle = lifecycle.transition(EpisodeState.RUNNING) + lifecycle = lifecycle.transition( + EpisodeState.TERMINATING, + termination_reason=TerminationReason.SUCCESS, + ) + lifecycle = lifecycle.transition(EpisodeState.FINISHED) + + assert lifecycle.sequence == 4 + assert lifecycle.state is EpisodeState.FINISHED + assert lifecycle.termination_reason is TerminationReason.SUCCESS + + +@pytest.mark.parametrize("initial", [EpisodeState.PREPARING, EpisodeState.READY]) +def test_episode_can_terminate_before_running(initial: EpisodeState) -> None: + lifecycle = EpisodeLifecycle("0000", state=initial) + lifecycle = lifecycle.transition( + EpisodeState.TERMINATING, + termination_reason=TerminationReason.INVALID_TASK, + ) + assert lifecycle.termination_reason is TerminationReason.INVALID_TASK + + +def test_entering_terminating_requires_reason() -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000", state=EpisodeState.RUNNING).transition( + EpisodeState.TERMINATING + ) + assert_error_code(caught, ErrorCode.TERMINATION_REQUIRED) + + +def test_reason_cannot_be_committed_before_terminating() -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000").transition( + EpisodeState.READY, + termination_reason=TerminationReason.SUCCESS, + ) + assert_error_code(caught, ErrorCode.UNEXPECTED_TERMINATION_REASON) + + +def test_reason_cannot_be_committed_twice() -> None: + lifecycle = EpisodeLifecycle("0000", state=EpisodeState.RUNNING).transition( + EpisodeState.TERMINATING, + termination_reason=TerminationReason.TIMEOUT, + ) + with pytest.raises(LifecycleError) as caught: + lifecycle.transition( + EpisodeState.FINISHED, + termination_reason=TerminationReason.SUCCESS, + ) + assert_error_code(caught, ErrorCode.TERMINATION_ALREADY_COMMITTED) + + +@pytest.mark.parametrize( + ("initial", "target"), + [ + (EpisodeState.PREPARING, EpisodeState.RUNNING), + (EpisodeState.READY, EpisodeState.FINISHED), + (EpisodeState.RUNNING, EpisodeState.READY), + (EpisodeState.TERMINATING, EpisodeState.RUNNING), + (EpisodeState.FINISHED, EpisodeState.PREPARING), + ], +) +def test_illegal_episode_transitions_are_rejected( + initial: EpisodeState, target: EpisodeState +) -> None: + reason = ( + TerminationReason.FAILURE + if initial in {EpisodeState.TERMINATING, EpisodeState.FINISHED} + else TerminationReason.NONE + ) + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000", state=initial, termination_reason=reason).transition(target) + assert_error_code(caught, ErrorCode.INVALID_TRANSITION) + + +def test_lifecycle_snapshots_are_immutable() -> None: + lifecycle = EpisodeLifecycle("0000") + with pytest.raises(FrozenInstanceError): + lifecycle.sequence = 2 # type: ignore[misc] + + +def test_raw_integer_target_is_rejected() -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000").transition(1) # type: ignore[arg-type] + assert_error_code(caught, ErrorCode.TYPE_MISMATCH) + + +@pytest.mark.parametrize("state", [EpisodeState.TERMINATING, EpisodeState.FINISHED]) +def test_terminal_snapshot_requires_reason(state: EpisodeState) -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000", state=state) + assert_error_code(caught, ErrorCode.TERMINATION_REQUIRED) + + +def test_non_terminal_snapshot_rejects_committed_reason() -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000", termination_reason=TerminationReason.SUCCESS) + assert_error_code(caught, ErrorCode.UNEXPECTED_TERMINATION_REASON) + + +@pytest.mark.parametrize("sequence", [-1, 2**64, True, 1.5]) +def test_sequence_must_be_uint64(sequence: object) -> None: + with pytest.raises(LifecycleError) as caught: + EpisodeLifecycle("0000", sequence=sequence) # type: ignore[arg-type] + issue = caught.value.issues[0] + assert issue.code in {ErrorCode.TYPE_MISMATCH, ErrorCode.INVALID_VALUE} + + +def test_sequence_cannot_overflow() -> None: + lifecycle = EpisodeLifecycle("0000", sequence=2**64 - 1) + with pytest.raises(LifecycleError) as caught: + lifecycle.transition(EpisodeState.READY) + assert_error_code(caught, ErrorCode.INVALID_VALUE) diff --git a/packages/rh_core/test/test_termination.py b/packages/rh_core/test/test_termination.py new file mode 100644 index 0000000..efe57ce --- /dev/null +++ b/packages/rh_core/test/test_termination.py @@ -0,0 +1,39 @@ +from __future__ import annotations + +import pytest + +from rh_core import TerminationReason, resolve_termination_reason + + +def test_no_candidate_resolves_to_none() -> None: + assert resolve_termination_reason([]) is TerminationReason.NONE + assert resolve_termination_reason([TerminationReason.NONE]) is TerminationReason.NONE + + +@pytest.mark.parametrize( + ("higher", "lower"), + [ + (TerminationReason.ENV_ERROR, TerminationReason.AGENT_ERROR), + (TerminationReason.AGENT_ERROR, TerminationReason.FAILURE), + (TerminationReason.FAILURE, TerminationReason.INVALID_TASK), + (TerminationReason.INVALID_TASK, TerminationReason.ABORTED), + (TerminationReason.ABORTED, TerminationReason.SUCCESS), + (TerminationReason.SUCCESS, TerminationReason.TIMEOUT), + ], +) +def test_priority_is_deterministic( + higher: TerminationReason, lower: TerminationReason +) -> None: + assert resolve_termination_reason([lower, higher]) is higher + assert resolve_termination_reason([higher, lower]) is higher + + +def test_duplicate_candidates_do_not_change_result() -> None: + assert resolve_termination_reason( + [TerminationReason.TIMEOUT, TerminationReason.SUCCESS, TerminationReason.SUCCESS] + ) is TerminationReason.SUCCESS + + +def test_raw_integer_candidate_is_rejected() -> None: + with pytest.raises(TypeError, match="TerminationReason"): + resolve_termination_reason([1]) # type: ignore[list-item] diff --git a/packages/rh_interfaces/README.md b/packages/rh_interfaces/README.md index 4385aef..74d2c9e 100644 --- a/packages/rh_interfaces/README.md +++ b/packages/rh_interfaces/README.md @@ -27,8 +27,15 @@ does not wrap those observations in a generic message. - Identifiers and request IDs are opaque non-empty strings. Validation belongs to the core/runtime layers rather than the generated interface classes. -- `PointNavTask.start`, `PointNavTask.goal`, and `ResetEnv.start` use the `map` - frame in the MVP. Task start and goal frames must match. +- `PointNavTask.goal` and `ResetEnv.initial_pose` use the `map` frame in the MVP. +- `ResetEnv.initial_pose` is the complete 3D robot initialization pose. Its ROS + orientation is a normalized quaternion; adapters for planar sources explicitly + supply zero `z`, roll, and pitch before conversion. +- PointNav describes a 3D target position and does not impose a target + orientation. A future PoseNav task must introduce explicit pose semantics + rather than reinterpreting the PointNav goal. +- PointNav success distance is the 3D Euclidean distance in metres from the + configured robot tracking point (MVP: the `base_link` origin) to the goal. - Durations use seconds, distances use metres, and Episode metrics use simulation time and the `map` frame. - `ComponentStatus.stamp` records the last transition time. Repeated status diff --git a/packages/rh_interfaces/msg/PointNavTask.msg b/packages/rh_interfaces/msg/PointNavTask.msg index a1ebf54..d7d9423 100644 --- a/packages/rh_interfaces/msg/PointNavTask.msg +++ b/packages/rh_interfaces/msg/PointNavTask.msg @@ -1,12 +1,13 @@ -# Immutable PointNav task snapshot. start and goal must use the same frame. -# RoboHarness MVP requires that frame to be "map". +# Immutable PointNav task snapshot. PointNav specifies a 3D target position, +# not a desired target orientation. A future PoseNav task must use a distinct +# interface. The MVP requires the goal frame to be "map". string experiment_id string episode_id -geometry_msgs/PoseStamped start -geometry_msgs/PoseStamped goal +geometry_msgs/PointStamped goal -# Goal is reached when planar distance is less than or equal to this value. +# Goal is reached when the 3D Euclidean distance from the configured robot +# tracking point (MVP: base_link origin) is less than or equal to this value. float64 success_radius_m # Episode timeout measured in simulation seconds. diff --git a/packages/rh_interfaces/srv/ResetEnv.srv b/packages/rh_interfaces/srv/ResetEnv.srv index b839c98..5239a9c 100644 --- a/packages/rh_interfaces/srv/ResetEnv.srv +++ b/packages/rh_interfaces/srv/ResetEnv.srv @@ -3,8 +3,9 @@ string request_id string experiment_id string episode_id -# Requested robot root pose. RoboHarness MVP requires the "map" frame. -geometry_msgs/PoseStamped start +# Requested robot root pose. RoboHarness MVP requires the "map" frame. The +# quaternion must be finite and normalized by the caller/adapter. +geometry_msgs/PoseStamped initial_pose # Reproducibility seed supplied to the environment backend. int64 seed diff --git a/pyproject.toml b/pyproject.toml index 3614226..3f43382 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -10,6 +10,9 @@ extend-exclude = [".build"] [tool.ruff.lint] select = ["E", "F", "I", "UP"] +[tool.ruff.lint.isort] +known-first-party = ["rh_core"] + [tool.pytest.ini_options] addopts = "-ra" testpaths = ["tests"] diff --git a/tests/contracts/README.md b/tests/contracts/README.md index 8aed671..fc0e8e7 100644 --- a/tests/contracts/README.md +++ b/tests/contracts/README.md @@ -4,3 +4,8 @@ This domain contains black-box tests for public RoboHarness contracts. The `rh_interfaces_contract_tests` package verifies generated C++ and Python ROS 2 types, stable field/constant definitions, serialization round trips, and the interface package dependency boundary. + +`rh_core_contract_tests` verifies that the ROS-independent domain enums remain +aligned with the wire constants, that the initial 3D pose and PointNav goal map +to their wire contracts without dimensional loss, and that core source and +manifest dependencies remain ROS-free. diff --git a/tests/contracts/rh_core_contract_tests/CMakeLists.txt b/tests/contracts/rh_core_contract_tests/CMakeLists.txt new file mode 100644 index 0000000..eb621e6 --- /dev/null +++ b/tests/contracts/rh_core_contract_tests/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) +project(rh_core_contract_tests) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_pytest REQUIRED) + +ament_add_pytest_test(test_core_interface_alignment test/test_core_interface_alignment.py + TIMEOUT 30 +) + +ament_package() diff --git a/tests/contracts/rh_core_contract_tests/package.xml b/tests/contracts/rh_core_contract_tests/package.xml new file mode 100644 index 0000000..1e11290 --- /dev/null +++ b/tests/contracts/rh_core_contract_tests/package.xml @@ -0,0 +1,21 @@ + + + + rh_core_contract_tests + 0.0.0 + Dependency and ROS-interface alignment tests for rh_core. + Staaaaaaaaar + + NOASSERTION + + ament_cmake + + ament_cmake_pytest + ament_index_python + rh_core + rh_interfaces + + + ament_cmake + + diff --git a/tests/contracts/rh_core_contract_tests/test/test_core_interface_alignment.py b/tests/contracts/rh_core_contract_tests/test/test_core_interface_alignment.py new file mode 100644 index 0000000..9d87222 --- /dev/null +++ b/tests/contracts/rh_core_contract_tests/test/test_core_interface_alignment.py @@ -0,0 +1,136 @@ +from __future__ import annotations + +import ast +import xml.etree.ElementTree as ET +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from rh_interfaces.msg import EpisodeState as EpisodeStateMessage +from rh_interfaces.msg import PointNavTask +from rh_interfaces.srv import ResetEnv + +import rh_core +from rh_core import ( + EpisodeSpec, + EpisodeState, + Point3D, + PointNavTaskSpec, + Pose3D, + TerminationReason, +) + + +def test_episode_lifecycle_values_match_wire_contract() -> None: + for name in ("PREPARING", "READY", "RUNNING", "TERMINATING", "FINISHED"): + assert getattr(EpisodeState, name).value == getattr(EpisodeStateMessage, name) + + +def test_termination_values_match_wire_contract() -> None: + for name in ( + "NONE", + "SUCCESS", + "TIMEOUT", + "ABORTED", + "FAILURE", + "ENV_ERROR", + "AGENT_ERROR", + "INVALID_TASK", + ): + assert getattr(TerminationReason, name).value == getattr(EpisodeStateMessage, name) + + +def test_pointnav_model_maps_losslessly_to_pr2_message() -> None: + task = PointNavTaskSpec( + goal=Point3D(frame_id="map", x=8.0, y=4.0, z=1.5), + success_radius_m=0.5, + timeout_s=120.0, + ) + episode = EpisodeSpec( + episode_id="0000", + scenario="warehouse_default", + initial_pose=Pose3D( + frame_id="map", + x=1.0, + y=2.0, + z=0.4, + roll=0.0, + pitch=0.0, + yaw=0.5, + ), + task=task, + seed=42, + ) + + message = PointNavTask() + message.experiment_id = "experiment-runtime-id" + message.episode_id = episode.episode_id + message.goal.header.frame_id = task.goal.frame_id + message.goal.point.x = task.goal.x + message.goal.point.y = task.goal.y + message.goal.point.z = task.goal.z + message.success_radius_m = task.success_radius_m + message.timeout_s = task.timeout_s + message.seed = episode.seed + + assert message.episode_id == "0000" + assert message.goal.point.y == 4.0 + assert message.goal.point.z == 1.5 + assert message.success_radius_m == 0.5 + assert message.timeout_s == 120.0 + assert message.seed == 42 + + reset = ResetEnv.Request() + reset.request_id = "reset-0000" + reset.experiment_id = message.experiment_id + reset.episode_id = episode.episode_id + reset.initial_pose.header.frame_id = episode.initial_pose.frame_id + reset.initial_pose.pose.position.x = episode.initial_pose.x + reset.initial_pose.pose.position.y = episode.initial_pose.y + reset.initial_pose.pose.position.z = episode.initial_pose.z + + assert reset.initial_pose.header.frame_id == "map" + assert reset.initial_pose.pose.position.z == 0.4 + + +def test_core_manifest_has_no_ros_or_implementation_dependencies() -> None: + manifest = Path(get_package_share_directory("rh_core")) / "package.xml" + root = ET.parse(manifest).getroot() + dependency_tags = { + "buildtool_depend", + "build_depend", + "build_export_depend", + "depend", + "exec_depend", + } + dependencies = { + element.text.strip() + for element in root + if element.tag in dependency_tags and element.text is not None + } + assert dependencies == {"ament_python", "python3-yaml"} + + +def test_core_source_does_not_import_ros_or_implementations() -> None: + forbidden_roots = { + "rclpy", + "rh_interfaces", + "rh_ros", + "rh_experiment", + "isaacsim", + } + source_directory = Path(rh_core.__file__).resolve().parent + violations: list[str] = [] + + for source_path in sorted(source_directory.glob("*.py")): + tree = ast.parse(source_path.read_text(encoding="utf-8"), filename=str(source_path)) + for node in ast.walk(tree): + imported_roots: list[str] = [] + if isinstance(node, ast.Import): + imported_roots = [alias.name.split(".", maxsplit=1)[0] for alias in node.names] + elif isinstance(node, ast.ImportFrom) and node.module: + imported_roots = [node.module.split(".", maxsplit=1)[0]] + for imported_root in imported_roots: + if imported_root in forbidden_roots: + violations.append(f"{source_path.name}:{node.lineno}: {imported_root}") + + assert violations == [] diff --git a/tests/contracts/rh_interfaces_contract_tests/test/test_cpp_typesupport.cpp b/tests/contracts/rh_interfaces_contract_tests/test/test_cpp_typesupport.cpp index baec43c..d9faf07 100644 --- a/tests/contracts/rh_interfaces_contract_tests/test/test_cpp_typesupport.cpp +++ b/tests/contracts/rh_interfaces_contract_tests/test/test_cpp_typesupport.cpp @@ -89,15 +89,22 @@ TEST(RhInterfacesContract, GeneratedCppTypesAreUsable) EXPECT_EQ(state.sequence, 4u); rh_interfaces::msg::PointNavTask task; - task.start.header.frame_id = "map"; task.goal.header.frame_id = "map"; + task.goal.point.y = 4.0; + task.goal.point.z = 1.5; task.success_radius_m = 0.5; + EXPECT_DOUBLE_EQ(task.goal.point.y, 4.0); + EXPECT_DOUBLE_EQ(task.goal.point.z, 1.5); EXPECT_DOUBLE_EQ(task.success_radius_m, 0.5); rh_interfaces::srv::ResetEnv::Request reset_request; reset_request.request_id = "request-1"; - reset_request.start.header.frame_id = "map"; - EXPECT_EQ(reset_request.start.header.frame_id, "map"); + reset_request.initial_pose.header.frame_id = "map"; + reset_request.initial_pose.pose.position.x = 1.0; + reset_request.initial_pose.pose.orientation.w = 1.0; + EXPECT_EQ(reset_request.initial_pose.header.frame_id, "map"); + EXPECT_DOUBLE_EQ(reset_request.initial_pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(reset_request.initial_pose.pose.orientation.w, 1.0); rh_interfaces::srv::ResetAgent::Response reset_response; reset_response.success = true; diff --git a/tests/contracts/rh_interfaces_contract_tests/test/test_python_contract.py b/tests/contracts/rh_interfaces_contract_tests/test/test_python_contract.py index 183269c..ab0f174 100644 --- a/tests/contracts/rh_interfaces_contract_tests/test/test_python_contract.py +++ b/tests/contracts/rh_interfaces_contract_tests/test/test_python_contract.py @@ -78,8 +78,7 @@ def test_stable_numeric_constants() -> None: { "experiment_id": "string", "episode_id": "string", - "start": "geometry_msgs/PoseStamped", - "goal": "geometry_msgs/PoseStamped", + "goal": "geometry_msgs/PointStamped", "success_radius_m": "double", "timeout_s": "double", "seed": "int64", @@ -104,7 +103,7 @@ def test_stable_numeric_constants() -> None: "request_id": "string", "experiment_id": "string", "episode_id": "string", - "start": "geometry_msgs/PoseStamped", + "initial_pose": "geometry_msgs/PoseStamped", "seed": "int64", }, ),