diff --git a/src/iiwa_scene_context/docs/interfaces.md b/src/iiwa_scene_context/docs/interfaces.md new file mode 100644 index 0000000..04112bf --- /dev/null +++ b/src/iiwa_scene_context/docs/interfaces.md @@ -0,0 +1,75 @@ +# `iiwa_scene_context` interfaces + +Contract version: **1**. Catalog version: **1**. Scene schema version: **1**. +All JSON is UTF-8 and uses stable Webots object ids. + +## ROS interfaces + +| Direction | Name | Type | Notes | +| --- | --- | --- | --- | +| service | `/iiwa_scene_context/get_scene` | `std_srvs/Trigger` | `success=true` and `message` = snapshot envelope JSON, or `success=false` and a reason (not ready / stale / no state). | +| topic | `/iiwa_scene_context/scene_json` | `std_msgs/String` | The same envelope JSON. QoS: reliable, transient-local, depth 1. A late subscriber receives the latest snapshot. | +| input | `/iiwa_experement/object_state` | `std_msgs/String` | Live supervisor JSON. Only logical ownership and object state are used. | +| input | `/iiwa_experement/scene_ready` | `std_srvs/Trigger` | Polled by the node. Until it returns `success=true`, no snapshot is published. | + +The node refuses with `success=false` while the scene is not ready, before the +first supervisor state, or when the last state is older than +`freshness_sec` (default 2.0 s). A stale snapshot must never be treated as +`found`/`success` by a consumer. + +## Snapshot envelope + +```json +{ + "schema_version": 1, + "contract_version": 1, + "catalog_version": 1, + "experiment_id": 20, + "state_version": 7, + "scene_revision": 3, + "timestamp": 12.5, + "scene_json": { "...": "see below" } +} +``` + +* `state_version` is the supervisor's discrete `version`. +* `scene_revision` starts at 1 and grows **only** when `state_version` + changes. Pose updates of a held object published between ownership changes + do not advance it. A graph built for another revision is rejected. +* `timestamp` is the local receive time of the snapshot. Consumers apply + their own freshness threshold. + +## `scene_json` + +Fields: `schema_version`, `contract_version`, `catalog_version`, `profile`, +`experiment_id`, `domain`, `components`, `destinations`, `tools`, +`initial_state`, `cameras`, `available_skills`, `skill_contracts`, +`service_objects`, `recovery_attempts_per_component`. + +`components` contains only objects present in the selected experiment +(`fixture`, `body`, and conditionally `cap`, `tray`, `screw_1`…`screw_10`). +`destinations` contains `work`, `hatch_seat`, `hole_1`…`hole_4`, one +`_socket` for every present tool, and the configured sealant sides. +`tools` contains only present tools; every tool carries its own socket id. + +`initial_state` is logical only: + +```json +{ + "tool_mounted": "vacuum", + "held_object": "cap", + "flags": ["tool_mounted:vacuum", "holding:cap", "hatch_held"], + "components": {"cap": "held", "screw_1": "tray"}, + "tools": {"vacuum": "docked", "magnetic_tool": "docked"}, + "holes": {"hole_1": {"state": "empty", "screw": null}}, + "scene_ready": true +} +``` + +No coordinates, rotations, TF frames, supervisor poses or other supervisor +internals are part of `scene_json`. Exact object poses are read from the +supervisor by skill execution and never by the model. + +`available_skills` and `skill_contracts` come from the server catalog in +`iiwa_scene_context/catalog.py`; a graph cannot extend them. `tighten` is not +available in this stage. diff --git a/src/iiwa_scene_context/iiwa_scene_context/__init__.py b/src/iiwa_scene_context/iiwa_scene_context/__init__.py new file mode 100644 index 0000000..7effa13 --- /dev/null +++ b/src/iiwa_scene_context/iiwa_scene_context/__init__.py @@ -0,0 +1,21 @@ +"""Scene catalogue and live snapshot contract for the Grant planner.""" +from .catalog import ( + AVAILABLE_SKILLS, + CATALOG_VERSION, + CONTRACT_VERSION, + DEFAULT_CATALOG, + SCENE_SCHEMA_VERSION, + SkillCatalog, +) +from .snapshot import build_envelope, build_scene + +__all__ = [ + 'AVAILABLE_SKILLS', + 'CATALOG_VERSION', + 'CONTRACT_VERSION', + 'DEFAULT_CATALOG', + 'SCENE_SCHEMA_VERSION', + 'SkillCatalog', + 'build_envelope', + 'build_scene', +] diff --git a/src/iiwa_scene_context/iiwa_scene_context/catalog.py b/src/iiwa_scene_context/iiwa_scene_context/catalog.py new file mode 100644 index 0000000..b6f32b6 --- /dev/null +++ b/src/iiwa_scene_context/iiwa_scene_context/catalog.py @@ -0,0 +1,431 @@ +"""Server-side skill and object catalog for the Grant planner scene. + +The catalog is the single source of truth for the ``scene_json`` contract. +It is deliberately pure Python: the snapshot builder, the graph validator and +the tests all import the same table. The model never supplies its own +``skill_contracts``; only this module does. +""" +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +from iiwa_experement.scenario import Tool, ScenarioSpec + + +SCENE_SCHEMA_VERSION = 1 +CONTRACT_VERSION = 1 +CATALOG_VERSION = 1 + +NATIVE_SKILLS = ( + 'detect', 'approach', 'lower', 'grab', 'lift', 'transfer', 'align', + 'release', 'retreat', 'capture', 'verify', 'stop', 'wait_scene_clear', + 'inspect_state', 'notify_operator', 'dispense', 'dock_tool', + 'handoff_screw', +) +ALIAS_SKILLS = ('pick', 'move_to_storage', 'inspect') +AVAILABLE_SKILLS = NATIVE_SKILLS + ALIAS_SKILLS + +TOOL_LABELS = { + 'vacuum': 'вакуумная присоска', + 'magnetic_tool': 'магнитный инструмент', + 'sealant_tool': 'инструмент нанесения герметика', + 'torque_tool': 'моментный инструмент', +} +TOOL_KINDS = { + 'vacuum': 'suction_cup', + 'magnetic_tool': 'magnet', + 'sealant_tool': 'dispenser', + 'torque_tool': 'torque_wrench', +} +_TOOL_SOCKET_SUFFIX = '_socket' +_ALLOWED_HOLE_IDS = ('hole_1', 'hole_2', 'hole_3', 'hole_4') +_ALLOWED_SEALANT_SIDES = ('side_1', 'side_2', 'side_3', 'side_4') + + +class CatalogError(ValueError): + """The requested id does not exist in this scene catalog.""" + + +# ``outcomes`` drives the validator: every result of a node must select +# exactly one edge, so every event listed here must be present. +SKILL_OUTCOMES: dict[str, dict[str, str]] = { + 'detect': { + 'found': 'Объект найден, актуальная локализация получена.', + 'not_found': 'Объект не найден, движение не выполнялось.', + 'failed': 'Наблюдение недоступно.', + }, + 'approach': { + 'success': 'Движение в безопасную точку над целью выполнено.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка движения.', + }, + 'lower': { + 'success': 'Осевое опускание к контакту выполнено.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка движения.', + }, + 'grab': { + 'success': 'Захват или установка инструмента подтверждены.', + 'grasp_failed': 'Захват не подтверждён; состояние нужно проверить.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка захвата.', + }, + 'lift': { + 'success': 'Осевой подъём выполнен.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка движения.', + }, + 'transfer': { + 'success': 'Перенос к цели на безопасной высоте выполнен.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка движения.', + }, + 'align': { + 'success': 'Ориентация совмещена на безопасной высоте.', + 'failed': 'Совмещение не подтверждено.', + }, + 'release': { + 'success': 'Объект отпущен в разрешённом месте.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Отпускание не подтверждено.', + }, + 'retreat': { + 'success': 'Осевой отвод выполнен.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка движения.', + }, + 'capture': { + 'success': 'Свежий кадр получен.', + 'failed': 'Кадр не получен.', + }, + 'verify': { + 'ok': 'Результат в допуске.', + 'not_ok': 'Результат вне допуска.', + 'failed': 'Невозможно измерить результат.', + }, + 'stop': { + 'success': 'Остановка подтверждена, состояние захвата сохранено.', + 'failed': 'Остановку подтвердить не удалось.', + }, + 'wait_scene_clear': { + 'clear': 'Зона свободна, продолжение разрешено.', + 'timeout': 'Условие не наступило за отведённое время.', + 'failed': 'Невозможно подтвердить условие.', + }, + 'inspect_state': { + 'empty': 'Захват пуст, деталь доступна для новой попытки.', + 'holding': 'Нужная деталь удерживается.', + 'placed': 'Деталь на месте, захват пуст.', + 'unknown': 'Состояние владения не подтверждено.', + }, + 'notify_operator': { + 'success': 'Уведомление оператору отправлено.', + 'failed': 'Доставка уведомления не подтверждена.', + }, + 'dispense': { + 'success': 'Участок герметика пройден.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка нанесения.', + }, + 'dock_tool': { + 'success': 'Инструмент возвращён в своё гнездо.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Стыковка не подтверждена.', + }, + 'handoff_screw': { + 'success': 'Винт передан виртуальному оператору у отверстия.', + 'grasp_failed': 'Передача не подтверждена; состояние нужно проверить.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка передачи.', + }, + # Compatible aliases for graphs produced from the published datasets. + 'pick': { + 'success': 'Захват подтверждён.', + 'grasp_failed': 'Захват не подтверждён; состояние нужно проверить.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка захвата.', + }, + 'move_to_storage': { + 'success': 'Деталь установлена на опору, захват сохранён.', + 'human_in_scene': 'Шаг прерван из-за человека в зоне движения.', + 'collision': 'Исполнитель сообщил о столкновении.', + 'failed': 'Иная ошибка переноса.', + }, + 'inspect': { + 'ok': 'Результат в допуске.', + 'not_ok': 'Результат вне допуска.', + 'failed': 'Невозможно измерить результат.', + }, +} + +SKILL_DESCRIPTIONS = { + 'detect': 'Найти объект, инструмент или место назначения; поза берётся из supervisor.', + 'approach': 'Переместить рабочий кончик в безопасную точку над целью и сохранить высоту.', + 'lower': 'Опустить рабочий кончик по оси к контакту.', + 'grab': 'Присоединить инструмент из гнезда или захватить деталь после контакта.', + 'lift': 'Поднять к сохранённой безопасной высоте без повторного захвата.', + 'transfer': 'Перенести удерживаемый объект к цели на безопасной высоте.', + 'align': 'Совместить ориентацию только на безопасной высоте.', + 'release': 'Отпустить деталь или вернуть инструмент строго в его гнездо.', + 'retreat': 'Сначала осевой отвод, затем уход в нейтральную позу.', + 'capture': 'Получить свежий кадр указанной камеры.', + 'verify': 'Проверить измеренное положение или доставку.', + 'stop': 'Остановить движение, сохранив состояние захвата.', + 'wait_scene_clear': 'Ограниченно ждать разрешения продолжить.', + 'inspect_state': 'Определить пустой захват, удержание, установку или неизвестность.', + 'notify_operator': 'Уведомить оператора и не двигать робота.', + 'dispense': 'Пройти вдоль заданного участка контура с инструментом герметика.', + 'dock_tool': 'Стыковать удерживаемый инструмент в его именованном гнезде.', + 'handoff_screw': 'Поднести винт к отверстию и передать виртуальному оператору.', + 'pick': 'Алиас grab для совместимых графов.', + 'move_to_storage': 'Алиас transfer+lower к опоре для совместимых графов.', + 'inspect': 'Алиас verify для совместимых графов.', +} + +# Optional node payload keys, validated by the graph schema. Fractions only +# apply to ``dispense``. +ALLOWED_PARAMS = { + 'dispense': ('start_fraction', 'end_fraction'), +} + + +@dataclass(frozen=True) +class SkillContract: + name: str + description: str + outcomes: dict[str, str] + + def as_dict(self) -> dict[str, Any]: + return { + 'description': self.description, + 'outcomes': dict(self.outcomes), + } + + +@dataclass(frozen=True) +class SkillCatalog: + """Object and skill registry for one scenario profile.""" + + contract_version: int = CONTRACT_VERSION + catalog_version: int = CATALOG_VERSION + profile: str = 'grant' + + def skill_names(self) -> tuple[str, ...]: + return AVAILABLE_SKILLS + + def skill_contracts(self) -> dict[str, dict[str, Any]]: + return { + name: SkillContract( + name, SKILL_DESCRIPTIONS[name], SKILL_OUTCOMES[name], + ).as_dict() + for name in AVAILABLE_SKILLS + } + + def outcomes(self, skill: str) -> dict[str, str]: + if skill not in SKILL_OUTCOMES: + raise CatalogError(f'unknown skill {skill!r}') + return dict(SKILL_OUTCOMES[skill]) + + def tool_socket_id(self, tool_name: str) -> str: + return f'{tool_name}{_TOOL_SOCKET_SUFFIX}' + + def component_ids(self, spec: ScenarioSpec) -> tuple[str, ...]: + ids = ['fixture', 'body'] + if spec.cap_state != 'absent': + ids.append('cap') + if spec.tray_pose is not None: + ids.append('tray') + ids.extend(screw.name for screw in spec.screws) + return tuple(ids) + + def tool_ids(self, spec: ScenarioSpec) -> tuple[str, ...]: + return tuple(tool.name for tool in spec.tools) + + def destination_ids(self, spec: ScenarioSpec) -> tuple[str, ...]: + ids = ['work', 'hatch_seat', *_ALLOWED_HOLE_IDS] + ids.extend(self.tool_socket_id(tool.name) for tool in spec.tools) + ids.extend(spec.sealant_sides) + return tuple(ids) + + def object_ids(self, spec: ScenarioSpec) -> tuple[str, ...]: + """Every id that may appear as ``object`` in a graph.""" + return self.component_ids(spec) + self.tool_ids(spec) + + def target_ids(self, spec: ScenarioSpec) -> tuple[str, ...]: + """Every id that may appear as ``target`` in a graph.""" + ids = list(self.destination_ids(spec)) + # The object itself is a legal target for pickup/detection nodes and + # for explicit socket placement (dock_tool uses the socket id). + ids.extend(self.component_ids(spec)) + ids.extend(self.tool_ids(spec)) + return tuple(ids) + + def component_kind(self, object_id: str, spec: ScenarioSpec) -> str: + if object_id == 'fixture': + return 'fixture' + if object_id == 'body': + return 'body' + if object_id == 'cap': + if spec.cap_state == 'absent': + raise CatalogError('hatch is absent in this scene') + return 'hatch' + if object_id == 'tray': + if spec.tray_pose is None: + raise CatalogError('tray is absent in this scene') + return 'tray' + if any(screw.name == object_id for screw in spec.screws): + return 'screw' + if object_id in self.tool_ids(spec): + return 'tool' + raise CatalogError(f'unknown component {object_id!r}') + + def destination_kind(self, destination_id: str, spec: ScenarioSpec) -> str: + if destination_id == 'work': + return 'workspace' + if destination_id == 'hatch_seat': + return 'seat' + if destination_id in _ALLOWED_HOLE_IDS: + return 'hole' + if destination_id.endswith(_TOOL_SOCKET_SUFFIX): + tool = destination_id[: -len(_TOOL_SOCKET_SUFFIX)] + if tool in self.tool_ids(spec): + return 'tool_socket' + if destination_id in spec.sealant_sides: + return 'contour' + raise CatalogError(f'unknown destination {destination_id!r}') + + def tool_socket_owner(self, socket_id: str) -> str | None: + if socket_id.endswith(_TOOL_SOCKET_SUFFIX): + return socket_id[: -len(_TOOL_SOCKET_SUFFIX)] + return None + + def validate_ids(self, spec: ScenarioSpec) -> None: + objects = set(self.object_ids(spec)) + targets = set(self.target_ids(spec)) + if 'cap' in objects and 'hatch_seat' not in targets: + raise CatalogError('a present hatch must have hatch_seat') + for tool in spec.tools: + socket_id = self.tool_socket_id(tool.name) + if socket_id not in targets: + raise CatalogError(f'tool {tool.name!r} has no named socket') + for side in spec.sealant_sides: + if side not in targets: + raise CatalogError(f'sealant side {side!r} is not a destination') + + +DEFAULT_CATALOG = SkillCatalog() + + +def build_component_entry( + catalog: SkillCatalog, + spec: ScenarioSpec, + object_id: str, + state: dict[str, Any], +) -> dict[str, Any]: + kind = catalog.component_kind(object_id, spec) + entry: dict[str, Any] = { + 'id': object_id, + 'name': _component_label(kind, object_id), + 'attributes': {'kind': kind, 'movable': kind not in ('fixture', 'body')}, + 'dependencies': [], + } + if kind == 'hatch': + entry['destination'] = 'hatch_seat' + elif kind == 'screw': + entry['destination'] = '' + entry['attributes']['allowed_destinations'] = list(_ALLOWED_HOLE_IDS) + else: + entry['destination'] = '' + if object_id == 'cap': + entry['state'] = str(state.get('cap', {}).get('state', 'unknown')) + elif kind == 'screw': + screw = state.get('screws', {}).get(object_id, {}) + entry['state'] = str(screw.get('state', 'unknown')) + entry['hole'] = screw.get('hole') + return entry + + +def _component_label(kind: str, object_id: str) -> str: + labels = { + 'fixture': 'оснастка', + 'body': 'корпус', + 'hatch': 'люк', + 'tray': 'лоток с винтами', + 'screw': 'винт', + } + if kind == 'screw': + return f'винт {object_id.split("_")[-1]}' + return labels.get(kind, object_id) + + +def destination_entry( + catalog: SkillCatalog, + spec: ScenarioSpec, + destination_id: str, + state: dict[str, Any], +) -> dict[str, Any]: + kind = catalog.destination_kind(destination_id, spec) + entry: dict[str, Any] = { + 'id': destination_id, + 'name': _destination_label(kind, destination_id), + 'kind': kind, + 'capacity': 1, + } + if kind == 'hole': + hole = state.get('holes', {}).get(destination_id, {}) + entry['state'] = str(hole.get('state', 'unknown')) + entry['screw'] = hole.get('screw') + elif kind == 'tool_socket': + tool = catalog.tool_socket_owner(destination_id) + tool_state = state.get('tools', {}).get(tool, {}) + entry['state'] = ( + 'occupied' + if tool_state.get('state') == 'docked' + else 'free' + ) + entry['tool'] = tool + elif kind == 'seat': + entry['state'] = 'empty' + else: + entry['state'] = 'available' + return entry + + +def _destination_label(kind: str, destination_id: str) -> str: + if destination_id == 'work': + return 'рабочая позиция' + if destination_id == 'hatch_seat': + return 'посадочное место люка на корпусе' + if kind == 'hole': + return f'отверстие {destination_id.split("_")[-1]}' + if kind == 'tool_socket': + return f'гнездо {destination_id[: -len(_TOOL_SOCKET_SUFFIX)]}' + if kind == 'contour': + return f'участок герметика {destination_id}' + return destination_id + + +def tool_entry( + catalog: SkillCatalog, + spec: ScenarioSpec, + tool: Tool, + state: dict[str, Any], +) -> dict[str, Any]: + tool_state = state.get('tools', {}).get(tool.name, {}) + return { + 'id': tool.name, + 'name': TOOL_LABELS.get(tool.name, tool.name), + 'kind': TOOL_KINDS.get(tool.name, 'tool'), + 'socket': catalog.tool_socket_id(tool.name), + 'state': str(tool_state.get('state', 'unknown')), + } diff --git a/src/iiwa_scene_context/iiwa_scene_context/node.py b/src/iiwa_scene_context/iiwa_scene_context/node.py new file mode 100644 index 0000000..b4ee9f8 --- /dev/null +++ b/src/iiwa_scene_context/iiwa_scene_context/node.py @@ -0,0 +1,243 @@ +"""ROS facade that publishes fresh, transient-local scene snapshots.""" +from __future__ import annotations + +import json +from pathlib import Path +from typing import Any, Mapping + +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import String +from std_srvs.srv import Trigger + +from iiwa_experement.scenario import ScenarioSpec, load_scenario + +from .catalog import SkillCatalog, DEFAULT_CATALOG +from .snapshot import build_envelope, build_scene + + +STATE_TOPIC = '/iiwa_experement/object_state' +READY_SERVICE = '/iiwa_experement/scene_ready' +SCENE_SERVICE = '/iiwa_scene_context/get_scene' +SCENE_TOPIC = '/iiwa_scene_context/scene_json' +DEFAULT_FRESHNESS_SEC = 2.0 + + +def scene_qos() -> QoSProfile: + return QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + +class SceneStore: + """Pure state store: readiness, freshness and the scene revision rule. + + ``scene_revision`` advances only when the supervisor's discrete state + version changes (grab, dock, place, delivery). Pose updates of a held + object keep the same version, so they do not create a new revision. + """ + + def __init__( + self, + spec: ScenarioSpec, + catalog: SkillCatalog = DEFAULT_CATALOG, + *, + freshness_sec: float = DEFAULT_FRESHNESS_SEC, + ) -> None: + self.spec = spec + self.catalog = catalog + self.freshness_sec = float(freshness_sec) + self.scene_ready = False + self.ready_message = 'scene not ready' + self.state: dict[str, Any] | None = None + self.received_at: float | None = None + self.scene_revision = 0 + self._last_version: int | None = None + self._last_envelope: dict[str, Any] | None = None + + # ------------------------------------------------------------------ inputs + def set_scene_ready(self, ready: bool, message: str = '') -> None: + self.scene_ready = bool(ready) + self.ready_message = message or ('scene ready' if ready else 'scene not ready') + + def update(self, state: Mapping[str, Any], received_at: float) -> dict[str, Any]: + if not isinstance(state, Mapping): + raise TypeError('supervisor state must be a mapping') + data = dict(state) + version = data.get('version') + try: + version_number = int(version) + except (TypeError, ValueError) as error: + raise ValueError('supervisor state has no integer version') from error + if self._last_version is None: + self.scene_revision = 1 + elif version_number != self._last_version: + self.scene_revision += 1 + self._last_version = version_number + self.state = data + self.received_at = float(received_at) + return data + + # ------------------------------------------------------------------ output + def stale_reason(self, now: float) -> str | None: + if not self.scene_ready: + return f'scene is not ready ({self.ready_message})' + if self.state is None or self.received_at is None: + return 'no supervisor object state received yet' + if float(now) - self.received_at > self.freshness_sec: + return ( + 'supervisor object state is stale ' + f'({float(now) - self.received_at:.2f} s old)' + ) + return None + + def snapshot(self, now: float) -> tuple[dict[str, Any] | None, str | None]: + reason = self.stale_reason(now) + if reason is not None: + return None, reason + assert self.state is not None and self.received_at is not None + assert self._last_version is not None + envelope = build_envelope( + build_scene(self.spec, self.state, self.catalog), + scene_revision=self.scene_revision, + state_version=self._last_version, + stamp=float(self.received_at), + experiment_id=self.spec.experiment_id, + ) + self._last_envelope = envelope + return envelope, None + + @property + def last_envelope(self) -> dict[str, Any] | None: + return self._last_envelope + + +class SceneContextNode(Node): + def __init__(self) -> None: + super().__init__('iiwa_scene_context') + package_share = Path(get_package_share_directory('iiwa_experement')) + self.declare_parameter('experiment_id', 1) + self.declare_parameter( + 'scene_config', str(package_share / 'config' / 'scenes.yaml'), + ) + self.declare_parameter('freshness_sec', DEFAULT_FRESHNESS_SEC) + self.declare_parameter('publish_period_sec', 0.5) + self.declare_parameter('ready_poll_period_sec', 1.0) + + experiment_id = int(self.get_parameter('experiment_id').value) + scene_config = Path(str(self.get_parameter('scene_config').value)) + self.spec = load_scenario(experiment_id, scene_config) + self.store = SceneStore( + self.spec, + DEFAULT_CATALOG, + freshness_sec=float(self.get_parameter('freshness_sec').value), + ) + self._last_published: tuple[int, int] | None = None + self._ready_client = self.create_client(Trigger, READY_SERVICE) + self._ready_request_pending = False + self._service = self.create_service( + Trigger, SCENE_SERVICE, self._handle_get_scene, + ) + self._publisher = self.create_publisher(String, SCENE_TOPIC, scene_qos()) + self.create_subscription( + String, STATE_TOPIC, self._on_state, 10, + ) + self.create_timer( + max(0.05, float(self.get_parameter('ready_poll_period_sec').value)), + self._poll_ready, + ) + self.create_timer( + max(0.05, float(self.get_parameter('publish_period_sec').value)), + self._publish_if_new, + ) + self.get_logger().info( + f'scene context started for experiment {experiment_id}' + ) + + # ------------------------------------------------------------------ inputs + def _now(self) -> float: + return self.get_clock().now().nanoseconds / 1e9 + + def _on_state(self, message: String) -> None: + try: + payload = json.loads(message.data) + self.store.update(payload, self._now()) + except Exception as error: + self.get_logger().error(f'ignoring invalid object state: {error}') + return + self._publish_if_new() + + def _poll_ready(self) -> None: + if self._ready_request_pending: + return + if not self._ready_client.service_is_ready(): + self._ready_client.wait_for_service(timeout_sec=0.0) + return + self._ready_request_pending = True + future = self._ready_client.call_async(Trigger.Request()) + future.add_done_callback(self._ready_result) + + def _ready_result(self, future) -> None: + self._ready_request_pending = False + try: + response = future.result() + except Exception as error: + self.store.set_scene_ready(False, f'scene_ready call failed: {error}') + return + self.store.set_scene_ready( + bool(response.success), str(response.message or ''), + ) + + # ------------------------------------------------------------------ output + def _publish_if_new(self) -> None: + envelope, _ = self.store.snapshot(self._now()) + if envelope is None: + return + key = (int(envelope['scene_revision']), int(envelope['state_version'])) + if key == self._last_published: + return + self._last_published = key + message = String() + message.data = json.dumps(envelope, ensure_ascii=False) + self._publisher.publish(message) + + def _handle_get_scene(self, request, response): + del request + envelope, reason = self.store.snapshot(self._now()) + if envelope is None: + response.success = False + response.message = str(reason) + return response + response.success = True + response.message = json.dumps(envelope, ensure_ascii=False) + return response + + +def main(args=None) -> None: + rclpy.init(args=args) + node = None + exit_code = 0 + try: + node = SceneContextNode() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + except Exception as error: + print(f'[iiwa_scene_context] {error}') + exit_code = 1 + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + if exit_code: + raise SystemExit(exit_code) + + +if __name__ == '__main__': + main() diff --git a/src/iiwa_scene_context/iiwa_scene_context/snapshot.py b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py new file mode 100644 index 0000000..f081be5 --- /dev/null +++ b/src/iiwa_scene_context/iiwa_scene_context/snapshot.py @@ -0,0 +1,199 @@ +"""Pure snapshot builders for the external planner contract. + +Coordinates never leave this module's inputs: the model receives logical ids, +logical ownership and camera names only. Exact poses are resolved at skill +execution time from the live supervisor. +""" +from __future__ import annotations + +from typing import Any, Mapping + +from iiwa_experement.scenario import ScenarioSpec + +from .catalog import ( + CATALOG_VERSION, + CONTRACT_VERSION, + SCENE_SCHEMA_VERSION, + SkillCatalog, + build_component_entry, + destination_entry, + tool_entry, +) + + +DEFAULT_CAMERAS = { + 'top': { + 'topic': '/d455_top/image_raw', + 'role': 'overview', + 'required_for': ['detect'], + }, + 'hand': { + 'topic': '/camera/color', + 'role': 'close_inspection', + 'required_for': ['detect', 'capture'], + }, +} + +SERVICE_OBJECTS = { + 'robot': 'робот', + 'workspace': 'рабочая зона', + 'camera': 'камера', + 'operator': 'виртуальный оператор', +} + + +def _state_mapping(supervisor_state: Mapping[str, Any] | None) -> dict[str, Any]: + if supervisor_state is None: + return {} + if not isinstance(supervisor_state, Mapping): + raise TypeError('supervisor_state must be a mapping') + return dict(supervisor_state) + + +def _flags(state: Mapping[str, Any]) -> list[str]: + flags: list[str] = [] + mounted = state.get('mounted_tool') + flags.append('tool_docked' if not mounted else f'tool_mounted:{mounted}') + held = state.get('held_object') + flags.append('gripper_empty' if not held else f'holding:{held}') + cap_state = (state.get('cap') or {}).get('state') + if cap_state: + flags.append(f'hatch_{cap_state}') + return flags + + +def _component_state( + catalog: SkillCatalog, + spec: ScenarioSpec, + state: Mapping[str, Any], +) -> dict[str, Any]: + components: dict[str, Any] = {} + for object_id in catalog.component_ids(spec): + kind = catalog.component_kind(object_id, spec) + if kind == 'hatch': + components[object_id] = str( + (state.get('cap') or {}).get('state', 'unknown') + ) + elif kind == 'screw': + components[object_id] = str( + (state.get('screws') or {}).get(object_id, {}).get( + 'state', 'unknown', + ) + ) + else: + components[object_id] = 'present' + return components + + +def _tool_state( + catalog: SkillCatalog, + spec: ScenarioSpec, + state: Mapping[str, Any], +) -> dict[str, str]: + return { + name: str( + (state.get('tools') or {}).get(name, {}).get( + 'state', 'unknown', + ) + ) + for name in catalog.tool_ids(spec) + } + + +def _hole_state( + spec: ScenarioSpec, + state: Mapping[str, Any], +) -> dict[str, Any]: + del spec + holes = state.get('holes') or {} + return { + name: { + 'state': str(entry.get('state', 'unknown')), + 'screw': entry.get('screw'), + } + for name, entry in holes.items() + if isinstance(entry, Mapping) + } + + +def build_scene( + spec: ScenarioSpec, + supervisor_state: Mapping[str, Any], + catalog: SkillCatalog, +) -> dict[str, Any]: + """Build the coordinate-free ``scene_json`` object for one snapshot.""" + state = _state_mapping(supervisor_state) + catalog.validate_ids(spec) + mounted = state.get('mounted_tool') + held = state.get('held_object') + + components = [ + build_component_entry(catalog, spec, object_id, state) + for object_id in catalog.component_ids(spec) + ] + destinations = [ + destination_entry(catalog, spec, destination_id, state) + for destination_id in catalog.destination_ids(spec) + ] + tools = [ + tool_entry(catalog, spec, tool, state) + for tool in spec.tools + ] + + initial_state = { + 'tool_mounted': mounted if isinstance(mounted, str) else None, + 'held_object': held if isinstance(held, str) else None, + 'flags': _flags(state), + 'components': _component_state(catalog, spec, state), + 'tools': _tool_state(catalog, spec, state), + 'holes': _hole_state(spec, state), + 'scene_ready': bool(state.get('ready', True)), + } + + return { + 'schema_version': SCENE_SCHEMA_VERSION, + 'contract_version': CONTRACT_VERSION, + 'catalog_version': CATALOG_VERSION, + 'profile': catalog.profile, + 'experiment_id': int(spec.experiment_id), + 'domain': 'grant_assembly', + 'components': components, + 'destinations': destinations, + 'tools': tools, + 'initial_state': initial_state, + 'cameras': DEFAULT_CAMERAS, + 'available_skills': list(catalog.skill_names()), + 'skill_contracts': catalog.skill_contracts(), + 'service_objects': dict(SERVICE_OBJECTS), + 'recovery_attempts_per_component': 1, + } + + +def build_envelope( + scene: Mapping[str, Any], + *, + scene_revision: int, + state_version: int, + stamp: float, + experiment_id: int | None = None, +) -> dict[str, Any]: + """Wrap ``scene_json`` in the transport envelope used by every interface.""" + if scene_revision < 0: + raise ValueError('scene_revision must be non-negative') + if state_version < 0: + raise ValueError('state_version must be non-negative') + envelope = { + 'schema_version': SCENE_SCHEMA_VERSION, + 'contract_version': CONTRACT_VERSION, + 'catalog_version': CATALOG_VERSION, + 'experiment_id': ( + int(experiment_id) + if experiment_id is not None + else int(scene.get('experiment_id', 0)) + ), + 'state_version': int(state_version), + 'scene_revision': int(scene_revision), + 'timestamp': float(stamp), + 'scene_json': dict(scene), + } + return envelope diff --git a/src/iiwa_scene_context/package.xml b/src/iiwa_scene_context/package.xml new file mode 100644 index 0000000..889661a --- /dev/null +++ b/src/iiwa_scene_context/package.xml @@ -0,0 +1,24 @@ + + + + iiwa_scene_context + 2026.11.10 + Live scene snapshot and scene_json contract for externally supplied planner graphs + daniel + Apache-2.0 + + ament_python + + ament_index_python + iiwa_experement + python3-yaml + rclpy + std_msgs + std_srvs + + python3-pytest + + + ament_python + + diff --git a/src/iiwa_scene_context/resource/iiwa_scene_context b/src/iiwa_scene_context/resource/iiwa_scene_context new file mode 100644 index 0000000..482f450 --- /dev/null +++ b/src/iiwa_scene_context/resource/iiwa_scene_context @@ -0,0 +1 @@ +iiwa_scene_context diff --git a/src/iiwa_scene_context/setup.cfg b/src/iiwa_scene_context/setup.cfg new file mode 100644 index 0000000..936d8cb --- /dev/null +++ b/src/iiwa_scene_context/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/iiwa_scene_context +[install] +install_scripts=$base/lib/iiwa_scene_context diff --git a/src/iiwa_scene_context/setup.py b/src/iiwa_scene_context/setup.py new file mode 100644 index 0000000..c9524c9 --- /dev/null +++ b/src/iiwa_scene_context/setup.py @@ -0,0 +1,28 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'iiwa_scene_context' + +setup( + name=package_name, + version='2026.11.10', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/doc', glob('docs/*.md')), + ], + install_requires=['setuptools', 'PyYAML'], + tests_require=['pytest'], + zip_safe=True, + maintainer='daniel', + maintainer_email='grabardm@ml-dev.ru', + description='Live scene snapshot and scene_json contract for externally supplied planner graphs', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'scene_context = iiwa_scene_context.node:main', + ], + }, +) diff --git a/src/iiwa_scene_context/test/test_node.py b/src/iiwa_scene_context/test/test_node.py new file mode 100644 index 0000000..67299f4 --- /dev/null +++ b/src/iiwa_scene_context/test/test_node.py @@ -0,0 +1,104 @@ +"""Readiness, freshness and scene_revision rules of the snapshot node.""" +from __future__ import annotations + +from pathlib import Path + +from std_srvs.srv import Trigger + +from iiwa_experement.scenario import load_scenario +from iiwa_scene_context.node import SceneStore, SceneContextNode, scene_qos +from iiwa_scene_context.snapshot import build_scene + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml' + + +def state(version: int, *, mounted=None, held=None): + return { + 'version': version, + 'mounted_tool': mounted, + 'held_object': held, + 'ready': True, + 'cap': {'state': 'free'}, + 'tools': {'vacuum': {'state': 'docked'}}, + 'screws': {}, + 'holes': {}, + } + + +def make_store(): + spec = load_scenario(1, SCENES) + return SceneStore(spec, freshness_sec=2.0) + + +def test_snapshot_requires_scene_ready_and_fresh_state(): + store = make_store() + store.update(state(1), received_at=10.0) + envelope, reason = store.snapshot(10.1) + assert envelope is None + assert 'not ready' in reason + + store.set_scene_ready(True, 'scene 1: 9 objects spawned') + envelope, reason = store.snapshot(10.1) + assert reason is None + assert envelope['scene_json']['experiment_id'] == 1 + + envelope, reason = store.snapshot(13.0) + assert envelope is None + assert 'stale' in reason + + +def test_revision_advances_on_state_version_but_not_on_pose_updates(): + store = make_store() + store.set_scene_ready(True) + store.update(state(1), received_at=10.0) + assert store.snapshot(10.1)[0]['scene_revision'] == 1 + + # A pose-only publish of the same held object carries the same version. + store.update(state(1, mounted='vacuum', held='cap'), received_at=10.5) + assert store.snapshot(10.6)[0]['scene_revision'] == 1 + + # A confirmed ownership change increments the supervisor version. + store.update(state(2, mounted='vacuum', held='cap'), received_at=11.0) + assert store.snapshot(11.1)[0]['scene_revision'] == 2 + + store.update(state(3, mounted='vacuum', held=None), received_at=11.5) + assert store.snapshot(11.6)[0]['scene_revision'] == 3 + + +def test_late_subscriber_qos_is_transient_local_and_depth_one(): + qos = scene_qos() + assert qos.durability.name == 'TRANSIENT_LOCAL' + assert qos.depth == 1 + assert qos.reliability.name == 'RELIABLE' + + +def test_scene_service_returns_json_or_a_reason(): + store = make_store() + node = object.__new__(SceneContextNode) + node.store = store + node._now = lambda: 10.1 + + response = Trigger.Response() + result = SceneContextNode._handle_get_scene(node, None, response) + assert result.success is False + assert 'not ready' in result.message + + store.set_scene_ready(True) + store.update(state(1), received_at=10.0) + response = Trigger.Response() + result = SceneContextNode._handle_get_scene(node, None, response) + assert result.success is True + assert '"scene_json"' in result.message + assert '"scene_revision": 1' in result.message + + +def test_snapshot_scene_is_the_catalog_scene_not_the_state(): + store = make_store() + store.set_scene_ready(True) + store.update(state(1), received_at=10.0) + envelope, _ = store.snapshot(10.1) + expected = build_scene(store.spec, store.state, store.catalog) + assert envelope['scene_json'] == expected + assert 'pose' not in envelope['scene_json'] diff --git a/src/iiwa_scene_context/test/test_snapshot.py b/src/iiwa_scene_context/test/test_snapshot.py new file mode 100644 index 0000000..dab466a --- /dev/null +++ b/src/iiwa_scene_context/test/test_snapshot.py @@ -0,0 +1,176 @@ +"""Scene snapshots expose only present ids and logical state.""" +from __future__ import annotations + +from pathlib import Path + +import pytest + +from iiwa_experement.scenario import load_scenario +from iiwa_scene_context.catalog import DEFAULT_CATALOG +from iiwa_scene_context.snapshot import build_envelope, build_scene + + +PACKAGE = Path(__file__).resolve().parents[1] +ROOT = PACKAGE.parent +SCENES = ROOT / 'iiwa_experement' / 'config' / 'scenes.yaml' +FORBIDDEN_KEYS = { + 'pose', 'poses', 'xyz', 'rotation', 'tcp', 'objects', 'simulation_time', + 'last_command_result', 'last_error', 'attached', 'version', +} + + +def spec_for(experiment_id: int): + return load_scenario(experiment_id, SCENES) + + +def state_for(spec, *, version=3, mounted=None, held=None, cap=None, + tool_states=None, screws=None, holes=None): + cap_entry = cap if cap is not None else {'state': spec.cap_state} + tools = {} + for tool in spec.tools: + tools[tool.name] = { + 'state': (tool_states or {}).get(tool.name, 'docked'), + } + screw_entries = {} + for screw in spec.screws: + screw_entries[screw.name] = { + 'state': (screws or {}).get(screw.name, 'tray'), + 'hole': None, + } + hole_entries = holes or { + name: {'state': 'empty', 'screw': None} + for name in ('hole_1', 'hole_2', 'hole_3', 'hole_4') + } + return { + 'version': version, + 'experiment_id': spec.experiment_id, + 'mounted_tool': mounted, + 'held_object': held, + 'ready': True, + 'cap': cap_entry, + 'tools': tools, + 'screws': screw_entries, + 'holes': hole_entries, + } + + +def ids(entries): + return [entry['id'] for entry in entries] + + +def walk_keys(value): + if isinstance(value, dict): + for key, nested in value.items(): + yield key + yield from walk_keys(nested) + elif isinstance(value, list): + for item in value: + yield from walk_keys(item) + + +@pytest.mark.parametrize('experiment_id', [1, 20]) +def test_scene_lists_only_present_objects_and_goals(experiment_id): + spec = spec_for(experiment_id) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + components = ids(scene['components']) + destinations = ids(scene['destinations']) + assert 'fixture' in components and 'body' in components + assert ('cap' in components) == (spec.cap_state != 'absent') + assert ('tray' in components) == (spec.tray_pose is not None) + assert all(f'screw_{index}' in components for index in range(1, 11)) == ( + spec.tray_pose is not None + ) + assert 'work' in destinations + assert 'hatch_seat' in destinations + assert {'hole_1', 'hole_2', 'hole_3', 'hole_4'} <= set(destinations) + for tool in spec.tools: + assert f'{tool.name}_socket' in destinations + assert set(ids(scene['tools'])) == set(spec.tool_names) + for side in spec.sealant_sides: + assert side in destinations + for index in range(1, 5): + if f'side_{index}' not in spec.sealant_sides: + assert f'side_{index}' not in destinations + if spec.tray_pose is None: + assert not any(name.startswith('screw_') for name in components) + else: + assert len([name for name in components if name.startswith('screw_')]) >= 10 + + +def test_each_present_tool_has_a_named_socket_and_stable_ids(): + for experiment_id in (1, 20): + spec = spec_for(experiment_id) + first = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + second = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + assert first['components'] == second['components'] + assert first['destinations'] == second['destinations'] + tool_ids = [tool['id'] for tool in first['tools']] + assert tool_ids == list(spec.tool_names) + for tool in first['tools']: + assert tool['socket'] == f"{tool['id']}_socket" + assert tool['socket'] in ids(first['destinations']) + + +def test_scene_json_has_no_coordinates_or_supervisor_internals(): + spec = spec_for(20) + state = state_for(spec, mounted='vacuum', held='cap', cap={'state': 'held'}) + state['tcp'] = {'xyz': [1, 2, 3]} + state['objects'] = {'grant_cap': {'xyz': [0, 0, 0]}} + state['simulation_time'] = 12.5 + scene = build_scene(spec, state, DEFAULT_CATALOG) + keys = set(walk_keys(scene)) + assert not (keys & FORBIDDEN_KEYS) + assert 'timestamp' not in keys + assert scene['experiment_id'] == 20 + assert scene['contract_version'] == 1 + assert scene['catalog_version'] == 1 + + +def test_logical_state_reflects_the_live_supervisor_snapshot(): + spec = spec_for(20) + scene = build_scene( + spec, + state_for( + spec, + mounted='vacuum', + held='cap', + cap={'state': 'held'}, + screws={'screw_1': 'delivered'}, + holes={'hole_1': {'state': 'empty', 'screw': 'screw_1'}}, + ), + DEFAULT_CATALOG, + ) + initial = scene['initial_state'] + assert initial['tool_mounted'] == 'vacuum' + assert initial['held_object'] == 'cap' + assert initial['tools']['vacuum'] == 'docked' + assert initial['components']['cap'] == 'held' + assert initial['components']['screw_1'] == 'delivered' + assert initial['holes']['hole_1']['screw'] == 'screw_1' + assert 'tool_mounted:vacuum' in initial['flags'] + assert 'holding:cap' in initial['flags'] + + +def test_scene_json_carries_server_skill_contracts_not_model_contracts(): + spec = spec_for(1) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + assert 'grab' in scene['available_skills'] + assert 'tighten' not in scene['available_skills'] + assert set(scene['skill_contracts']) == set(scene['available_skills']) + assert scene['skill_contracts']['grab']['outcomes']['grasp_failed'] + assert 'grasp_failed' in scene['skill_contracts']['pick']['outcomes'] + + +def test_envelope_keeps_transport_separate_from_scene(): + spec = spec_for(20) + scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG) + envelope = build_envelope( + scene, scene_revision=7, state_version=4, stamp=123.5, + ) + assert envelope['experiment_id'] == 20 + assert envelope['state_version'] == 4 + assert envelope['scene_revision'] == 7 + assert envelope['timestamp'] == 123.5 + assert envelope['scene_json'] == scene + assert envelope['scene_json'] is not envelope + assert envelope['scene_json']['experiment_id'] == 20