feat(scene-context): publish the live coordinate-free scene contract
This commit is contained in:
Даниил Грабарь
@@ -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
|
||||
`<tool>_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.
|
||||
@@ -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',
|
||||
]
|
||||
@@ -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')),
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>iiwa_scene_context</name>
|
||||
<version>2026.11.10</version>
|
||||
<description>Live scene snapshot and scene_json contract for externally supplied planner graphs</description>
|
||||
<maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>iiwa_experement</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1 @@
|
||||
iiwa_scene_context
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/iiwa_scene_context
|
||||
[install]
|
||||
install_scripts=$base/lib/iiwa_scene_context
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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']
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user