change validator
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,69 @@
|
||||
# Интеграция графов LLM в штатный запуск `cobot`
|
||||
|
||||
> Для код-агента. Реализуй задачи по порядку в ветке `experement` по текущей рабочей копии. Здесь описаны изменения и критерии приёмки.
|
||||
|
||||
После `cobot run` робот должен принимать готовый JSON-граф LLM и перемещать установленный оператором инструмент в нужную точку. Обычный Webots использует `digital_twin.webots.world`, а запуск контроллера использует FRI. Опыты Grant по-прежнему запускаются в Webots через существующие launch-файлы с явным номером опыта. Внешний клиент вызывает LLM и отправляет готовый граф.
|
||||
|
||||
| Запуск | Исполнитель | Источник контекста для LLM |
|
||||
| --- | --- | --- |
|
||||
| `cobot run local` или `cobot run docker` → «Симулятор Webots» | Базовые перемещения через штатный MoveIt | `iiwa_scene_context` публикует снимок базовой сцены |
|
||||
| `cobot run local` или `cobot run docker` → «Контроллер» | Те же перемещения через штатный MoveIt и FRI при включении в YAML | Именованные позиции и телеметрия робота через штатные интерфейсы |
|
||||
| `ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20` | Существующие навыки Grant | Существующий `scene_json` от Grant Supervisor |
|
||||
|
||||
Для первой версии предлагаю поддержать именованную позицию и явную TCP-позу, включать исполнение графов на физическом роботе отдельной настройкой и покрыть обе среды `cobot run`. Навыки захвата, переноса объектов, стыковки инструмента и технологических операций входят только в профиль Grant. На физическом роботе `scene_json` пока отсутствует. Имя инструмента, его TCP, коллизионная модель и нагрузка берутся из действующей конфигурации робота и контроллера.
|
||||
|
||||
## 1. Подключить узлы к штатному запуску
|
||||
|
||||
- [ ] Добавить в `cobot-setting.yaml` и `src/iiwa_config/config/setting.yaml` одинаковый блок `llm_planner`. В блок входят `enabled: true`, `hardware_enabled: false` и `velocity_scale: 0.10`. Значение масштаба `0.10` служит начальным значением для Webots. Для физического прогона оператор задаёт согласованное значение. В `src/iiwa_utils/iiwa_utils/setting_loader.py` добавить типизированное чтение блока, строгие булевы значения и конечный масштаб в диапазоне `(0, 1]`. Отсутствие блока в старом YAML означает выключенную интеграцию. При выключенном `enabled` штатный запуск работает независимо от `hardware_enabled`.
|
||||
- [ ] Создать `src/iiwa_llm_plan_executor/launch/cobot.launch.py` с аргументами `setting`, `simulate`, `rviz`. Включить `iiwa_bringup/iiwa.launch.py` один раз с теми же аргументами. При `enabled: true` и `simulate: true` добавить `scene_context` и `llm_plan_executor` в базовом режиме с `use_sim_time: true`. При `simulate: false` и двух включённых флагах добавить только исполнитель с `use_sim_time: false`. В обоих режимах использовать настройки и пути штатного bringup. Проверять конфигурацию до создания процессов.
|
||||
- [ ] Использовать общий конструктор команды запуска в `cobot/commands/run.py` для `_run_local` и `_run_docker`. Сохранить меню, подключение ROS overlay, передачу `simulate`, монтирование `cobot-setting.yaml` и остановку дочернего дерева. Для устаревшей сборки вывести команду `cobot rebuild --symlink-install` или `cobot docker-setup` и вернуть ошибку запуска.
|
||||
- [ ] Объявить зависимости нового launch в `src/iiwa_llm_plan_executor/package.xml` и проверить установку через `setup.py`. Оставить направление зависимости от нового launch к `iiwa_bringup`. Обратная зависимость образует цикл `iiwa_bringup → iiwa_llm_plan_executor → iiwa_experement → iiwa_bringup`. Существующие `planner.launch.py` и `experiment.launch.py` продолжают включать низкоуровневый bringup и свои узлы Grant.
|
||||
- [ ] Тесты в `cobot/test/` и `src/iiwa_llm_plan_executor/test/` покрывают матрицу Webots/FRI × флаги, старый и неверный YAML, аргументы local/Docker и единственный экземпляр контроллера, MoveIt и `iiwa_web`. При запуске обычного Webots отсутствуют процессы `scene_spawner`, `grant_supervisor` и `reference_runner`. Для существующего launch Grant сохранить прежние тесты, включая отказ при `simulate:=false`.
|
||||
|
||||
## 2. Принимать базовые графы без сцены Grant
|
||||
|
||||
- [ ] В `src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/{node,engine,validator,skills}.py` выделить режим `base` с собственными проверками и двумя навыками. Сохранить общий разбор JSON, публикацию оригинала, `run_id`, повторную отправку, один активный запуск, статус, журнал и отмену. Сценозависимые проверки ревизии, владения и готовности оставить в режиме Grant. В `base` определять готовность по актуальному полному `/joint_states`, готовности штатного сервера движения и активному контроллеру. Проверять состояние перед запуском графа и перед каждым шагом. Выход FRI из рабочего состояния, потеря обратной связи или недоступный контроллер прерывают граф и запрещают следующий шаг. Учитывать уже реализованную обработку `COMMANDING_ACTIVE` в `iiwa_controller`.
|
||||
- [ ] Сохранить формат `status`, `entry`, `plan`, `transitions` и интерфейс `/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/srv/SubmitPlan`). Для `move_to_named` поле `target` задаёт имя `group_state` из активного SRDF для `planning.planning_group`. Для `move_to_pose` требовать ровно шесть чисел `x`, `y`, `z` в метрах и `a`, `b`, `c` в радианах в `params`. Углы задаются в порядке ZYX по соглашению KUKA ABC, фрейм фиксирован `base_link`, TCP-линк берётся из `planning.pose_link`. Для обоих навыков `object` пустой. У `move_to_pose` поле `target` также пустое. Имена, числа, допустимые поля и переходы проверить для всех узлов при приёме. Отклонять неизвестные команды, ссылки на объекты Grant, `NaN`/`Inf`, внешние фреймы и попытки переопределить масштаб скорости. Публиковать результат шага `success` или `motion_failed` по результату сервера. Для `motion_failed` разрешать только переход в `operator_required` или `replan_required`. При отсутствии такого перехода завершать граф как `failed` с причиной.
|
||||
- [ ] Передавать TCP-цель в штатный action `cobot/move_to_pose` (`iiwa_msgs/action/MoveToPose`). Для именованного движения добавить отменяемый `cobot/move_to_named_pose` (`iiwa_msgs/action/MoveToNamed`) в `src/iiwa_planning/scripts/move_to_pose_server.py` и `src/iiwa_msgs`. Вынести существующее планирование именованной цели из `cobot/move_to_named` в общий участок, чтобы оба интерфейса использовали SRDF, Pilz PTP, лимиты и `MotionSerialiser`. Штатный сервис сохраняет контракт. Для TCP-цели применять настроенный планировщик и масштаб, согласованные с MoveIt. Успех шага требует успешного результата action, включая выполнение траектории контроллером.
|
||||
- [ ] Подключить отмену графа к goal handle MoveIt и штатной остановке. Проверить окно между отправкой цели и получением goal handle, потерю контроллера и повторный submit после отмены. `POST /stop` должен остановить граф, отправленный через ROS, HTTP или MCP. Текущий `_active_run_id` в `src/iiwa_web/iiwa_web/planner.py` учитывает только HTTP-запросы. Источник активного запуска и его отмены должен находиться в executor. Штатный `/cobot/stop` должен инвалидировать активную операцию и ожидающие шаги графа. В базовом режиме графы и обычные запросы MoveIt проходят через одну сериализацию. Проверить, что прямой HTTP-ввод траекторий не перебивает активный граф.
|
||||
- [ ] Добавить тесты базового графа с подменой ROS action-сервера. Покрыть успешные именованную и TCP-цели, отсутствие `scene_json` на FRI, отказ при неизвестном имени и некорректных координатах, `not_ready` при устаревших суставах, ошибку контроллера, отмену до принятия цели и в процессе исполнения, дубликат и параллельный запуск. Прогнать существующие тесты Grant и `test_motion_priority.py`.
|
||||
|
||||
## 3. Выдавать честный `scene_json` в базовом Webots
|
||||
|
||||
- [ ] В `src/iiwa_scene_context/iiwa_scene_context/{node,snapshot}.py` добавить режим `base`. Источник актуальности базового снимка — `/joint_states` и TF из `base_link` к настроенному TCP, если трансформ доступен. Публиковать тип базового профиля, метку времени, наблюдаемое состояние, доступные базовые навыки и имена поз из активного SRDF. В пустом мире набор наблюдаемых объектов задачи пуст. У базового снимка нет номера опыта. Отсутствующие данные о камерах, объектах и установленном инструменте обозначать явно. Сцена без объектов задачи не подтверждает отсутствие препятствий для MoveIt.
|
||||
- [ ] Сохранить `/iiwa_scene_context/get_scene`, QoS `RELIABLE`/`TRANSIENT_LOCAL` для `/iiwa_scene_context/scene_json` и действующий формат Grant. Базовый контекст не подписывается на `/iiwa_experement/object_state` и не ждёт `/iiwa_experement/scene_ready`. Возвращать понятный отказ до первого валидного состояния и при его устаревании. Для FRI сервис сцены не запускается, а HTTP/MCP сообщает, что `scene_json` доступен только в Webots. Маршруты отправки, статуса и отмены работают при включённом `web.enabled` в обоих режимах.
|
||||
- [ ] Тесты проверяют свежесть и содержимое пустой сцены без Grant, достоверность источников полей и неизменный контракт сцены опыта. Проверить, что пропажа базового `scene_json` сама по себе не блокирует допустимую команду при наличии актуальной телеметрии и готового MoveIt.
|
||||
|
||||
## 4. Собрать и проверить запуск из установленных пакетов
|
||||
|
||||
- [ ] В обоих `docker/jazzy/{ros-iiwa7,ros-iiwa7-webots}/Dockerfile` добавить исходники `iiwa_experement`, `iiwa_scene_context` и `iiwa_llm_plan_executor` в контекст перед `rosdep` и `colcon`. Проверить зависимости из `package.xml` и доступность launch, конфигов и ROS-интерфейсов в release-образах после удаления `src`. Docker для контроллера не требует приложения Webots. Через `cobot docker-setup` собрать оба образа. Проверить `cobot run docker` на новом Webots-образе и стенде FRI с имитацией аппаратуры.
|
||||
- [ ] С ROS 2 Jazzy запустить `cobot rebuild --symlink-install`, подключить `install/setup.bash` и выполнить `colcon test --base-paths src` и `colcon test-result --verbose`. Для общего запуска `pytest` по нескольким пакетам использовать `--import-mode=importlib` из-за совпадающих имён `test_launch.py`. Сверить результаты с исходным состоянием рабочей копии, включая незакоммиченный `src/iiwa_msgs/srv/SubmitPlan.srv`.
|
||||
- [ ] В обычном Webots запустить `cobot run local` → «Симулятор Webots». Проверить один набор штатных узлов и базовый снимок через `ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger '{}'`. Сначала отправить отклоняемый граф и убедиться, что контроллер не получил цель. Затем отправить разрешённую именованную цель и допустимую TCP-цель, выбранную для установленной модели и сцены. Для обеих сверить `/plan`, `/status`, фактическую позу и `completed`. Отдельно проверить отмену, общий stop и чистое завершение по Ctrl-C. Повторить на Docker. Проверить, что ручной запуск Grant с `experiment_id:=20` отдаёт прежнюю сцену и принимает прежний граф.
|
||||
- [ ] Для FRI сначала проверить ветку запуска с имитацией action-сервера и контроллера либо с тестовым FRI-стендом из `src/iiwa_controller/test/`. Убедиться, что нет Webots и сервиса сцены, выключенная настройка исключает executor, а при включении граф до готовности возвращает `not_ready`. Физическую приёмку выполнять с оператором после проверки сети, активного режима FRI, лимитов, модели инструмента, Load Data и рабочего пространства. Начать с одной заранее проверенной именованной позиции на согласованной скорости. В отчёте указать отдельно результаты Webots, FRI-имитации и физической проверки.
|
||||
- [ ] Описать операторский запуск и примеры графов в `src/iiwa_llm_plan_executor/doc/README.md`, контракт базовой сцены в `src/iiwa_scene_context/docs/interfaces.md` и настройки в `doc/lwc-doc/docs/getting-started/configuration{,.en}.md`.
|
||||
|
||||
Пример тела `POST /planner/runs` для Webots. Перед запуском проверьте, что `work` есть в активном SRDF и применим к текущей сцене.
|
||||
|
||||
```json
|
||||
{
|
||||
"run_id": "base-webots-1",
|
||||
"response": {
|
||||
"status": "ready",
|
||||
"entry": "go",
|
||||
"plan": [{
|
||||
"id": "go",
|
||||
"skill": "move_to_named",
|
||||
"target": "work",
|
||||
"transitions": [
|
||||
{"event": "success", "next": "completed"},
|
||||
{"event": "motion_failed", "next": "operator_required"}
|
||||
]
|
||||
}]
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
## Ссылки для реализации
|
||||
|
||||
- Проектные контракты по запуску, API и остановке. [Deployment](../../../openspec/specs/deployment/spec.md), [External API](../../../openspec/specs/external-api/spec.md), [Motion arbitration](../../../openspec/specs/motion-arbitration/spec.md).
|
||||
- Документация ROS 2 Jazzy по вложенным launch-файлам и зависимостям. [Launch](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Launch/Using-ROS2-Launch-For-Large-Projects.rst), [rosdep](https://github.com/ros2/ros2_documentation/blob/jazzy/source/Tutorials/Intermediate/Rosdep.rst).
|
||||
@@ -466,8 +466,10 @@ class PlanningSceneBridge:
|
||||
use_sim_time: bool = True,
|
||||
srdf_path: str | None = None,
|
||||
spin_fn: Callable[[float], None] | None = None,
|
||||
on_timing: Callable[[dict], None] | None = None,
|
||||
) -> None:
|
||||
self._owns_node = node is None
|
||||
self._on_timing = on_timing
|
||||
if node is None:
|
||||
node = Node('planning_scene_bridge')
|
||||
node.set_parameters([Parameter(
|
||||
@@ -571,7 +573,10 @@ class PlanningSceneBridge:
|
||||
self._seen_world.clear()
|
||||
self._seen_attached.clear()
|
||||
self._publisher.publish(scene)
|
||||
self._spin()
|
||||
# A short non-blocking turn lets rclpy deliver the message to local
|
||||
# subscribers. The authoritative synchronization check happens in
|
||||
# assert_synchronized() through /get_planning_scene.
|
||||
self._spin_once(0.0)
|
||||
|
||||
def _fetch_acm(self) -> None:
|
||||
if getattr(self, '_get_scene', None) is None or self.node is None:
|
||||
@@ -632,31 +637,20 @@ class PlanningSceneBridge:
|
||||
return scene
|
||||
|
||||
def _refresh_joint_state(self) -> None:
|
||||
"""Wait for the next /joint_states sample before a full scene update.
|
||||
"""Make sure a measured joint sample exists before a full scene update.
|
||||
|
||||
A full PlanningScene carries a robot state. Using a sample captured
|
||||
before a preceding motion would make the MoveItPy planner start from
|
||||
an old pose on the next trajectory.
|
||||
The full scene is published with ``robot_state.is_diff=True`` so it
|
||||
never overwrites MoveIt's current state. Waiting for the *next*
|
||||
sample on every scene update added a fixed delay between motions
|
||||
without improving that guarantee; the executor already checks joint
|
||||
state freshness immediately before every plan.
|
||||
"""
|
||||
if self.node is None:
|
||||
if self.node is None or self._latest_joint_state is not None:
|
||||
return
|
||||
previous = None
|
||||
if self._latest_joint_state is not None:
|
||||
previous = (
|
||||
self._latest_joint_state.header.stamp.sec,
|
||||
self._latest_joint_state.header.stamp.nanosec,
|
||||
)
|
||||
end = time.monotonic() + 0.5
|
||||
end = time.monotonic() + 0.1
|
||||
while time.monotonic() < end:
|
||||
self._spin_once(0.02)
|
||||
current = self._latest_joint_state
|
||||
if current is None:
|
||||
continue
|
||||
stamp = (
|
||||
current.header.stamp.sec,
|
||||
current.header.stamp.nanosec,
|
||||
)
|
||||
if previous is None or stamp != previous:
|
||||
self._spin_once(0.01)
|
||||
if self._latest_joint_state is not None:
|
||||
return
|
||||
|
||||
def _relay_full_scene(self) -> None:
|
||||
@@ -667,11 +661,30 @@ class PlanningSceneBridge:
|
||||
move_group and republishing it once keeps both planners identical
|
||||
without a feedback loop.
|
||||
"""
|
||||
started = time.monotonic()
|
||||
relayed = False
|
||||
try:
|
||||
relayed = self._relay_full_scene_impl()
|
||||
finally:
|
||||
on_timing = getattr(self, '_on_timing', None)
|
||||
if on_timing is not None:
|
||||
try:
|
||||
on_timing({
|
||||
'stage': 'planning_scene_relay',
|
||||
'started_at': started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - started,
|
||||
'relayed': relayed,
|
||||
})
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _relay_full_scene_impl(self) -> bool:
|
||||
if getattr(self, '_get_scene', None) is None or self.node is None:
|
||||
return
|
||||
return False
|
||||
if not self._get_scene.service_is_ready():
|
||||
if not self._get_scene.wait_for_service(timeout_sec=1.0):
|
||||
return
|
||||
return False
|
||||
expected_world = set(self._world_objects)
|
||||
expected_attached = set(self._attached)
|
||||
deadline = time.monotonic() + 3.0
|
||||
@@ -715,10 +728,10 @@ class PlanningSceneBridge:
|
||||
scene.robot_state.is_diff = True
|
||||
self._publisher.publish(scene)
|
||||
self._spin()
|
||||
return
|
||||
return True
|
||||
# Never relay a stale scene: that would wipe the objects from the
|
||||
# MoveItPy monitor and make the next motion unverified.
|
||||
return
|
||||
return False
|
||||
|
||||
def _publish_full(self) -> None:
|
||||
self._refresh_joint_state()
|
||||
@@ -913,8 +926,9 @@ class PlanningSceneBridge:
|
||||
last_world, last_attached = snapshot
|
||||
if world_expected <= last_world and attached_expected <= last_attached:
|
||||
if not served:
|
||||
# The service proves move_group; still let the next
|
||||
# monitored message confirm the second monitor.
|
||||
# move_group already has the scene, but MoveItPy's
|
||||
# monitor also needs to have seen the same ids before
|
||||
# the next plan is allowed.
|
||||
continue
|
||||
return
|
||||
elif served:
|
||||
|
||||
@@ -1,5 +1,23 @@
|
||||
# Executing external Qwen3 graphs in Webots
|
||||
|
||||
## Краткая инструкция (русский)
|
||||
|
||||
```bash
|
||||
source /opt/ros/jazzy/setup.bash
|
||||
colcon build --base-paths src --symlink-install
|
||||
source install/setup.bash
|
||||
ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
|
||||
```
|
||||
|
||||
Дождитесь готовности сцены, прочитайте `/planner/scene` или
|
||||
`/iiwa_scene_context/get_scene`, передайте `scene_json` и инструкцию Qwen3
|
||||
снаружи, затем отправьте готовый граф в
|
||||
`/iiwa_llm_plan_executor/submit_plan` (`iiwa_msgs/SubmitPlan`) или через
|
||||
`POST /planner/runs` с телом `{"run_id": "...", "response": {...}}`.
|
||||
`scene_revision` не передаётся и не возвращается. Отклонённый запрос не
|
||||
регистрируется, поэтому тот же `run_id` можно использовать после исправления
|
||||
графа. Подробности: `docs/interfaces.md`.
|
||||
|
||||
This package accepts a **ready-made** JSON graph. It does not call Qwen3, does
|
||||
not evaluate the model and does not compare runs with a reference sequence.
|
||||
The experiment number selects only the initial Webots scene.
|
||||
@@ -35,17 +53,22 @@ ros2 topic echo /iiwa_scene_context/scene_json --qos-durability transient_local
|
||||
```
|
||||
|
||||
Give `scene_json` and your instruction to Qwen3 outside this project. Submit
|
||||
the answer, the `scene_revision` from the same snapshot and a client-chosen
|
||||
`run_id`:
|
||||
the answer and a client-chosen `run_id`; the executor obtains and validates its
|
||||
own fresh scene snapshot, so no scene revision travels through the API:
|
||||
|
||||
```bash
|
||||
ros2 service call /iiwa_llm_plan_executor/submit webots_ros2_msgs/srv/SetString \
|
||||
"{value: '{\"run_id\":\"demo-1\",\"scene_revision\":1,\"response\":{\"status\":\"ready\",\"entry\":\"detect\",\"plan\":[...]}}'}"
|
||||
ros2 service call /iiwa_llm_plan_executor/submit_plan iiwa_msgs/srv/SubmitPlan \
|
||||
"{run_id: 'demo-1', response: '{\"status\":\"ready\",\"entry\":\"detect\",\"plan\":[...]}'}"
|
||||
|
||||
ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local
|
||||
ros2 topic echo /iiwa_llm_plan_executor/status
|
||||
```
|
||||
|
||||
The legacy `/iiwa_llm_plan_executor/submit` (`webots_ros2_msgs/SetString`) still
|
||||
routes into the same admission path. Rejections return `code` and `reason`
|
||||
from `SubmitPlan`; a rejected `run_id` is not registered and can be reused with
|
||||
a corrected graph.
|
||||
|
||||
The accepted graph is published **unchanged** in
|
||||
`/iiwa_llm_plan_executor/plan` before the first movement. A rejected graph
|
||||
never appears there; read the reason from
|
||||
@@ -64,7 +87,7 @@ HTTP (existing `iiwa_web` server; default port 8007):
|
||||
curl http://localhost:8007/planner/scene
|
||||
curl -X POST http://localhost:8007/planner/runs \
|
||||
-H 'Content-Type: application/json' \
|
||||
-d '{"run_id":"demo-1","scene_revision":1,"response":"<Qwen JSON string>"}'
|
||||
-d '{"run_id":"demo-1","response":"<Qwen JSON string>"}'
|
||||
curl http://localhost:8007/planner/runs/demo-1
|
||||
curl -X POST http://localhost:8007/planner/runs/demo-1/cancel
|
||||
```
|
||||
@@ -75,12 +98,14 @@ The same four operations are MCP tools built from the same router
|
||||
|
||||
## Nodes and terminals
|
||||
|
||||
Only one graph is active per robot. Repeating the same `run_id` is
|
||||
idempotent; a different `run_id` while active is rejected (`busy`).
|
||||
Only one graph is active per robot. Repeating an identical accepted graph is
|
||||
idempotent (`duplicate`); another graph with the same accepted `run_id` is a
|
||||
`conflict`; a different `run_id` while a run is active is rejected (`busy`).
|
||||
Accepted run ids stay bound after every terminal state.
|
||||
|
||||
Status events contain the run id, node id, skill, result event, the chosen
|
||||
next node, the scene revision, the mounted tool, the held object and, at the
|
||||
end, the terminal. Terminals:
|
||||
next node, the mounted tool, the held object and, at the end, the terminal.
|
||||
The internal scene revision is never exposed. Terminals:
|
||||
|
||||
* `completed` — the submitted graph and its checked postconditions finished.
|
||||
* `replan_required` — the graph stopped and a fresh snapshot was emitted; the
|
||||
|
||||
@@ -17,11 +17,12 @@ colcon test --base-paths src --packages-select \
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
Result: build of 10 packages succeeded; `365 tests, 0 errors, 0 failures,
|
||||
0 skipped` for the four runtime packages. The new camera/tool geometry tests
|
||||
in `iiwa_utils` pass separately (`7 passed`); four pre-existing
|
||||
`test_body_hatch_worlds.py` cases still fail because their `.wbt`/`.proto`
|
||||
files are absent from this checkout.
|
||||
Result: the five affected packages build successfully; `colcon test` reports
|
||||
`402 tests, 0 errors, 0 failures, 0 skipped` for `iiwa_msgs`,
|
||||
`iiwa_experement`, `iiwa_scene_context`, `iiwa_llm_plan_executor` and
|
||||
`iiwa_web`. Four pre-existing `test_body_hatch_worlds.py` cases in other
|
||||
packages still fail because their `.wbt`/`.proto` files are absent from this
|
||||
checkout; they are unrelated to this change.
|
||||
|
||||
`git diff --check` is clean.
|
||||
|
||||
@@ -33,8 +34,9 @@ ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
|
||||
|
||||
* 19 scene objects spawned (fixture, body, cap, tray, 10 screws, 4 tools).
|
||||
* `/iiwa_experement/scene_ready` true; `/iiwa_scene_context/get_scene` served
|
||||
`contract_version=1`, `catalog_version=1`, `scene_revision=1`,
|
||||
`state_version=0` with all present ids and no coordinates.
|
||||
`schema_version=2`, `contract_version=2`, `catalog_version=2`,
|
||||
`state_version=0`, live poses and action rules. `scene_revision` is not part
|
||||
of the public envelope.
|
||||
* The camera contract was corrected to the topics actually published by the
|
||||
existing Webots camera plugins: `/d455_top/image_raw/image_color` and
|
||||
`/camera/color/image_color`.
|
||||
@@ -54,12 +56,77 @@ client-supplied `run_id`; the accepted graph appeared in
|
||||
| `transfer(vacuum,vacuum_socket) → dock_tool(vacuum,vacuum_socket) → completed` | `completed`; status ended with `mounted_tool: null`. This was a separate graph after the previous one. |
|
||||
| `grab(cap) → transfer(cap,hatch_seat) → align → release(cap,hatch_seat)` (cup already mounted) | Cap grasp, transfer, align and release succeeded; the supervisor reported the hatch installed. The final `verify` first failed because of a web-wrapper attribute shadowing bug (`latest`), which was fixed afterwards; a clean repeat of the whole cap graph was not obtained in this session. |
|
||||
| cap graph without explicit `grab(vacuum)`, with a `grasp_failed` edge | Admitted; execution returned `grasp_failed` with `reason: "required tool vacuum is not mounted"`, `next: "operator_required"`, `mounted_tool: null`, `held_object: null`. The supervisor state stayed at version 0 with the cap `free` and `vacuum` `docked`; the log contains no planner call for that run. `/plan` held the original graph, `/status`, `GET /planner/runs/...` and the JSONL log held the actual event/reason/ownership/next edge. |
|
||||
| stale `scene_revision=999` | Rejected: `stale_scene: submitted revision 999 does not match current revision 1`. |
|
||||
| invalid graph with a stray node key | `POST /planner/runs` returned 422 with `code=invalid` and the exact schema reason; `GET /planner/runs/<id>` returned 404 and no JSONL run log was created. The corrected graph with the **same** `run_id` was accepted, then `completed`. |
|
||||
| different graph with an already accepted `run_id` | 409 `code=conflict`; the original run and its single `/plan` publication were unchanged. |
|
||||
| identical accepted graph repeated with the same `run_id` | 200 `code=duplicate`; no second motion. |
|
||||
|
||||
The screw handoff graph (`grab(magnetic_tool) → grab(screw_1) →
|
||||
transfer(screw_1,hole_1) → handoff_screw`) is covered by package tests but was
|
||||
not completed in this Webots session because the preceding cap/cartesian
|
||||
planner runs were not repeatable within the remaining time.
|
||||
The screw handoff graph in experiment 11 was attempted in Webots. The tool
|
||||
grab succeeded, but the subsequent screw approach was rejected by the strict
|
||||
motion policy: `screw_1` failed at `tcp tilt ... 10.26 deg (limit 10.00)`, and
|
||||
`screw_6` failed with `NO_IK_SOLUTION` at the far tray point. This is the same
|
||||
planning limitation recorded below; it is not a physical-robot test. The
|
||||
handoff logic itself remains covered by package tests.
|
||||
|
||||
## 2026-11-11 contract and pause review
|
||||
|
||||
### Live `scene_json`
|
||||
|
||||
* Experiment 20: 14 components (fixture, body, cap, tray, 10 screws), four
|
||||
tools, 14 destinations (work, hatch_seat, four holes, four sockets, four
|
||||
contour sides). The live supervisor pose overrides the manifest pose; held
|
||||
objects are republished with their current pose and `source=webots_supervisor`.
|
||||
* Experiment 8: only fixture/body, `sealant_tool`, four contour sides and that
|
||||
tool's socket; no cap, tray, screw or unrelated tool. `dispense` is present,
|
||||
`handoff_screw` is absent.
|
||||
* Experiment 11: cap, tray, 10 screws, `magnetic_tool`, four holes and its
|
||||
socket; `handoff_screw` is present.
|
||||
* `docs/scene_json_experiment_20.json` is generated by the same `build_scene`
|
||||
function; a unit test fails if the example drifts.
|
||||
|
||||
### Dispense and tool return (experiment 8)
|
||||
|
||||
Graph: `grab(sealant_tool) → dispense(side_1) → transfer(sealant_tool,
|
||||
sealant_tool_socket) → dock_tool(sealant_tool, sealant_tool_socket)`.
|
||||
Result: `completed`. `side_1` was an existing contour destination, the
|
||||
dispense used a continuous LIN segment, and the tool was returned to its own
|
||||
socket.
|
||||
|
||||
### Tool grab/return (experiment 20)
|
||||
|
||||
Graph: `grab(vacuum) → transfer(vacuum, work) → transfer(vacuum, vacuum_socket)
|
||||
→ dock_tool(vacuum, vacuum_socket)`. Result: `completed`; `mounted_tool`
|
||||
ended as `null`.
|
||||
|
||||
### Timing before/after
|
||||
|
||||
The same approach/retreat graph was run in experiment 20 before and after the
|
||||
planning-scene pause change. The monotonic JSONL stages now show:
|
||||
|
||||
| stage | before (s) | after (s) |
|
||||
| --- | --- | --- |
|
||||
| inter-motion gap (`since_previous_trajectory_sec`) | 0.0015 | 0.0013 |
|
||||
| `_wait_stationary` (normal) | 0.20–0.35 | 0.20–0.35 |
|
||||
| PlanningScene relay | 0.10–0.21 | 0.10–0.12 |
|
||||
| PlanningScene check | 0.10 | 0.10 |
|
||||
| MoveIt plan | 3.6–7.5 | 3.6–7.5 |
|
||||
| trajectory check | 0.002–0.004 | 0.002–0.004 |
|
||||
| controller execution | 4.0–12.2 | 4.0–12.2 |
|
||||
| supervisor response | 0.02–0.04 | 0.02–0.04 |
|
||||
|
||||
The fixed `0.5 s` wait for the next `/joint_states` sample in every full
|
||||
planning-scene publish was removed; freshness is still checked immediately
|
||||
before every plan. The monitored-scene rendezvous remains because it is the
|
||||
proof that MoveItPy also received the scene. The only measured ~3 s interval
|
||||
is a genuine `_wait_stationary` of 3.3–3.7 s after a dock transfer while the
|
||||
Webots controller is still decelerating; it is deliberately kept because it is
|
||||
the required stationary-start check, not an added sleep. `start_delay_sec`
|
||||
remains a single 3 s gate after scene load and is never applied per motion.
|
||||
|
||||
### Tilt limitation
|
||||
|
||||
The existing strict 10° tcp tilt limit rejected the far-tray screw approach in
|
||||
experiment 11 (`screw_1`: 10.26°; `screw_6`: `NO_IK_SOLUTION`). No speed or
|
||||
tilt limit was raised to hide it.
|
||||
|
||||
## J7 and tilt evidence
|
||||
|
||||
@@ -116,7 +183,7 @@ source /opt/ros/jazzy/setup.bash
|
||||
source install/setup.bash
|
||||
ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
|
||||
ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {}
|
||||
# submit a graph through /iiwa_llm_plan_executor/submit; see doc/README.md
|
||||
# submit a graph through /iiwa_llm_plan_executor/submit_plan; see doc/README.md
|
||||
ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local
|
||||
ros2 topic echo /iiwa_llm_plan_executor/status
|
||||
```
|
||||
|
||||
@@ -1,11 +1,10 @@
|
||||
# `iiwa_llm_plan_executor` interfaces
|
||||
# Интерфейсы `iiwa_llm_plan_executor`
|
||||
|
||||
Graph contract version: **1** (it matches `iiwa_scene_context` contract 1).
|
||||
The executor never calls Qwen3 and never rewrites the model graph.
|
||||
Версия контракта графа: **2** (совпадает с `iiwa_scene_context` 2).
|
||||
Исполнитель не вызывает Qwen3 и не переписывает граф модели. Система
|
||||
принимает готовый JSON-граф.
|
||||
|
||||
## Graph response
|
||||
|
||||
The accepted response is exactly:
|
||||
## Ответ графа
|
||||
|
||||
```json
|
||||
{
|
||||
@@ -28,206 +27,117 @@ The accepted response is exactly:
|
||||
}
|
||||
```
|
||||
|
||||
* `status` is `ready` or `blocked`. `blocked` is never executed.
|
||||
* `plan` is a list of at most 256 nodes. Unknown keys, duplicate ids,
|
||||
reserved terminal ids, missing `entry`, missing required events, unknown
|
||||
events, unknown `next`, unreachable nodes and nodes that cannot reach a
|
||||
terminal are rejected.
|
||||
* Transitions may also be sent once at top level as
|
||||
`{"from": "...", "event": "...", "to": "..."}`; they are merged into the
|
||||
node by `parse_response`. Mixing both forms for the same event is rejected.
|
||||
* Reserved terminals: `completed`, `replan_required`, `operator_required`.
|
||||
`completed` is the verified end of the submitted graph; it is not a score of
|
||||
the model against a Russian instruction.
|
||||
* `params` is optional and currently only `dispense` accepts
|
||||
`start_fraction`/`end_fraction` in `0..1`.
|
||||
* `status` — `ready` или `blocked`; `blocked` никогда не исполняется.
|
||||
* `plan` — не более 256 узлов. Отклоняются неизвестные ключи, дубли `id`,
|
||||
зарезервированные терминалы, отсутствующий `entry`, пропущенные события,
|
||||
неизвестные `next`, недостижимые узлы и узлы без пути к терминалу.
|
||||
* Терминалы: `completed`, `replan_required`, `operator_required`.
|
||||
* `params` пока поддерживает только `dispense.start_fraction` /
|
||||
`dispense.end_fraction` в диапазоне `0..1`.
|
||||
|
||||
## Skills and aliases
|
||||
## Навыки
|
||||
|
||||
Native: `detect`, `approach`, `lower`, `grab`, `lift`, `transfer`, `align`,
|
||||
`release`, `retreat`, `capture`, `verify`, `stop`, `wait_scene_clear`,
|
||||
`inspect_state`, `notify_operator`, `dispense`, `dock_tool`,
|
||||
`handoff_screw`.
|
||||
Реализованные навыки: `detect`, `approach`, `lower`, `grab`, `lift`,
|
||||
`transfer`, `align`, `release`, `retreat`, `capture`, `verify`, `stop`,
|
||||
`wait_scene_clear`, `inspect_state`, `notify_operator`, `dispense`,
|
||||
`dock_tool`, `handoff_screw`.
|
||||
|
||||
Compatibility aliases: `pick` (= `grab`), `move_to_storage`
|
||||
(= `transfer` + axial `lower`, keeping the grasp), `inspect` (= `verify`).
|
||||
`tighten` is not in this profile and is rejected before any motion.
|
||||
Алиасы: `pick` (= `grab`), `move_to_storage` (= `transfer` + осевой
|
||||
`lower`), `inspect` (= `verify`). `tighten` не входит в профиль и
|
||||
отклоняется до любого движения.
|
||||
|
||||
The contract, outcomes and available ids come from
|
||||
`iiwa_scene_context/catalog.py` and the live `scene_json`; a model cannot add
|
||||
a skill or a contract.
|
||||
Набор навыков и правила приходят из `iiwa_scene_context/catalog.py` и живого
|
||||
`scene_json`; модель не может добавить навык или контракт. В сцене без
|
||||
герметика `dispense` не рекламируется, без винтов — `handoff_screw`, без
|
||||
инструментов — `dock_tool`.
|
||||
|
||||
## Admission rules
|
||||
## Приём графа
|
||||
|
||||
`parse_response(raw) -> PlanGraph` performs the strict JSON shape check.
|
||||
`validate_graph(graph, scene_envelope, catalog, expected_scene_revision=...)`
|
||||
returns `ValidationResult(ok, code, reason, details)` with codes:
|
||||
`parse_response(raw) -> PlanGraph` выполняет строгую проверку формы.
|
||||
`validate_graph(graph, scene_envelope, catalog)` возвращает
|
||||
`ValidationResult(ok, code, reason, details)`.
|
||||
|
||||
| code | meaning |
|
||||
| --- | --- |
|
||||
| `ok` | admitted |
|
||||
| `blocked` | the model returned `status=blocked`; no motion |
|
||||
| `replan_required` | a fresh scene/plan is required before motion (missing explicit transfer, unconfirmed ownership, or a graph branch that contradicts the live ownership state) |
|
||||
| `rejected` | structural or safety violation |
|
||||
| `stale_scene` | the graph was built for another `scene_revision` |
|
||||
Порядок приёма:
|
||||
|
||||
Important pre-movement rules:
|
||||
1. формат JSON и схема графа;
|
||||
2. готовность сцены/MoveIt/контроллера и истёкшая задержка старта;
|
||||
3. собственный свежий снимок сцены исполнителем;
|
||||
4. проверка свежести снимка;
|
||||
5. валидация структуры, ветвей и владения относительно этого снимка;
|
||||
6. публикация принятого графа.
|
||||
|
||||
* A missing required tool is **not** an admission error. `pick(cap)` /
|
||||
`grab(cap)` is admitted even when `vacuum` is docked. At execution time the
|
||||
skill re-reads the live supervisor state. If `vacuum` is not mounted it
|
||||
returns `grasp_failed` with the reason
|
||||
`required tool vacuum is not mounted`, performs no contact motion, sends no
|
||||
capture command, changes no object state, and the engine follows exactly the
|
||||
graph's `grasp_failed` edge. The `success` edge is not assumed.
|
||||
* The same rule applies to screws without `magnetic_tool`
|
||||
(`required tool magnetic_tool is not mounted`) and to other tool-dependent
|
||||
skills, which return their contract's failure event.
|
||||
* After a `grasp_failed` caused specifically by the missing tool, no contact
|
||||
and no object change occurred. For the **same** `scene_revision` the
|
||||
following `inspect_state` for that part may therefore only produce `empty`;
|
||||
`holding`/`placed` are not simulated. Unknown ownership (a generic grasp
|
||||
failure) keeps the conservative check of all `inspect_state` outcomes.
|
||||
A known empty hatch that is already installed makes `placed` possible after
|
||||
a release, while a free hatch makes `empty` possible.
|
||||
* The validator still simulates the reachable ownership branches. It skips a
|
||||
nominal success edge only when that success is impossible from the current
|
||||
tool/held state; a recovery branch that then assumes the part is held is
|
||||
rejected with its own ownership reason (for example `hatch is not held`),
|
||||
not with a missing-tool reason.
|
||||
* `release(vacuum)` without a target, or anywhere except its named socket and
|
||||
after an explicit `transfer(vacuum, vacuum_socket)`, is `replan_required`.
|
||||
The cup stays mounted. `release(vacuum, vacuum_socket)` and
|
||||
`dock_tool(vacuum, vacuum_socket)` are accepted only at the reached socket.
|
||||
* A graph ending in `work` with `vacuum` mounted is accepted; the tool stays in
|
||||
`patron`.
|
||||
* `grasp_failed` must go to `inspect_state` or `stop`; it can never become
|
||||
`completed`. `collision` must reach `stop`/`notify_operator` or
|
||||
`replan_required`/`operator_required`.
|
||||
* Screws are handed to the virtual operator at a hole; the hole stays empty
|
||||
and no insertion/tightening occurs.
|
||||
* No task is inferred from `experiment_id`; only the initial scene is selected.
|
||||
`RunRecord` создаётся **только после** успешного прохождения всех шагов и
|
||||
публикации. Поэтому отклонённый `run_id` свободен: исправленный граф можно
|
||||
отправить снова. Отклонённый запрос не появляется в
|
||||
`GET /planner/runs/{run_id}`, в `/iiwa_llm_plan_executor/plan` и в JSONL-журнале
|
||||
запусков. API возвращает конкретные `code`/`reason`.
|
||||
|
||||
## ROS execution interfaces
|
||||
Коды отклонения: `invalid`, `rejected`, `blocked`, `replan_required`,
|
||||
`not_ready`, `stale_scene`, `busy`, `conflict`.
|
||||
|
||||
| Direction | Name | Type | Payload |
|
||||
Правила `run_id`:
|
||||
|
||||
* пока запуск активен, другой `run_id` получает `busy`;
|
||||
* повтор идентичного принятого графа с тем же `run_id` — `duplicate`, второго
|
||||
движения нет;
|
||||
* другой граф с уже принятым `run_id` — `conflict`;
|
||||
* после `completed`, `operator_required`, `replan_required`, `failed` или
|
||||
`canceled` `run_id` остаётся занятым;
|
||||
* одновременные отправки защищены блокировкой, активный граф на робота один.
|
||||
|
||||
Перед первым движением исполнитель повторно проверяет свежесть сцены и
|
||||
`state_version`. Если сцена изменилась или стала недоступной, движение не
|
||||
начинается. Во время выполнения внутренний счётчик (не публикуется) ловит
|
||||
внешнее изменение сцены; изменения владения, подтверждённые собственными
|
||||
`grab`/`release`/`dock_tool`/`handoff_screw`, разрешены.
|
||||
|
||||
## ROS-интерфейсы
|
||||
|
||||
| Направление | Имя | Тип | Payload |
|
||||
| --- | --- | --- | --- |
|
||||
| service | `/iiwa_llm_plan_executor/submit` | `webots_ros2_msgs/SetString` | `{"run_id", "scene_revision", "response"}`. `success` means admitted; the rejection reason is in `/status` and `get_status`. |
|
||||
| topic | `/iiwa_llm_plan_executor/plan` | `std_msgs/String` | `{"run_id","scene_revision","response"}` exactly for the accepted graph; transient-local, depth 1. Published once per `run_id` before the first node. Rejected graphs never appear. |
|
||||
| topic | `/iiwa_llm_plan_executor/status` | `std_msgs/String` | Each node event and terminal. Fields: `run_id`, `state`, `reason`, `node_id`, `skill`, `event`, `next`, `scene_revision`, `terminal`, `mounted_tool`, `held_object`, `timestamp`, plus event details. |
|
||||
| service | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. |
|
||||
| service | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` or a plain run id; stops execution and preserves the grasp. |
|
||||
| сервис | `/iiwa_llm_plan_executor/submit_plan` | `iiwa_msgs/SubmitPlan` | request `run_id`, `response` (JSON-строка графа). Ответ: `accepted`, `code`, `reason`, `status_json`. |
|
||||
| сервис | `/iiwa_llm_plan_executor/submit` | `webots_ros2_msgs/SetString` | Устаревший совместимый вход `{"run_id","response"}`; направляется в тот же механизм приёма. Поле `scene_revision` игнорируется. |
|
||||
| топик | `/iiwa_llm_plan_executor/plan` | `std_msgs/String` | `{"run_id","response"}` для принятого графа; transient-local, depth 1. Отклонённые графы сюда не попадают. |
|
||||
| топик | `/iiwa_llm_plan_executor/status` | `std_msgs/String` | События и терминал: `run_id`, `state`, `reason`, `node_id`, `skill`, `event`, `next`, `terminal`, `mounted_tool`, `held_object`, `timestamp`, `event_detail`/`timing`. Поля `scene_revision` нет. |
|
||||
| сервис | `/iiwa_llm_plan_executor/get_status` | `iiwa_msgs/GetRunStatus` | request `string run_id`; response `bool found`, `string status_json`. |
|
||||
| сервис | `/iiwa_llm_plan_executor/cancel` | `webots_ros2_msgs/SetString` | `{"run_id": "..."}` или строка; останавливает выполнение, сохраняя захват. |
|
||||
|
||||
Every event is logged with the actual skill result, the failure reason when
|
||||
present, `mounted_tool` and `held_object` read from the supervisor, and the
|
||||
`next` node selected from the published graph. The same event is written to
|
||||
the JSONL log and returned by `GET /planner/runs/{run_id}`.
|
||||
## Тайминги этапов
|
||||
|
||||
Admission conditions: fresh `scene_json`, matching `scene_revision`,
|
||||
`scene_ready`, a ready MoveIt/controller, fresh `joint_states`, and at least
|
||||
3.0 s of simulation time since the first ready scene. Only one run may be
|
||||
active. Repeating `submit` with the same `run_id` is idempotent and never
|
||||
starts a second motion; a different `run_id` while active is rejected as
|
||||
`busy`.
|
||||
Каждый запуск пишет JSONL в `~/.ros/iiwa_llm_plan_executor/runs/<run_id>.jsonl`.
|
||||
События `motion_timing` и `timing` содержат монотонные `started_at`,
|
||||
`finished_at`, `duration_sec` для этапов:
|
||||
|
||||
Terminals are `completed`, `replan_required`, `operator_required`, plus
|
||||
`failed`/`canceled` for safety stops. `replan_required` emits the newest
|
||||
`scene_json`; the external client decides whether to call Qwen3 again.
|
||||
Execution runs in a worker thread, so `get_status` and `cancel` stay
|
||||
responsive. A JSONL log with every event is written under `trace_dir`
|
||||
(default `~/.ros/iiwa_llm_plan_executor/runs`).
|
||||
* завершение предыдущей траектории — `since_previous_trajectory_sec`;
|
||||
* `_wait_stationary` — `wait_stationary`;
|
||||
* синхронизация PlanningScene — `planning_scene_check` /
|
||||
`planning_scene_sync`;
|
||||
* построение плана MoveIt — `moveit_plan`;
|
||||
* проверка траектории — `trajectory_check`;
|
||||
* ожидание контроллера — `controller_execution`;
|
||||
* ответ supervisor — `supervisor_command` (`service_wait_sec`,
|
||||
`service_call_sec`, `result_wait_sec`);
|
||||
* захват кадра — `frame_capture`.
|
||||
|
||||
`start_delay_sec` — задержка после загрузки сцены, применяется один раз перед
|
||||
стартом графа. Это не пауза между движениями.
|
||||
|
||||
## Skill phases
|
||||
## Терминалы и журнал
|
||||
|
||||
A skill may contain the mandatory geometric phases of one action. `approach`
|
||||
moves above the target and saves that height; `lower` moves along the tool
|
||||
axis to contact; `grab` performs only the missing phases, confirms the grasp
|
||||
through the supervisor, then lifts back to the saved approach pose. Explicit
|
||||
`approach`/`lower`/`lift` nodes do not repeat motion already performed.
|
||||
`completed` — граф и его проверенные постусловия завершены.
|
||||
`replan_required` — нужен новый снимок и, возможно, новый вызов Qwen3.
|
||||
`operator_required` — нужно решение человека; робот остановлен.
|
||||
`failed` / `canceled` — безопасная остановка (устаревшее состояние, лимиты,
|
||||
столкновение, отмена, лимит шагов); состояние захвата сохраняется.
|
||||
|
||||
`release` always lifts axially before any lateral motion or wrist rotation.
|
||||
`transfer` stops at the safe height over the destination. `release(cap,
|
||||
hatch_seat)` lowers, places, detaches and withdraws. `handoff_screw` lowers at
|
||||
the hole, hands the screw to the virtual operator and withdraws; it never
|
||||
inserts or tightens and the hole remains empty.
|
||||
## Ограничения движения
|
||||
|
||||
`detect` first withdraws axially from a contact, moves to `work`, waits for a
|
||||
fresh top frame and reads the exact pose from the supervisor. When the object
|
||||
is configured for close inspection, a second fresh frame from the hand camera
|
||||
is required; RGB/D is used for liveness only.
|
||||
|
||||
|
||||
## Constrained motion adapter
|
||||
|
||||
`MotionPolicy` (`config/motion_limits.yaml`) checks every sampled point before
|
||||
the controller sees a plan:
|
||||
|
||||
* Every joint stays inside the real per-joint position limits
|
||||
(`iiwa_config/config/moveit/joint_limits.yaml`, soft limits included).
|
||||
Joint 7 keeps the neutral `work` value as a documented preference, but the
|
||||
former +/-90 deg hard corridor is no longer a rejection boundary; safe paths
|
||||
may use the full joint range.
|
||||
* The tcp local +Z (working direction) must point down within
|
||||
`max_tilt_deg` (10 deg).
|
||||
* Consecutive points may not jump more than `max_joint_jump_rad` (0.6 rad) or
|
||||
`max_cartesian_jump_m` (0.15 m); the first point must match the measured
|
||||
joints used as IK seed.
|
||||
* The planned trajectory is resampled in joint space at
|
||||
`trajectory.max_sample_step_rad` (0.01 rad). Every sample is checked for
|
||||
joint limits, self-collision, scene collision, camera clearance and tcp
|
||||
orientation before execution.
|
||||
* `joint_states` and the planning scene freshness are checked before planning.
|
||||
|
||||
Motion selection:
|
||||
|
||||
| Purpose | Motion |
|
||||
| --- | --- |
|
||||
| between `work`/`home`, to an object, tool or placement point, part transfer, safe alignment | Pilz PTP |
|
||||
| axial contact, axial lift/retreat, dispense contour | Pilz LIN |
|
||||
|
||||
If a lateral PTP would start in a contact pose, the skill first performs the
|
||||
axial LIN retreat to the saved approach height. The `dispense` contour itself
|
||||
stays a continuous LIN contour; only its approach and retreat use PTP/axial
|
||||
rules.
|
||||
|
||||
One explicit exception is recorded: if the measured start posture is not yet the
|
||||
downward working posture (the calibrated home pose points the tip up) and the
|
||||
gripper is empty, the first reorientation to `work` may pass through larger
|
||||
tilt angles. Joint limits, joint jumps, Cartesian jumps and the final
|
||||
downward orientation are still checked, and the event records
|
||||
`reorientation: true`. A reorientation with a held object is always rejected.
|
||||
|
||||
`MoveItPyPlanner` uses the installed MoveItPy 2.12 API:
|
||||
|
||||
```python
|
||||
arm.set_start_state(robot_state=clamped_robot_state) # measured joints as IK seed
|
||||
arm.set_path_constraints(empty_constraints) # no artificial J7 corridor
|
||||
arm.set_goal_state(pose_stamped_msg=..., pose_link='tcp')
|
||||
result = arm.plan(single_plan_parameters=params)
|
||||
result.trajectory.get_robot_trajectory_msg()
|
||||
# resample + joint/self/world/camera checks, then:
|
||||
moveit.execute(result.trajectory, controllers=[])
|
||||
```
|
||||
|
||||
The planner never falls back to an unconstrained `MoveToPose` action. If a
|
||||
plan is missing, the status names the phase, pipeline, planner id, target tcp
|
||||
pose, start joints and MoveIt error code (for example `NO_IK_SOLUTION`). If
|
||||
our validation rejects a plan, the status names the real cause: joint limit,
|
||||
jump, orientation or the colliding link pair with the measured minimum
|
||||
distance and required clearance.
|
||||
|
||||
The measured start state is clamped to the same soft joint limits used by
|
||||
MoveIt (`soft_min_position`/`soft_max_position`) before it becomes the IK
|
||||
seed. Without that clamp a Webots-controller overshoot of ~0.00003 rad makes
|
||||
`CheckStartStateBounds` abort every following plan.
|
||||
|
||||
Camera collision model: the generated SRDF disables only constructive
|
||||
contact pairs (`link7-camera_holder`, `camera_holder-patron`,
|
||||
`camera_holder-camera_corner`, `camera_corner-camera_hand`,
|
||||
`camera_holder-camera_hand`). Every camera-to-robot pair, including
|
||||
`camera_hand-link6` and `camera_corner-link6`, remains checked. The D455
|
||||
collision box matches its visible bounds; the measured AABB clearance from the
|
||||
camera corner/hand to link6/link7 is at least 35 mm at `home` and `work`, and
|
||||
the planner requires a non-zero minimum sampled clearance of 3 mm.
|
||||
Свободные перемещения между рабочими точками — Pilz PTP. Осевое опускание,
|
||||
подъём и нанесение герметика — Pilz LIN. Каждый план плотно пересэмплируется
|
||||
и проверяется по реальным лимитам суставов, self/scene collision, зазору до
|
||||
камер, наклону TCP (≤10°) и скачкам суставов. Боковое движение из контактной
|
||||
позы предваряется осевым LIN-отводом. Исполнитель не откатывается к
|
||||
неограниченному `MoveToPose`.
|
||||
|
||||
Это проверка в Webots, а не валидация физического робота и не оценка
|
||||
качества Qwen3.
|
||||
|
||||
@@ -44,8 +44,9 @@ class RunRecord:
|
||||
run_id: str
|
||||
state: str = 'new'
|
||||
reason: str = ''
|
||||
scene_revision: int = 0
|
||||
state_version: int = 0
|
||||
response: str = ''
|
||||
canonical_response: str = ''
|
||||
node_id: str | None = None
|
||||
skill: str | None = None
|
||||
event: str | None = None
|
||||
@@ -71,14 +72,13 @@ class RunStatus:
|
||||
skill: str | None
|
||||
event: str | None
|
||||
next: str | None
|
||||
scene_revision: int
|
||||
terminal: str | None
|
||||
mounted_tool: str | None
|
||||
held_object: str | None
|
||||
events: tuple[dict[str, Any], ...] = ()
|
||||
|
||||
def as_dict(self) -> dict[str, Any]:
|
||||
payload = {
|
||||
return {
|
||||
'found': self.found,
|
||||
'run_id': self.run_id,
|
||||
'state': self.state,
|
||||
@@ -87,13 +87,11 @@ class RunStatus:
|
||||
'skill': self.skill,
|
||||
'event': self.event,
|
||||
'next': self.next,
|
||||
'scene_revision': self.scene_revision,
|
||||
'terminal': self.terminal,
|
||||
'mounted_tool': self.mounted_tool,
|
||||
'held_object': self.held_object,
|
||||
'events': list(self.events),
|
||||
}
|
||||
return payload
|
||||
|
||||
def to_json(self) -> str:
|
||||
return json.dumps(self.as_dict(), ensure_ascii=False)
|
||||
@@ -106,6 +104,13 @@ class CancelResult:
|
||||
run_id: str
|
||||
|
||||
|
||||
def _canonical(graph: PlanGraph) -> str:
|
||||
return json.dumps(
|
||||
graph.as_dict(), sort_keys=True, ensure_ascii=False,
|
||||
separators=(',', ':'),
|
||||
)
|
||||
|
||||
|
||||
class GraphEngine:
|
||||
def __init__(
|
||||
self,
|
||||
@@ -182,117 +187,89 @@ class GraphEngine:
|
||||
self,
|
||||
run_id: str,
|
||||
response_json: str | Mapping[str, Any],
|
||||
scene_revision: int,
|
||||
) -> AdmissionResult:
|
||||
"""Admit one graph against a fresh scene snapshot.
|
||||
|
||||
Nothing is registered until the format, readiness, freshness and
|
||||
ownership checks have all passed and the accepted graph has been
|
||||
published. A rejected request therefore never occupies its run_id.
|
||||
"""
|
||||
run_id = str(run_id).strip()
|
||||
if not run_id:
|
||||
return AdmissionResult(False, 'rejected', 'run_id must not be empty', '')
|
||||
return AdmissionResult(False, 'invalid', 'run_id must not be empty', '')
|
||||
raw_response = (
|
||||
response_json
|
||||
if isinstance(response_json, str)
|
||||
else json.dumps(response_json, ensure_ascii=False)
|
||||
)
|
||||
try:
|
||||
graph = parse_response(raw_response)
|
||||
except PlanSchemaError as error:
|
||||
return AdmissionResult(False, 'invalid', str(error), run_id)
|
||||
canonical = _canonical(graph)
|
||||
|
||||
with self._lock:
|
||||
existing = self._runs.get(run_id)
|
||||
if existing is not None:
|
||||
if existing.state in ACTIVE_STATES:
|
||||
if existing.canonical_response == canonical:
|
||||
return AdmissionResult(
|
||||
True, 'duplicate',
|
||||
f'run {run_id!r} is already active; no second motion',
|
||||
f'run {run_id!r} was already admitted; no second motion',
|
||||
run_id,
|
||||
)
|
||||
if existing.state == 'rejected':
|
||||
return AdmissionResult(
|
||||
False, existing.reason.split(':', 1)[0] or 'rejected',
|
||||
existing.reason, run_id,
|
||||
)
|
||||
return AdmissionResult(
|
||||
True, 'duplicate',
|
||||
f'run {run_id!r} was already admitted; no second motion',
|
||||
False, 'conflict',
|
||||
f'run {run_id!r} is already bound to a different graph',
|
||||
run_id,
|
||||
)
|
||||
active = [
|
||||
record for record in self._runs.values()
|
||||
if record.state in ACTIVE_STATES
|
||||
]
|
||||
now = self._clock()
|
||||
record = RunRecord(
|
||||
run_id=run_id,
|
||||
response=raw_response,
|
||||
scene_revision=int(scene_revision),
|
||||
)
|
||||
self._runs[run_id] = record
|
||||
if active:
|
||||
record.state = 'rejected'
|
||||
record.reason = 'busy: another run is active'
|
||||
return AdmissionResult(
|
||||
False, 'busy', 'another graph is already active', run_id,
|
||||
)
|
||||
now = self._clock()
|
||||
reason = self._not_ready(now)
|
||||
if reason is not None:
|
||||
record.state = 'rejected'
|
||||
record.reason = f'not_ready: {reason}'
|
||||
return AdmissionResult(False, 'not_ready', reason, run_id)
|
||||
envelope = self._envelope()
|
||||
if envelope is None:
|
||||
record.state = 'rejected'
|
||||
record.reason = 'not_ready: no fresh scene snapshot'
|
||||
return AdmissionResult(
|
||||
False, 'not_ready', 'no fresh scene snapshot', run_id,
|
||||
)
|
||||
envelope_revision = int(envelope.get('scene_revision', -1))
|
||||
if envelope_revision != int(scene_revision):
|
||||
record.state = 'rejected'
|
||||
record.reason = (
|
||||
f'stale_scene: submitted revision {scene_revision} does not '
|
||||
f'match current revision {envelope_revision}'
|
||||
)
|
||||
return AdmissionResult(
|
||||
False, 'stale_scene',
|
||||
f'submitted scene_revision {scene_revision} does not match '
|
||||
f'the current revision {envelope_revision}',
|
||||
run_id,
|
||||
)
|
||||
stale = self._scene_age(envelope, now)
|
||||
if stale is not None:
|
||||
record.state = 'rejected'
|
||||
record.reason = f'stale_scene: {stale}'
|
||||
return AdmissionResult(False, 'stale_scene', stale, run_id)
|
||||
try:
|
||||
graph = parse_response(raw_response)
|
||||
except PlanSchemaError as error:
|
||||
record.state = 'rejected'
|
||||
record.reason = f'rejected: {error}'
|
||||
return AdmissionResult(False, 'rejected', str(error), run_id)
|
||||
validation: ValidationResult = validate_graph(
|
||||
graph,
|
||||
envelope,
|
||||
self.catalog,
|
||||
expected_scene_revision=scene_revision,
|
||||
)
|
||||
if not validation.ok:
|
||||
record.state = 'rejected'
|
||||
record.reason = f'{validation.code}: {validation.reason}'
|
||||
record.details = validation.details
|
||||
return AdmissionResult(
|
||||
False, validation.code, validation.reason, run_id,
|
||||
)
|
||||
record.state = 'admitted'
|
||||
record.scene_revision = envelope_revision
|
||||
try:
|
||||
if self.on_plan is not None:
|
||||
self.on_plan({
|
||||
'run_id': run_id,
|
||||
'scene_revision': envelope_revision,
|
||||
'response': raw_response,
|
||||
})
|
||||
except Exception as error:
|
||||
record.state = 'rejected'
|
||||
record.reason = f'rejected: plan publication failed: {error}'
|
||||
return AdmissionResult(
|
||||
False, 'rejected',
|
||||
f'plan publication failed: {error}', run_id,
|
||||
)
|
||||
record = RunRecord(
|
||||
run_id=run_id,
|
||||
state='admitted',
|
||||
response=canonical,
|
||||
canonical_response=canonical,
|
||||
state_version=int(envelope.get('state_version', 0)),
|
||||
)
|
||||
self._runs[run_id] = record
|
||||
if self.threaded:
|
||||
thread = threading.Thread(
|
||||
target=self._execute,
|
||||
@@ -301,7 +278,16 @@ class GraphEngine:
|
||||
daemon=True,
|
||||
)
|
||||
record.thread = thread
|
||||
thread.start()
|
||||
try:
|
||||
thread.start()
|
||||
except Exception as error:
|
||||
# Startup failed before any motion: do not keep the run id
|
||||
# bound to a request that was never executed.
|
||||
self._runs.pop(run_id, None)
|
||||
return AdmissionResult(
|
||||
False, 'rejected',
|
||||
f'could not start the worker: {error}', run_id,
|
||||
)
|
||||
return AdmissionResult(True, 'accepted', 'graph admitted', run_id)
|
||||
|
||||
# ------------------------------------------------------------------ helpers
|
||||
@@ -314,7 +300,7 @@ class GraphEngine:
|
||||
return None
|
||||
if not isinstance(envelope.get('scene_json'), Mapping):
|
||||
return None
|
||||
if 'scene_revision' not in envelope:
|
||||
if 'state_version' not in envelope:
|
||||
return None
|
||||
return envelope
|
||||
|
||||
@@ -338,7 +324,8 @@ class GraphEngine:
|
||||
if hasattr(context, 'held_object'):
|
||||
context.held_object = initial.get('held_object') or None
|
||||
if hasattr(context, 'scene_revision'):
|
||||
context.scene_revision = int(envelope.get('scene_revision', 0))
|
||||
# Internal only: the executor never exposes this counter.
|
||||
context.scene_revision = int(envelope.get('state_version', 0))
|
||||
run.context = context
|
||||
return context
|
||||
|
||||
@@ -357,7 +344,6 @@ class GraphEngine:
|
||||
'skill': run.skill,
|
||||
'event': run.event,
|
||||
'next': run.next,
|
||||
'scene_revision': run.scene_revision,
|
||||
'terminal': run.terminal,
|
||||
'mounted_tool': run.mounted_tool,
|
||||
'held_object': run.held_object,
|
||||
@@ -381,6 +367,8 @@ class GraphEngine:
|
||||
event: str,
|
||||
next_id: str,
|
||||
outcome: Any,
|
||||
*,
|
||||
timing: dict[str, Any] | None = None,
|
||||
) -> None:
|
||||
context = run.context
|
||||
if context is not None:
|
||||
@@ -396,12 +384,13 @@ class GraphEngine:
|
||||
'event': event,
|
||||
'reason': details.get('reason', ''),
|
||||
'next': next_id,
|
||||
'scene_revision': run.scene_revision,
|
||||
'mounted_tool': run.mounted_tool,
|
||||
'held_object': run.held_object,
|
||||
'details': details,
|
||||
'timestamp': self._clock(),
|
||||
}
|
||||
if timing:
|
||||
entry['timing'] = timing
|
||||
run.events.append(entry)
|
||||
self._emit_status(run, event_detail=entry)
|
||||
|
||||
@@ -433,6 +422,17 @@ class GraphEngine:
|
||||
if envelope is None:
|
||||
self._terminal(run, 'failed', 'scene snapshot is unavailable')
|
||||
return
|
||||
age = self._scene_age(envelope, self._clock())
|
||||
if age is not None:
|
||||
self._terminal(run, 'failed', age)
|
||||
return
|
||||
if int(envelope.get('state_version', -1)) != run.state_version:
|
||||
self._terminal(
|
||||
run, 'failed',
|
||||
'scene changed before the first motion '
|
||||
f'({run.state_version} -> {envelope.get("state_version")})',
|
||||
)
|
||||
return
|
||||
self._execute(run.run_id, graph, envelope)
|
||||
|
||||
def _execute(self, run_id: str, graph: PlanGraph, first_envelope: Mapping[str, Any]) -> None:
|
||||
@@ -440,11 +440,31 @@ class GraphEngine:
|
||||
run = self._runs[run_id]
|
||||
run.state = 'running'
|
||||
try:
|
||||
context = self._context(run, first_envelope)
|
||||
envelope = first_envelope
|
||||
# Recheck immediately before any motion: the graph was admitted
|
||||
# against this state version, and the worker must not start on a
|
||||
# scene that changed or became stale in between.
|
||||
now = self._clock()
|
||||
current = self._envelope()
|
||||
if current is None:
|
||||
self._terminal(run, 'failed', 'fresh scene snapshot is unavailable')
|
||||
return
|
||||
age = self._scene_age(current, now)
|
||||
if age is not None:
|
||||
self._terminal(run, 'failed', age)
|
||||
return
|
||||
if int(current.get('state_version', -1)) != run.state_version:
|
||||
self._terminal(
|
||||
run, 'failed',
|
||||
'scene changed before the first motion '
|
||||
f'({run.state_version} -> {current.get("state_version")})',
|
||||
)
|
||||
return
|
||||
context = self._context(run, current)
|
||||
envelope = current
|
||||
node_id = graph.entry
|
||||
steps = 0
|
||||
while True:
|
||||
step_started = time.monotonic()
|
||||
if run.cancel_requested:
|
||||
self._terminal(run, 'canceled', 'cancel requested')
|
||||
return
|
||||
@@ -454,7 +474,7 @@ class GraphEngine:
|
||||
f'executed step limit of {self.max_steps} reached',
|
||||
)
|
||||
return
|
||||
now = self._clock()
|
||||
robot_started = time.monotonic()
|
||||
if self.robot_fresh is not None:
|
||||
try:
|
||||
fresh, message = self.robot_fresh()
|
||||
@@ -463,6 +483,9 @@ class GraphEngine:
|
||||
if not fresh:
|
||||
self._terminal(run, 'failed', f'robot state: {message}')
|
||||
return
|
||||
robot_check_sec = time.monotonic() - robot_started
|
||||
now = self._clock()
|
||||
scene_started = time.monotonic()
|
||||
current = self._envelope()
|
||||
if current is None:
|
||||
self._terminal(run, 'failed', 'fresh scene snapshot is unavailable')
|
||||
@@ -471,28 +494,38 @@ class GraphEngine:
|
||||
if age is not None:
|
||||
self._terminal(run, 'failed', age)
|
||||
return
|
||||
revision = int(current.get('scene_revision', -1))
|
||||
if revision != run.scene_revision:
|
||||
version = int(current.get('state_version', -1))
|
||||
if version != run.state_version:
|
||||
previous_node = graph.node(node_id) if node_id in graph.ids else None
|
||||
acknowledged = (
|
||||
run.skill in OWNERSHIP_SKILLS
|
||||
and run.event == 'success'
|
||||
and previous_node is not None
|
||||
and revision > run.scene_revision
|
||||
and version > run.state_version
|
||||
)
|
||||
if not acknowledged:
|
||||
self._terminal(
|
||||
run, 'failed',
|
||||
'scene_revision changed externally '
|
||||
f'({run.scene_revision} -> {revision}); motion stopped',
|
||||
'scene state changed externally '
|
||||
f'({run.state_version} -> {version}); motion stopped',
|
||||
)
|
||||
return
|
||||
run.scene_revision = revision
|
||||
run.state_version = version
|
||||
if hasattr(context, 'scene_revision'):
|
||||
context.scene_revision = revision
|
||||
context.scene_revision = version
|
||||
envelope = current
|
||||
scene_check_sec = time.monotonic() - scene_started
|
||||
node = graph.node(node_id)
|
||||
skill_started = time.monotonic()
|
||||
outcome = self.skills.execute(node, context)
|
||||
skill_sec = time.monotonic() - skill_started
|
||||
timing = {
|
||||
'step': steps,
|
||||
'robot_check_sec': robot_check_sec,
|
||||
'scene_check_sec': scene_check_sec,
|
||||
'skill_sec': skill_sec,
|
||||
'step_sec': time.monotonic() - step_started,
|
||||
}
|
||||
event = str(outcome.event)
|
||||
transitions = {
|
||||
transition.event: transition.next for transition in node.transitions
|
||||
@@ -502,7 +535,9 @@ class GraphEngine:
|
||||
run.event = event
|
||||
if event not in transitions:
|
||||
run.next = None
|
||||
self._record_event(run, node, event, '', outcome)
|
||||
self._record_event(
|
||||
run, node, event, '', outcome, timing=timing,
|
||||
)
|
||||
self._terminal(
|
||||
run, 'failed',
|
||||
f'unknown skill outcome {event!r} for node {node.id!r}',
|
||||
@@ -510,7 +545,9 @@ class GraphEngine:
|
||||
return
|
||||
next_id = transitions[event]
|
||||
run.next = next_id
|
||||
self._record_event(run, node, event, next_id, outcome)
|
||||
self._record_event(
|
||||
run, node, event, next_id, outcome, timing=timing,
|
||||
)
|
||||
steps += 1
|
||||
if run.cancel_requested:
|
||||
self._terminal(run, 'canceled', 'cancel requested')
|
||||
@@ -525,11 +562,6 @@ class GraphEngine:
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
if next_id == 'completed' and hasattr(context, 'held_object'):
|
||||
if context.held_object is not None:
|
||||
# The graph deliberately leaves an object in the
|
||||
# gripper only if it never asked to release it.
|
||||
pass
|
||||
self._terminal(run, next_id, f'graph reached {next_id}')
|
||||
return
|
||||
node_id = next_id
|
||||
@@ -544,7 +576,7 @@ class GraphEngine:
|
||||
if run is None:
|
||||
return RunStatus(
|
||||
False, run_id, 'unknown', 'run_id was not submitted',
|
||||
None, None, None, None, 0, None, None, None, (),
|
||||
None, None, None, None, None, None, None, (),
|
||||
)
|
||||
return RunStatus(
|
||||
True,
|
||||
@@ -555,7 +587,6 @@ class GraphEngine:
|
||||
run.skill,
|
||||
run.event,
|
||||
run.next,
|
||||
run.scene_revision,
|
||||
run.terminal,
|
||||
run.mounted_tool,
|
||||
run.held_object,
|
||||
|
||||
@@ -9,6 +9,8 @@ to run it.
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import time
|
||||
from contextlib import contextmanager
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, Callable, Mapping
|
||||
|
||||
@@ -50,6 +52,23 @@ class MotionOutcome:
|
||||
}
|
||||
|
||||
|
||||
@contextmanager
|
||||
def _stage(timings: list[dict[str, Any]], name: str, **details):
|
||||
started = time.monotonic()
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
finished = time.monotonic()
|
||||
entry = {
|
||||
'stage': name,
|
||||
'started_at': started,
|
||||
'finished_at': finished,
|
||||
'duration_sec': finished - started,
|
||||
}
|
||||
entry.update(details)
|
||||
timings.append(entry)
|
||||
|
||||
|
||||
_POSE_TO_MATRIX = None
|
||||
|
||||
|
||||
@@ -524,6 +543,7 @@ class MoveItBackend:
|
||||
self._on_event = on_event
|
||||
self.stopped = False
|
||||
self.stop_reason: str | None = None
|
||||
self._last_motion_end: float | None = None
|
||||
|
||||
def _emit(self, **payload) -> None:
|
||||
if self._on_event is not None:
|
||||
@@ -560,12 +580,49 @@ class MoveItBackend:
|
||||
phase: str = 'transfer',
|
||||
held_object: str | None = None,
|
||||
) -> MotionOutcome:
|
||||
blocked = self._guard()
|
||||
started = time.monotonic()
|
||||
since_previous = (
|
||||
None if self._last_motion_end is None
|
||||
else max(0.0, started - self._last_motion_end)
|
||||
)
|
||||
timings: list[dict[str, Any]] = []
|
||||
outcome: MotionOutcome | None = None
|
||||
try:
|
||||
outcome = self._plan_and_execute_impl(
|
||||
goal, phase, held_object, timings,
|
||||
)
|
||||
return outcome
|
||||
finally:
|
||||
finished = time.monotonic()
|
||||
self._emit(
|
||||
event='motion_timing',
|
||||
phase=phase,
|
||||
held_object=held_object,
|
||||
success=bool(outcome is not None and outcome.success),
|
||||
started_at=started,
|
||||
finished_at=finished,
|
||||
total_duration_sec=finished - started,
|
||||
since_previous_trajectory_sec=since_previous,
|
||||
stages=timings,
|
||||
)
|
||||
if outcome is not None and outcome.success:
|
||||
self._last_motion_end = finished
|
||||
|
||||
def _plan_and_execute_impl(
|
||||
self,
|
||||
goal: Transform,
|
||||
phase: str,
|
||||
held_object: str | None,
|
||||
timings: list[dict[str, Any]],
|
||||
) -> MotionOutcome:
|
||||
with _stage(timings, 'state_guard'):
|
||||
blocked = self._guard()
|
||||
if blocked is not None:
|
||||
return blocked
|
||||
if self._before_plan is not None:
|
||||
try:
|
||||
self._before_plan()
|
||||
with _stage(timings, 'wait_stationary'):
|
||||
self._before_plan()
|
||||
except Exception as error:
|
||||
return self._fail(f'settle wait failed: {error}', 'stale_state')
|
||||
planned = None
|
||||
@@ -573,12 +630,14 @@ class MoveItBackend:
|
||||
last_error = ''
|
||||
for attempt in range(3):
|
||||
try:
|
||||
candidate = self.planner.plan(goal, phase, held_object)
|
||||
with _stage(timings, 'moveit_plan', attempt=attempt):
|
||||
candidate = self.planner.plan(goal, phase, held_object)
|
||||
except Exception as error:
|
||||
last_error = str(error)
|
||||
if attempt + 1 < 3:
|
||||
try:
|
||||
self._before_plan and self._before_plan()
|
||||
with _stage(timings, 'wait_stationary', attempt=attempt):
|
||||
self._before_plan and self._before_plan()
|
||||
except Exception:
|
||||
pass
|
||||
continue
|
||||
@@ -591,11 +650,12 @@ class MoveItBackend:
|
||||
current = None
|
||||
if current is None:
|
||||
current = candidate.current_joints
|
||||
candidate_check = self.policy.validate_trajectory(
|
||||
candidate.points, candidate.tcp_poses, current,
|
||||
start_tcp_pose=candidate.start_tcp_pose,
|
||||
held_object=held_object,
|
||||
)
|
||||
with _stage(timings, 'trajectory_check', attempt=attempt):
|
||||
candidate_check = self.policy.validate_trajectory(
|
||||
candidate.points, candidate.tcp_poses, current,
|
||||
start_tcp_pose=candidate.start_tcp_pose,
|
||||
held_object=held_object,
|
||||
)
|
||||
if candidate_check.ok:
|
||||
planned = candidate
|
||||
check = candidate_check
|
||||
@@ -603,7 +663,8 @@ class MoveItBackend:
|
||||
last_error = candidate_check.reason
|
||||
if attempt + 1 < 3:
|
||||
try:
|
||||
self._before_plan and self._before_plan()
|
||||
with _stage(timings, 'wait_stationary', attempt=attempt):
|
||||
self._before_plan and self._before_plan()
|
||||
except Exception:
|
||||
pass
|
||||
if planned is None or check is None:
|
||||
@@ -617,7 +678,8 @@ class MoveItBackend:
|
||||
)
|
||||
if self._collision_checker is not None:
|
||||
try:
|
||||
collision_reason = self._collision_checker(planned)
|
||||
with _stage(timings, 'collision_check'):
|
||||
collision_reason = self._collision_checker(planned)
|
||||
except Exception as error:
|
||||
collision_reason = f'collision check failed: {error}'
|
||||
if collision_reason:
|
||||
@@ -649,7 +711,8 @@ class MoveItBackend:
|
||||
planner_details=dict(planned.details), **check.details,
|
||||
)
|
||||
try:
|
||||
executed = self.planner.execute(planned)
|
||||
with _stage(timings, 'controller_execution'):
|
||||
executed = self.planner.execute(planned)
|
||||
except Exception as error:
|
||||
return self._fail(str(error), 'execution_failed')
|
||||
if not executed:
|
||||
|
||||
@@ -7,7 +7,7 @@ import threading
|
||||
import time
|
||||
import uuid
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping
|
||||
from typing import Any, Callable, Mapping
|
||||
|
||||
import rclpy
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
@@ -38,7 +38,7 @@ from iiwa_experement.planning_scene import (
|
||||
screw_object_id,
|
||||
)
|
||||
from iiwa_experement.scenario import ScenarioSpec, load_scenario
|
||||
from iiwa_msgs.srv import GetRunStatus
|
||||
from iiwa_msgs.srv import GetRunStatus, SubmitPlan
|
||||
from iiwa_scene_context.catalog import DEFAULT_CATALOG
|
||||
from iiwa_scene_context.node import SCENE_TOPIC
|
||||
|
||||
@@ -53,6 +53,7 @@ COMMAND_SERVICE = '/iiwa_experement/object_command'
|
||||
STATE_TOPIC = '/iiwa_experement/object_state'
|
||||
SCENE_SERVICE = '/iiwa_scene_context/get_scene'
|
||||
SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit'
|
||||
SUBMIT_PLAN_SERVICE = '/iiwa_llm_plan_executor/submit_plan'
|
||||
CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel'
|
||||
STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status'
|
||||
PLAN_TOPIC = '/iiwa_llm_plan_executor/plan'
|
||||
@@ -94,8 +95,15 @@ class RunLog:
|
||||
class SupervisorFacade:
|
||||
"""Thread-safe supervisor client that never spins its own node."""
|
||||
|
||||
def __init__(self, node: Node, callback_group) -> None:
|
||||
def __init__(
|
||||
self,
|
||||
node: Node,
|
||||
callback_group,
|
||||
*,
|
||||
on_timing: Callable[[dict[str, Any]], None] | None = None,
|
||||
) -> None:
|
||||
self.node = node
|
||||
self.on_timing = on_timing
|
||||
self._lock = threading.Condition()
|
||||
self._latest: dict[str, Any] | None = None
|
||||
self._client = node.create_client(
|
||||
@@ -138,9 +146,12 @@ class SupervisorFacade:
|
||||
target=None,
|
||||
target_name: str = '',
|
||||
) -> dict[str, Any]:
|
||||
request_started = time.monotonic()
|
||||
service_wait_started = time.monotonic()
|
||||
if not self._client.service_is_ready():
|
||||
if not self._client.wait_for_service(timeout_sec=5.0):
|
||||
raise RuntimeError(f'{COMMAND_SERVICE} is unavailable')
|
||||
service_wait_sec = time.monotonic() - service_wait_started
|
||||
request_id = uuid.uuid4().hex
|
||||
payload: dict[str, Any] = {
|
||||
'request_id': request_id,
|
||||
@@ -157,17 +168,21 @@ class SupervisorFacade:
|
||||
}
|
||||
request = webots_ros2_msgs.srv.SetString.Request()
|
||||
request.value = json.dumps(payload)
|
||||
call_started = time.monotonic()
|
||||
future = self._client.call_async(request)
|
||||
done = threading.Event()
|
||||
future.add_done_callback(lambda _: done.set())
|
||||
if not done.wait(timeout=15.0):
|
||||
raise RuntimeError(f'supervisor command {op} timed out')
|
||||
service_call_sec = time.monotonic() - call_started
|
||||
try:
|
||||
response = future.result()
|
||||
except Exception as error:
|
||||
raise RuntimeError(f'supervisor command {op} failed: {error}') from error
|
||||
accepted = bool(response.success)
|
||||
result_started = time.monotonic()
|
||||
state = self._wait_for_result(request_id, timeout=3.0)
|
||||
result_wait_sec = time.monotonic() - result_started
|
||||
last = (state or {}).get('last_command_result') or {}
|
||||
if last.get('request_id') != request_id:
|
||||
raise RuntimeError(
|
||||
@@ -177,6 +192,21 @@ class SupervisorFacade:
|
||||
raise RuntimeError(
|
||||
last.get('error') or f'supervisor rejected {op}'
|
||||
)
|
||||
if self.on_timing is not None:
|
||||
try:
|
||||
self.on_timing({
|
||||
'stage': 'supervisor_command',
|
||||
'op': op,
|
||||
'object': object_id,
|
||||
'started_at': request_started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - request_started,
|
||||
'service_wait_sec': service_wait_sec,
|
||||
'service_call_sec': service_call_sec,
|
||||
'result_wait_sec': result_wait_sec,
|
||||
})
|
||||
except Exception:
|
||||
pass
|
||||
return state
|
||||
|
||||
def _wait_for_result(self, request_id: str, timeout: float) -> dict[str, Any]:
|
||||
@@ -201,13 +231,27 @@ class SceneAdapter:
|
||||
bridge: PlanningSceneBridge,
|
||||
spec: ScenarioSpec,
|
||||
geometry: GeometrySpec,
|
||||
*,
|
||||
on_timing: Callable[[dict[str, Any]], None] | None = None,
|
||||
) -> None:
|
||||
self.bridge = bridge
|
||||
self.spec = spec
|
||||
self.geometry = geometry
|
||||
self.on_timing = on_timing
|
||||
|
||||
def _sync(self) -> None:
|
||||
started = time.monotonic()
|
||||
self.bridge.assert_synchronized(*self.bridge.expected_sets())
|
||||
if self.on_timing is not None:
|
||||
try:
|
||||
self.on_timing({
|
||||
'stage': 'planning_scene_sync',
|
||||
'started_at': started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - started,
|
||||
})
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def attach_tool(self, tool: str) -> None:
|
||||
self.bridge.attach(attached_tool(tool, self.geometry))
|
||||
@@ -367,6 +411,12 @@ class PlannerExecutorNode(Node):
|
||||
self._handle_submit,
|
||||
callback_group=self.callback_group,
|
||||
)
|
||||
self.create_service(
|
||||
SubmitPlan,
|
||||
SUBMIT_PLAN_SERVICE,
|
||||
self._handle_submit_plan,
|
||||
callback_group=self.callback_group,
|
||||
)
|
||||
self.create_service(
|
||||
webots_ros2_msgs.srv.SetString,
|
||||
CANCEL_SERVICE,
|
||||
@@ -402,7 +452,9 @@ class PlannerExecutorNode(Node):
|
||||
callback_group=self.callback_group,
|
||||
)
|
||||
|
||||
self.supervisor = SupervisorFacade(self, self.callback_group)
|
||||
self.supervisor = SupervisorFacade(
|
||||
self, self.callback_group, on_timing=self._on_timing,
|
||||
)
|
||||
self._scene_client = self.create_client(
|
||||
Trigger, SCENE_SERVICE, callback_group=self.callback_group,
|
||||
)
|
||||
@@ -415,8 +467,11 @@ class PlannerExecutorNode(Node):
|
||||
node=self,
|
||||
use_sim_time=True,
|
||||
spin_fn=lambda timeout: time.sleep(max(0.0, float(timeout))),
|
||||
on_timing=self._on_timing,
|
||||
)
|
||||
self.adapter = SceneAdapter(
|
||||
self.bridge, self.spec, self.geometry, on_timing=self._on_timing,
|
||||
)
|
||||
self.adapter = SceneAdapter(self.bridge, self.spec, self.geometry)
|
||||
self.motion = MoveItBackend(
|
||||
self.geometry,
|
||||
self.policy,
|
||||
@@ -592,12 +647,20 @@ class PlannerExecutorNode(Node):
|
||||
def _scene_guard(self) -> tuple[bool, str]:
|
||||
if not self._bridge_ready:
|
||||
return False, 'planning scene bridge is not initialized'
|
||||
started = time.monotonic()
|
||||
try:
|
||||
self.bridge.assert_synchronized(
|
||||
*self.bridge.expected_sets(), timeout_sec=2.0,
|
||||
)
|
||||
except Exception as error:
|
||||
return False, f'planning scene is not synchronized: {error}'
|
||||
finally:
|
||||
self._on_timing({
|
||||
'stage': 'planning_scene_check',
|
||||
'started_at': started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - started,
|
||||
})
|
||||
return True, 'ok'
|
||||
|
||||
def _ensure_motion_ready(self) -> None:
|
||||
@@ -653,14 +716,32 @@ class PlannerExecutorNode(Node):
|
||||
condition = self._frame_conditions.get(topic)
|
||||
if condition is None:
|
||||
return None
|
||||
deadline = time.monotonic() + self.frame_timeout
|
||||
started = time.monotonic()
|
||||
deadline = started + self.frame_timeout
|
||||
with condition:
|
||||
baseline = self._frame_counts[topic]
|
||||
while self._frame_counts[topic] <= baseline:
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0.0:
|
||||
self._on_timing({
|
||||
'stage': 'frame_capture',
|
||||
'camera': name,
|
||||
'started_at': started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - started,
|
||||
'captured': False,
|
||||
})
|
||||
return None
|
||||
condition.wait(timeout=remaining)
|
||||
self._on_timing({
|
||||
'stage': 'frame_capture',
|
||||
'camera': name,
|
||||
'started_at': started,
|
||||
'finished_at': time.monotonic(),
|
||||
'duration_sec': time.monotonic() - started,
|
||||
'captured': True,
|
||||
'stamp': self._frame_stamps.get(topic),
|
||||
})
|
||||
return self._frame_stamps.get(topic)
|
||||
|
||||
# ------------------------------------------------------------------ engine
|
||||
@@ -669,7 +750,7 @@ class PlannerExecutorNode(Node):
|
||||
context = SkillContext(
|
||||
spec=self.spec,
|
||||
geometry=self.geometry,
|
||||
scene_revision=int(envelope.get('scene_revision', 0)),
|
||||
scene_revision=int(envelope.get('state_version', 0)),
|
||||
)
|
||||
context.frame_provider = self._frame_provider
|
||||
context.scene_clear_provider = lambda: True
|
||||
@@ -680,18 +761,24 @@ class PlannerExecutorNode(Node):
|
||||
|
||||
def _publish_plan(self, payload: dict[str, Any]) -> None:
|
||||
self._last_plan = dict(payload)
|
||||
# Bind the timing/log context to the new run immediately; otherwise
|
||||
# the first motion events of a run can be labelled with the previous
|
||||
# run id until the first status message arrives.
|
||||
self._last_status = {
|
||||
'run_id': payload.get('run_id'),
|
||||
'state': 'accepted',
|
||||
'reason': '',
|
||||
}
|
||||
message = String()
|
||||
message.data = json.dumps(payload, ensure_ascii=False)
|
||||
self._plan_publisher.publish(message)
|
||||
self.log.write({
|
||||
'run_id': payload.get('run_id'),
|
||||
'event': 'plan_accepted',
|
||||
'scene_revision': payload.get('scene_revision'),
|
||||
'response': payload.get('response'),
|
||||
})
|
||||
self.get_logger().info(
|
||||
f"accepted graph for run {payload.get('run_id')} "
|
||||
f"(scene_revision={payload.get('scene_revision')})"
|
||||
f"accepted graph for run {payload.get('run_id')}"
|
||||
)
|
||||
|
||||
def _publish_status(self, payload: dict[str, Any]) -> None:
|
||||
@@ -706,14 +793,12 @@ class PlannerExecutorNode(Node):
|
||||
self.log.write({
|
||||
'run_id': run_id,
|
||||
'event': 'replan_required',
|
||||
'scene_revision': None if envelope is None else envelope.get('scene_revision'),
|
||||
})
|
||||
if envelope is not None:
|
||||
self._publish_status({
|
||||
'run_id': run_id,
|
||||
'state': 'replan_scene',
|
||||
'reason': 'fresh scene snapshot for the external planner',
|
||||
'scene_revision': envelope.get('scene_revision'),
|
||||
'scene': envelope,
|
||||
'timestamp': self._now(),
|
||||
})
|
||||
@@ -721,8 +806,27 @@ class PlannerExecutorNode(Node):
|
||||
def _on_stop(self, reason: str) -> None:
|
||||
self.motion.stop(reason)
|
||||
|
||||
def _current_run_id(self) -> str | None:
|
||||
return (
|
||||
(self._last_status or {}).get('run_id')
|
||||
or (self._last_plan or {}).get('run_id')
|
||||
)
|
||||
|
||||
def _on_timing(self, payload: dict[str, Any]) -> None:
|
||||
"""Write monotonic stage timings to the run JSONL log."""
|
||||
self.log.write({
|
||||
**payload,
|
||||
'event': 'timing',
|
||||
'run_id': self._current_run_id(),
|
||||
'timestamp': self._now(),
|
||||
})
|
||||
|
||||
def _on_motion_event(self, payload: dict[str, Any]) -> None:
|
||||
self.log.write({**payload, 'event': payload.get('event', 'motion')})
|
||||
self.log.write({
|
||||
**payload,
|
||||
'event': payload.get('event', 'motion'),
|
||||
'run_id': self._current_run_id(),
|
||||
})
|
||||
|
||||
def _notify_operator(self, reason: str, details: dict[str, Any]) -> bool:
|
||||
self.get_logger().warning(f'operator required: {reason}')
|
||||
@@ -736,22 +840,9 @@ class PlannerExecutorNode(Node):
|
||||
return True
|
||||
|
||||
# ------------------------------------------------------------------ services
|
||||
def _handle_submit(self, request, response):
|
||||
try:
|
||||
payload = json.loads(request.value)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError('submit payload must be a JSON object')
|
||||
run_id = str(payload.get('run_id', '')).strip()
|
||||
scene_revision = int(payload.get('scene_revision'))
|
||||
graph = payload.get('response')
|
||||
if not isinstance(graph, (str, dict)):
|
||||
raise ValueError('response must be a JSON string or object')
|
||||
except Exception as error:
|
||||
response.success = False
|
||||
self.get_logger().error(f'invalid submit request: {error}')
|
||||
return response
|
||||
result = self.engine.submit(run_id, graph, scene_revision)
|
||||
response.success = bool(result.accepted)
|
||||
def _submit_payload(self, run_id: str, graph):
|
||||
"""Shared admission path for both submit services."""
|
||||
result = self.engine.submit(run_id, graph)
|
||||
if result.accepted:
|
||||
self.get_logger().info(
|
||||
f'run {run_id}: {result.code} ({result.message})'
|
||||
@@ -760,16 +851,53 @@ class PlannerExecutorNode(Node):
|
||||
self.get_logger().warning(
|
||||
f'run {run_id} rejected: {result.code}: {result.message}'
|
||||
)
|
||||
self._publish_status({
|
||||
'run_id': run_id,
|
||||
'state': 'rejected',
|
||||
'reason': f'{result.code}: {result.message}',
|
||||
'terminal': result.code,
|
||||
'mounted_tool': None,
|
||||
'held_object': None,
|
||||
'scene_revision': scene_revision,
|
||||
'timestamp': self._now(),
|
||||
})
|
||||
return result
|
||||
|
||||
def _handle_submit(self, request, response):
|
||||
"""Legacy webots_ros2_msgs/SetString submission.
|
||||
|
||||
It is kept for compatibility and routes into the same admission path.
|
||||
A legacy ``scene_revision`` field is ignored: admission always uses the
|
||||
executor's own fresh snapshot.
|
||||
"""
|
||||
try:
|
||||
payload = json.loads(request.value)
|
||||
if not isinstance(payload, dict):
|
||||
raise ValueError('submit payload must be a JSON object')
|
||||
run_id = str(payload.get('run_id', '')).strip()
|
||||
graph = payload.get('response')
|
||||
if not isinstance(graph, (str, dict)):
|
||||
raise ValueError('response must be a JSON string or object')
|
||||
except Exception as error:
|
||||
response.success = False
|
||||
self.get_logger().error(f'invalid submit request: {error}')
|
||||
return response
|
||||
result = self._submit_payload(run_id, graph)
|
||||
response.success = bool(result.accepted)
|
||||
return response
|
||||
|
||||
def _handle_submit_plan(self, request, response):
|
||||
try:
|
||||
run_id = str(request.run_id or '').strip()
|
||||
graph = request.response
|
||||
if not isinstance(graph, str) or not graph.strip():
|
||||
raise ValueError('response must be a non-empty JSON string')
|
||||
except Exception as error:
|
||||
response.accepted = False
|
||||
response.code = 'invalid'
|
||||
response.reason = str(error)
|
||||
response.status_json = ''
|
||||
self.get_logger().error(f'invalid submit_plan request: {error}')
|
||||
return response
|
||||
result = self._submit_payload(run_id, graph)
|
||||
response.accepted = bool(result.accepted)
|
||||
response.code = result.code
|
||||
response.reason = result.message
|
||||
response.status_json = ''
|
||||
if result.accepted:
|
||||
status = self.engine.get_status(run_id)
|
||||
if status.found:
|
||||
response.status_json = status.to_json()
|
||||
return response
|
||||
|
||||
def _handle_cancel(self, request, response):
|
||||
|
||||
@@ -689,8 +689,6 @@ def validate_graph(
|
||||
graph: PlanGraph,
|
||||
scene: Mapping[str, Any],
|
||||
catalog: SkillCatalog,
|
||||
*,
|
||||
expected_scene_revision: int | None = None,
|
||||
) -> ValidationResult:
|
||||
"""Return an admission decision; never mutates the graph or the scene."""
|
||||
if graph.status == 'blocked':
|
||||
@@ -703,14 +701,6 @@ def validate_graph(
|
||||
if len(graph.nodes) > 256:
|
||||
return _reject('graph exceeds the 256-node limit')
|
||||
facts = _facts(scene)
|
||||
if expected_scene_revision is not None:
|
||||
revision = facts.envelope.get('scene_revision')
|
||||
if revision is not None and int(revision) != int(expected_scene_revision):
|
||||
return _reject(
|
||||
f'graph was built for scene_revision {expected_scene_revision}, '
|
||||
f'current revision is {revision}',
|
||||
code='stale_scene',
|
||||
)
|
||||
structural = _structural_checks(graph, facts)
|
||||
if structural is not None:
|
||||
return structural
|
||||
@@ -724,7 +714,7 @@ def validate_graph(
|
||||
{
|
||||
'nodes': len(graph.nodes),
|
||||
'entry': graph.entry,
|
||||
'scene_revision': facts.envelope.get('scene_revision'),
|
||||
'state_version': facts.envelope.get('state_version'),
|
||||
'available_skills': len(facts.available_skills),
|
||||
},
|
||||
)
|
||||
|
||||
@@ -5,8 +5,7 @@ import threading
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from iiwa_experement.geometry import load_geometry
|
||||
from iiwa_experement.scenario import load_scenario
|
||||
from iiwa_llm_plan_executor.engine import GraphEngine
|
||||
from iiwa_llm_plan_executor.schema import parse_response
|
||||
@@ -16,6 +15,7 @@ from iiwa_scene_context.snapshot import build_envelope, build_scene
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
|
||||
GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml')
|
||||
SAFE = {
|
||||
'collision': 'operator_required',
|
||||
'grasp_failed': 'operator_required',
|
||||
@@ -77,10 +77,11 @@ def envelope(revision=1, stamp=5.0, experiment_id=1):
|
||||
'holes': {},
|
||||
},
|
||||
DEFAULT_CATALOG,
|
||||
GEOMETRY,
|
||||
stamp=stamp,
|
||||
)
|
||||
return build_envelope(
|
||||
scene,
|
||||
scene_revision=revision,
|
||||
state_version=revision,
|
||||
stamp=stamp,
|
||||
experiment_id=experiment_id,
|
||||
@@ -129,6 +130,11 @@ GRAB_GRAPH = lambda: graph(
|
||||
next_on={'success': 'completed'}),
|
||||
)
|
||||
|
||||
BAD_GRAPH = lambda: graph(
|
||||
'g',
|
||||
node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}),
|
||||
)
|
||||
|
||||
|
||||
def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False,
|
||||
max_steps=512, **kwargs):
|
||||
@@ -137,6 +143,7 @@ def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False,
|
||||
state = {'envelope': envelope(1, clock_box['now'])}
|
||||
if provider is not None:
|
||||
state['provider'] = provider
|
||||
|
||||
def default_provider():
|
||||
return state['envelope']
|
||||
events = {
|
||||
@@ -164,38 +171,51 @@ def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False,
|
||||
|
||||
|
||||
def test_submission_waits_for_scene_and_the_three_second_delay():
|
||||
engine, _, _, _, clock = make_engine(clock=2.0)
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine, _, _, _, _ = make_engine(clock=2.0)
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert rejected.accepted is False
|
||||
assert rejected.code == 'not_ready'
|
||||
assert 'start delay' in rejected.message
|
||||
assert engine.get_status('r1').found is False
|
||||
|
||||
engine, _, _, _, clock = make_engine(clock=3.01)
|
||||
accepted = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine, _, _, _, _ = make_engine(clock=3.01)
|
||||
accepted = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert accepted.accepted is True
|
||||
engine.run_accepted('r1')
|
||||
|
||||
engine.update_readiness(scene_ready=False, ready_at=0.0, motion_ready=False)
|
||||
rejected = engine.submit('r2', STOP_GRAPH().as_dict(), 1)
|
||||
rejected = engine.submit('r2', STOP_GRAPH().as_dict())
|
||||
assert rejected.code == 'not_ready'
|
||||
|
||||
|
||||
def test_rejected_graphs_are_never_published_or_executed():
|
||||
def test_rejected_graphs_are_not_registered_published_or_executed():
|
||||
engine, skills, events, _, _ = make_engine()
|
||||
blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1)
|
||||
blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []})
|
||||
assert blocked.code == 'blocked'
|
||||
bad_next = graph(
|
||||
'g',
|
||||
node('g', 'grab', 'vacuum', next_on={'success': 'ghost'}),
|
||||
)
|
||||
rejected = engine.submit('r2', bad_next.as_dict(), 1)
|
||||
rejected = engine.submit('r2', BAD_GRAPH().as_dict())
|
||||
assert rejected.accepted is False
|
||||
assert rejected.code == 'rejected'
|
||||
assert events['plans'] == []
|
||||
assert skills.calls == []
|
||||
stale = engine.submit('r3', STOP_GRAPH().as_dict(), 2)
|
||||
assert stale.code == 'stale_scene'
|
||||
assert engine.get_status('r2').state == 'rejected'
|
||||
assert engine.get_status('r1').found is False
|
||||
assert engine.get_status('r2').found is False
|
||||
assert not any(
|
||||
event.get('state') == 'rejected' for event in events['statuses']
|
||||
)
|
||||
|
||||
|
||||
def test_rejected_run_id_can_be_reused_with_a_corrected_graph():
|
||||
engine, skills, events, _, _ = make_engine()
|
||||
rejected = engine.submit('same-id', BAD_GRAPH().as_dict())
|
||||
assert rejected.accepted is False
|
||||
assert engine.get_status('same-id').found is False
|
||||
|
||||
accepted = engine.submit('same-id', STOP_GRAPH().as_dict())
|
||||
assert accepted.accepted is True
|
||||
assert accepted.code == 'accepted'
|
||||
assert len(events['plans']) == 1
|
||||
engine.run_accepted('same-id')
|
||||
assert engine.get_status('same-id').state == 'completed'
|
||||
|
||||
|
||||
def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed():
|
||||
@@ -207,7 +227,7 @@ def test_cap_grab_without_vacuum_is_admitted_and_follows_grasp_failed():
|
||||
'grasp_failed': 'operator_required',
|
||||
}),
|
||||
)
|
||||
accepted = engine.submit('r1', value.as_dict(), 1)
|
||||
accepted = engine.submit('r1', value.as_dict())
|
||||
assert accepted.accepted is True
|
||||
assert len(events['plans']) == 1
|
||||
skills.events = {'g': 'grasp_failed'}
|
||||
@@ -232,7 +252,7 @@ def test_accepted_graph_is_published_once_before_the_first_motion():
|
||||
|
||||
engine.on_plan = on_plan
|
||||
skills.on_execute = lambda node, context: order.append(('move', node.id))
|
||||
accepted = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
accepted = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert accepted.accepted
|
||||
engine.run_accepted('r1')
|
||||
assert order[0] == ('plan', 'r1')
|
||||
@@ -241,25 +261,62 @@ def test_accepted_graph_is_published_once_before_the_first_motion():
|
||||
status = engine.get_status('r1')
|
||||
assert status.state == 'completed'
|
||||
assert status.found is True
|
||||
assert 'scene_revision' not in status.as_dict()
|
||||
|
||||
|
||||
def test_same_run_id_is_idempotent_and_a_second_run_is_busy():
|
||||
def test_same_run_id_is_idempotent_and_a_different_graph_conflicts():
|
||||
engine, _, events, _, _ = make_engine()
|
||||
first = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
first = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert first.accepted
|
||||
duplicate = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
duplicate = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert duplicate.accepted is True
|
||||
assert duplicate.code == 'duplicate'
|
||||
assert len(events['plans']) == 1
|
||||
busy = engine.submit('r2', STOP_GRAPH().as_dict(), 1)
|
||||
|
||||
conflict = engine.submit('r1', GRAB_GRAPH().as_dict())
|
||||
assert conflict.accepted is False
|
||||
assert conflict.code == 'conflict'
|
||||
assert len(events['plans']) == 1
|
||||
|
||||
busy = engine.submit('r2', STOP_GRAPH().as_dict())
|
||||
assert busy.accepted is False
|
||||
assert busy.code == 'busy'
|
||||
assert len(events['plans']) == 1
|
||||
|
||||
|
||||
def test_accepted_run_id_stays_bound_after_a_terminal_state():
|
||||
engine, _, events, _, _ = make_engine()
|
||||
assert engine.submit('r1', STOP_GRAPH().as_dict()).accepted
|
||||
engine.run_accepted('r1')
|
||||
assert engine.get_status('r1').state == 'completed'
|
||||
|
||||
duplicate = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert duplicate.accepted is True
|
||||
assert duplicate.code == 'duplicate'
|
||||
conflict = engine.submit('r1', GRAB_GRAPH().as_dict())
|
||||
assert conflict.accepted is False
|
||||
assert conflict.code == 'conflict'
|
||||
assert len(events['plans']) == 1
|
||||
|
||||
|
||||
def test_plan_publication_failure_does_not_register_the_run():
|
||||
engine, skills, _, _, _ = make_engine()
|
||||
|
||||
def broken(_payload):
|
||||
raise RuntimeError('publisher unavailable')
|
||||
|
||||
engine.on_plan = broken
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert rejected.accepted is False
|
||||
assert rejected.code == 'rejected'
|
||||
assert 'publisher unavailable' in rejected.message
|
||||
assert engine.get_status('r1').found is False
|
||||
assert skills.calls == []
|
||||
|
||||
|
||||
def test_each_event_selects_exactly_one_edge_and_is_visible_in_status():
|
||||
engine, skills, _, _, _ = make_engine()
|
||||
engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
skills.events = {'a': 'success', 'b': 'success'}
|
||||
engine.run_accepted('r1')
|
||||
status = engine.get_status('r1')
|
||||
@@ -267,6 +324,7 @@ def test_each_event_selects_exactly_one_edge_and_is_visible_in_status():
|
||||
assert [event['node_id'] for event in status.events] == ['a', 'b']
|
||||
assert [event['next'] for event in status.events] == ['b', 'completed']
|
||||
assert all(event['run_id'] == 'r1' for event in status.events)
|
||||
assert all('scene_revision' not in event for event in status.events)
|
||||
|
||||
|
||||
def test_replan_returns_a_fresh_scene_and_operator_required_stops():
|
||||
@@ -276,12 +334,12 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops():
|
||||
node('d', 'detect', 'cap',
|
||||
next_on={'found': 'replan_required'}),
|
||||
)
|
||||
engine.submit('r1', replan_graph.as_dict(), 1)
|
||||
engine.submit('r1', replan_graph.as_dict())
|
||||
skills.events = {'d': 'found'}
|
||||
engine.run_accepted('r1')
|
||||
assert engine.get_status('r1').state == 'replan_required'
|
||||
assert len(events['replans']) == 1
|
||||
assert events['replans'][0]['scene_revision'] == 1
|
||||
assert events['replans'][0]['state_version'] == 1
|
||||
|
||||
engine, skills, events, _, _ = make_engine()
|
||||
operator_graph = graph(
|
||||
@@ -289,7 +347,7 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops():
|
||||
node('d', 'detect', 'cap',
|
||||
next_on={'found': 'operator_required'}),
|
||||
)
|
||||
engine.submit('r2', operator_graph.as_dict(), 1)
|
||||
engine.submit('r2', operator_graph.as_dict())
|
||||
skills.events = {'d': 'found'}
|
||||
engine.run_accepted('r2')
|
||||
assert engine.get_status('r2').state == 'operator_required'
|
||||
@@ -297,7 +355,7 @@ def test_replan_returns_a_fresh_scene_and_operator_required_stops():
|
||||
|
||||
def test_unknown_skill_outcome_stops_execution():
|
||||
engine, skills, events, _, _ = make_engine()
|
||||
engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
skills.events = {'a': 'bogus'}
|
||||
engine.run_accepted('r1')
|
||||
status = engine.get_status('r1')
|
||||
@@ -309,30 +367,30 @@ def test_unknown_skill_outcome_stops_execution():
|
||||
def test_lost_freshness_and_missing_scene_stop_motion():
|
||||
engine, _, events, _, _ = make_engine()
|
||||
engine.robot_fresh = lambda: (False, 'joint_states is stale')
|
||||
engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
engine.run_accepted('r1')
|
||||
assert engine.get_status('r1').state == 'failed'
|
||||
assert 'joint_states' in engine.get_status('r1').reason
|
||||
assert events['stops']
|
||||
|
||||
engine, skills, events, state, _ = make_engine()
|
||||
engine.submit('r2', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r2', STOP_GRAPH().as_dict())
|
||||
skills.on_execute = lambda node, context: state.update(envelope=None)
|
||||
engine.run_accepted('r2')
|
||||
assert engine.get_status('r2').state == 'failed'
|
||||
assert 'scene snapshot' in engine.get_status('r2').reason
|
||||
|
||||
|
||||
def test_scene_revision_change_stops_non_ownership_nodes_but_not_own_graph_changes():
|
||||
def test_scene_state_change_stops_non_ownership_nodes_but_not_own_graph_changes():
|
||||
engine, skills, events, state, _ = make_engine()
|
||||
engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
skills.on_execute = lambda node, context: state.update(envelope=envelope(2))
|
||||
engine.run_accepted('r1')
|
||||
assert engine.get_status('r1').state == 'failed'
|
||||
assert 'changed externally' in engine.get_status('r1').reason
|
||||
|
||||
engine, skills, events, state, _ = make_engine()
|
||||
engine.submit('r2', GRAB_GRAPH().as_dict(), 1)
|
||||
engine.submit('r2', GRAB_GRAPH().as_dict())
|
||||
|
||||
def own_change(node, context):
|
||||
if node.id == 'g':
|
||||
@@ -343,10 +401,22 @@ def test_scene_revision_change_stops_non_ownership_nodes_but_not_own_graph_chang
|
||||
engine.run_accepted('r2')
|
||||
status = engine.get_status('r2')
|
||||
assert status.state == 'completed'
|
||||
assert status.scene_revision == 2
|
||||
assert status.mounted_tool == 'vacuum'
|
||||
|
||||
|
||||
def test_changed_scene_before_the_first_motion_stops_the_run():
|
||||
engine, skills, events, state, _ = make_engine()
|
||||
accepted = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert accepted.accepted
|
||||
state['envelope'] = envelope(2)
|
||||
engine.run_accepted('r1')
|
||||
status = engine.get_status('r1')
|
||||
assert status.state == 'failed'
|
||||
assert 'before the first motion' in status.reason
|
||||
assert skills.calls == []
|
||||
assert events['stops']
|
||||
|
||||
|
||||
def test_step_limit_stops_a_recovering_cycle():
|
||||
engine, skills, _, _, _ = make_engine(max_steps=3)
|
||||
cycle = graph(
|
||||
@@ -356,7 +426,7 @@ def test_step_limit_stops_a_recovering_cycle():
|
||||
node('b', 'wait_scene_clear', 'workspace',
|
||||
next_on={'clear': 'a'}),
|
||||
)
|
||||
engine.submit('r1', cycle.as_dict(), 1)
|
||||
engine.submit('r1', cycle.as_dict())
|
||||
skills.events = {'a': 'clear', 'b': 'clear'}
|
||||
engine.run_accepted('r1')
|
||||
status = engine.get_status('r1')
|
||||
@@ -371,9 +441,8 @@ def test_cancellation_stops_the_active_graph_and_preserves_ownership():
|
||||
lambda node, context: setattr(context, 'mounted_tool', node.object)
|
||||
if node.skill == 'grab' else None
|
||||
)
|
||||
engine.submit('r1', GRAB_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', GRAB_GRAPH().as_dict())
|
||||
assert skills.started.wait(timeout=2.0)
|
||||
# A tool grab confirms ownership before the next node.
|
||||
result = engine.cancel('r1')
|
||||
assert result.accepted
|
||||
assert events['stops']
|
||||
@@ -387,7 +456,7 @@ def test_cancellation_stops_the_active_graph_and_preserves_ownership():
|
||||
def test_cancel_rejects_unknown_or_finished_runs():
|
||||
engine, _, _, _, _ = make_engine()
|
||||
assert engine.cancel('missing').accepted is False
|
||||
engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
engine.run_accepted('r1')
|
||||
assert engine.cancel('r1').accepted is False
|
||||
|
||||
@@ -399,15 +468,17 @@ def test_status_query_marks_unknown_ids():
|
||||
assert status.state == 'unknown'
|
||||
|
||||
|
||||
def test_not_ready_or_unavailable_scene_is_rejected():
|
||||
def test_not_ready_or_unavailable_scene_is_rejected_without_registration():
|
||||
engine, _, _, state, _ = make_engine()
|
||||
state['envelope'] = None
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert rejected.code == 'not_ready'
|
||||
assert engine.get_status('r1').found is False
|
||||
|
||||
|
||||
def test_stale_scene_timestamp_is_rejected_before_execution():
|
||||
engine, _, _, state, clock = make_engine(clock=10.0)
|
||||
engine, _, _, state, _ = make_engine(clock=10.0)
|
||||
state['envelope'] = envelope(1, stamp=1.0)
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
|
||||
rejected = engine.submit('r1', STOP_GRAPH().as_dict())
|
||||
assert rejected.code == 'stale_scene'
|
||||
assert engine.get_status('r1').found is False
|
||||
|
||||
@@ -87,10 +87,11 @@ def envelope(*, mounted: str | None = None):
|
||||
'holes': {},
|
||||
},
|
||||
DEFAULT_CATALOG,
|
||||
GEOMETRY,
|
||||
stamp=5.0,
|
||||
)
|
||||
return build_envelope(
|
||||
scene,
|
||||
scene_revision=1,
|
||||
state_version=1,
|
||||
stamp=5.0,
|
||||
experiment_id=20,
|
||||
@@ -108,7 +109,7 @@ def make_engine(*, mounted: str | None = None):
|
||||
context = SkillContext(
|
||||
spec=SPEC,
|
||||
geometry=GEOMETRY,
|
||||
scene_revision=int(env['scene_revision']),
|
||||
scene_revision=int(env['state_version']),
|
||||
)
|
||||
context.frame_provider = lambda name: 5.0
|
||||
context.notifier = lambda reason, details: True
|
||||
@@ -128,15 +129,10 @@ def make_engine(*, mounted: str | None = None):
|
||||
|
||||
def test_full_qwen_graph_empty_patron_follows_actual_failure_path():
|
||||
engine, motion, supervisor, scene_envelope = make_engine(mounted=None)
|
||||
result = validate_graph(
|
||||
FULL_GRAPH,
|
||||
scene_envelope,
|
||||
DEFAULT_CATALOG,
|
||||
expected_scene_revision=1,
|
||||
)
|
||||
result = validate_graph(FULL_GRAPH, scene_envelope, DEFAULT_CATALOG)
|
||||
assert result.ok is True, result.reason
|
||||
|
||||
accepted = engine.submit('full-empty', FULL_GRAPH.as_dict(), 1)
|
||||
accepted = engine.submit('full-empty', FULL_GRAPH.as_dict())
|
||||
assert accepted.accepted is True
|
||||
engine.run_accepted('full-empty')
|
||||
status = engine.get_status('full-empty')
|
||||
@@ -205,7 +201,6 @@ def test_full_qwen_graph_with_vacuum_has_a_success_cycle_without_completed():
|
||||
FULL_GRAPH,
|
||||
envelope(mounted='vacuum'),
|
||||
DEFAULT_CATALOG,
|
||||
expected_scene_revision=1,
|
||||
)
|
||||
assert result.ok is False
|
||||
assert 'a_retry_pick' in result.reason
|
||||
|
||||
@@ -16,7 +16,7 @@ from iiwa_llm_plan_executor.node import (
|
||||
_safe_name,
|
||||
plan_qos,
|
||||
)
|
||||
from iiwa_msgs.srv import GetRunStatus
|
||||
from iiwa_msgs.srv import GetRunStatus, SubmitPlan
|
||||
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
@@ -39,14 +39,14 @@ class FakeEngine:
|
||||
self.submitted = []
|
||||
self.cancelled = []
|
||||
|
||||
def submit(self, run_id, graph, revision):
|
||||
self.submitted.append((run_id, graph, revision))
|
||||
def submit(self, run_id, graph):
|
||||
self.submitted.append((run_id, graph))
|
||||
return self.admission
|
||||
|
||||
def get_status(self, run_id):
|
||||
return self.status or RunStatus(
|
||||
True, run_id, 'completed', 'done', 'a', 'stop', 'success',
|
||||
'completed', 1, 'completed', 'vacuum', None, (),
|
||||
'completed', 'completed', 'vacuum', None, (),
|
||||
)
|
||||
|
||||
def cancel(self, run_id):
|
||||
@@ -102,18 +102,19 @@ def test_safe_name_and_run_log_are_machine_readable(tmp_path):
|
||||
|
||||
def test_plan_is_published_once_and_kept_for_late_subscribers():
|
||||
node = fake_node()
|
||||
payload = {'run_id': 'r1', 'scene_revision': 2, 'response': '{}'}
|
||||
payload = {'run_id': 'r1', 'response': '{}'}
|
||||
PlannerExecutorNode._publish_plan(node, payload)
|
||||
PlannerExecutorNode._publish_plan(node, payload)
|
||||
assert len(node._plan_publisher.messages) == 2
|
||||
assert node._last_plan == payload
|
||||
assert json.loads(node._plan_publisher.messages[-1])['run_id'] == 'r1'
|
||||
assert 'scene_revision' not in json.loads(
|
||||
node._plan_publisher.messages[-1]
|
||||
)
|
||||
|
||||
|
||||
def test_submit_parses_json_and_forwards_the_untouched_response():
|
||||
node = fake_node()
|
||||
published = []
|
||||
node._publish_status = lambda payload: published.append(payload)
|
||||
request = SimpleNamespace(value=json.dumps({
|
||||
'run_id': 'r1',
|
||||
'scene_revision': 3,
|
||||
@@ -126,34 +127,65 @@ def test_submit_parses_json_and_forwards_the_untouched_response():
|
||||
assert node.engine.submitted[0][1] == {
|
||||
'status': 'ready', 'entry': 'a', 'plan': [],
|
||||
}
|
||||
assert node.engine.submitted[0][2] == 3
|
||||
assert len(node.engine.submitted[0]) == 2
|
||||
|
||||
|
||||
def test_rejected_submit_returns_false_and_publishes_the_reason():
|
||||
def test_submit_plan_service_returns_code_and_reason_without_registration():
|
||||
node = fake_node(engine=FakeEngine(
|
||||
admission=AdmissionResult(False, 'not_ready', 'waiting for scene', 'r1'),
|
||||
))
|
||||
response = SubmitPlan.Response()
|
||||
PlannerExecutorNode._handle_submit_plan(
|
||||
node,
|
||||
SimpleNamespace(run_id='r1', response='{"status":"blocked","entry":"","plan":[]}'),
|
||||
response,
|
||||
)
|
||||
assert response.accepted is False
|
||||
assert response.code == 'not_ready'
|
||||
assert response.reason == 'waiting for scene'
|
||||
assert response.status_json == ''
|
||||
assert len(node.engine.submitted) == 1
|
||||
|
||||
|
||||
def test_submit_plan_service_returns_the_status_of_an_accepted_run():
|
||||
node = fake_node()
|
||||
response = SubmitPlan.Response()
|
||||
PlannerExecutorNode._handle_submit_plan(
|
||||
node,
|
||||
SimpleNamespace(run_id='r1', response='{"status":"ready","entry":"a","plan":[]}'),
|
||||
response,
|
||||
)
|
||||
assert response.accepted is True
|
||||
assert response.code == 'accepted'
|
||||
status = json.loads(response.status_json)
|
||||
assert status['run_id'] == 'r1'
|
||||
assert 'scene_revision' not in status
|
||||
|
||||
|
||||
def test_rejected_submit_returns_false_without_registering_or_publishing():
|
||||
node = fake_node(engine=FakeEngine(
|
||||
admission=AdmissionResult(False, 'replan_required', 'need vacuum', 'r1'),
|
||||
))
|
||||
published = []
|
||||
node._publish_status = lambda payload: published.append(payload)
|
||||
request = SimpleNamespace(value=json.dumps({
|
||||
'run_id': 'r1', 'scene_revision': 1,
|
||||
'run_id': 'r1',
|
||||
'response': {'status': 'blocked', 'entry': '', 'plan': []},
|
||||
}))
|
||||
response = SimpleNamespace(success=True)
|
||||
PlannerExecutorNode._handle_submit(node, request, response)
|
||||
assert response.success is False
|
||||
assert published[-1]['state'] == 'rejected'
|
||||
assert 'replan_required' in published[-1]['reason']
|
||||
assert published == []
|
||||
|
||||
|
||||
def test_replan_scene_status_keeps_the_run_id():
|
||||
node = fake_node()
|
||||
node._last_status = {'run_id': 'run-42', 'state': 'running'}
|
||||
envelope = {'scene_revision': 7, 'scene_json': {'components': []}}
|
||||
envelope = {'state_version': 7, 'scene_json': {'components': []}}
|
||||
PlannerExecutorNode._on_replan(node, envelope)
|
||||
payload = json.loads(node._status_publisher.messages[-1])
|
||||
assert payload['run_id'] == 'run-42'
|
||||
assert payload['scene_revision'] == 7
|
||||
assert 'scene_revision' not in payload
|
||||
assert payload['scene'] == envelope
|
||||
|
||||
|
||||
@@ -197,7 +229,7 @@ def test_scene_provider_enforces_freshness():
|
||||
node._now = lambda: 10.0
|
||||
node.scene_max_age = 2.0
|
||||
node._latest_scene = {
|
||||
'timestamp': 9.0, 'scene_revision': 1, 'scene_json': {},
|
||||
'timestamp': 9.0, 'state_version': 1, 'scene_json': {},
|
||||
}
|
||||
assert PlannerExecutorNode._scene_provider(node) is not None
|
||||
node._latest_scene['timestamp'] = 7.0
|
||||
@@ -213,7 +245,7 @@ def test_context_factory_sets_sources_and_inspection_configuration():
|
||||
node._frame_provider = lambda name: 1.0
|
||||
node._notify_operator = lambda reason, details: True
|
||||
context = PlannerExecutorNode._context_factory(
|
||||
node, {'scene_revision': 4}, {},
|
||||
node, {'state_version': 4}, {},
|
||||
)
|
||||
assert context.scene_revision == 4
|
||||
assert context.scene_adapter == 'adapter'
|
||||
@@ -229,10 +261,10 @@ def test_future_scene_timestamps_cannot_delay_the_start_gate():
|
||||
node._scene_ready_at = None
|
||||
node._latest_scene = None
|
||||
message = String()
|
||||
message.data = json.dumps({'timestamp': 500.0, 'scene_revision': 1})
|
||||
message.data = json.dumps({'timestamp': 500.0, 'state_version': 1})
|
||||
PlannerExecutorNode._on_scene(node, message)
|
||||
assert node._scene_ready_at == 10.0
|
||||
message.data = json.dumps({'timestamp': 5.0, 'scene_revision': 1})
|
||||
message.data = json.dumps({'timestamp': 5.0, 'state_version': 1})
|
||||
PlannerExecutorNode._on_scene(node, message)
|
||||
assert node._scene_ready_at == 5.0
|
||||
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
"""Graph admission: structure, safe branches, ownership and scene revision."""
|
||||
"""Graph admission: structure, safe branches and ownership."""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from iiwa_experement.geometry import load_geometry
|
||||
from iiwa_experement.scenario import load_scenario
|
||||
from iiwa_llm_plan_executor.schema import PlanSchemaError, parse_response
|
||||
from iiwa_llm_plan_executor.validator import validate_graph
|
||||
@@ -14,6 +15,7 @@ from iiwa_scene_context.snapshot import build_envelope, build_scene
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
|
||||
GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml')
|
||||
SAFE_DEFAULTS = {
|
||||
'collision': 'operator_required',
|
||||
'grasp_failed': 'operator_required',
|
||||
@@ -33,6 +35,7 @@ def spec_for(experiment_id: int = 1):
|
||||
|
||||
def scene_for(experiment_id: int = 1, *, mounted=None, held=None,
|
||||
cap_state=None, revision=1):
|
||||
del revision
|
||||
spec = spec_for(experiment_id)
|
||||
tools = {tool.name: {'state': 'docked'} for tool in spec.tools}
|
||||
if mounted:
|
||||
@@ -50,10 +53,11 @@ def scene_for(experiment_id: int = 1, *, mounted=None, held=None,
|
||||
'holes': {},
|
||||
},
|
||||
DEFAULT_CATALOG,
|
||||
GEOMETRY,
|
||||
stamp=10.0,
|
||||
)
|
||||
return build_envelope(
|
||||
scene,
|
||||
scene_revision=revision,
|
||||
state_version=1,
|
||||
stamp=10.0,
|
||||
experiment_id=experiment_id,
|
||||
@@ -93,13 +97,8 @@ def graph(entry_id, *nodes):
|
||||
})
|
||||
|
||||
|
||||
def expect(graph_value, scene, *, expected_revision=None):
|
||||
return validate_graph(
|
||||
graph_value,
|
||||
scene,
|
||||
DEFAULT_CATALOG,
|
||||
expected_scene_revision=expected_revision,
|
||||
)
|
||||
def expect(graph_value, scene):
|
||||
return validate_graph(graph_value, scene, DEFAULT_CATALOG)
|
||||
|
||||
|
||||
def test_blocked_status_never_reaches_execution():
|
||||
@@ -358,13 +357,6 @@ def test_cycles_are_bounded_and_must_keep_a_recovery_branch():
|
||||
assert expect(recovering_cycle, scene_for(1)).ok is True
|
||||
|
||||
|
||||
def test_wrong_scene_revision_is_rejected():
|
||||
value = graph('a', node('a', 'detect', 'cap'))
|
||||
result = expect(value, scene_for(1, revision=3), expected_revision=2)
|
||||
assert result.ok is False
|
||||
assert result.code == 'stale_scene'
|
||||
|
||||
|
||||
def test_aliases_for_pick_move_to_storage_and_inspect_are_supported():
|
||||
value = graph(
|
||||
'grab_vacuum',
|
||||
|
||||
@@ -13,6 +13,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"srv/MoveToPose.srv"
|
||||
"srv/MoveToNamedPose.srv"
|
||||
"srv/GetRunStatus.srv"
|
||||
"srv/SubmitPlan.srv"
|
||||
"action/MoveToPose.action"
|
||||
"action/MoveToJoints.action"
|
||||
DEPENDENCIES geometry_msgs action_msgs
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
# Submit a ready-made plan graph without a user-supplied scene revision.
|
||||
string run_id
|
||||
# The untouched graph as a JSON string (object or string form).
|
||||
string response
|
||||
---
|
||||
bool accepted
|
||||
# accepted | duplicate | invalid | rejected | not_ready | busy | conflict
|
||||
string code
|
||||
# Human-readable reason for the admission decision.
|
||||
string reason
|
||||
# JSON status of an accepted or duplicate run; empty when nothing was registered.
|
||||
string status_json
|
||||
@@ -1,75 +1,115 @@
|
||||
# `iiwa_scene_context` interfaces
|
||||
# Интерфейсы `iiwa_scene_context`
|
||||
|
||||
Contract version: **1**. Catalog version: **1**. Scene schema version: **1**.
|
||||
All JSON is UTF-8 and uses stable Webots object ids.
|
||||
Версия схемы сцены: **2**. Версия контракта: **2**. Версия каталога: **2**.
|
||||
Весь JSON — UTF-8, идентификаторы объектов стабильны и совпадают с Webots DEF.
|
||||
|
||||
## ROS interfaces
|
||||
## ROS-интерфейсы
|
||||
|
||||
| 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. |
|
||||
| сервис | `/iiwa_scene_context/get_scene` | `std_srvs/Trigger` | `success=true`, `message` = JSON конверта; иначе причина (сцена не готова / устарела / нет состояния). |
|
||||
| топик | `/iiwa_scene_context/scene_json` | `std_msgs/String` | Тот же конверт. QoS: reliable, transient-local, depth 1; поздний подписчик получает последний снимок. |
|
||||
| вход | `/iiwa_experement/object_state` | `std_msgs/String` | Живое состояние supervisor: владение, состояния и позы объектов. |
|
||||
| вход | `/iiwa_experement/scene_ready` | `std_srvs/Trigger` | Опрашивается узлом. До `success=true` снимок не публикуется. |
|
||||
|
||||
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.
|
||||
Узел отвечает `success=false`, пока сцена не готова, пока не пришло первое
|
||||
состояние supervisor или пока последнее состояние старше `freshness_sec`
|
||||
(по умолчанию 2.0 с). Устаревший снимок нельзя трактовать как актуальный.
|
||||
|
||||
## Snapshot envelope
|
||||
## Конверт снимка
|
||||
|
||||
```json
|
||||
{
|
||||
"schema_version": 1,
|
||||
"contract_version": 1,
|
||||
"catalog_version": 1,
|
||||
"schema_version": 2,
|
||||
"contract_version": 2,
|
||||
"catalog_version": 2,
|
||||
"experiment_id": 20,
|
||||
"state_version": 7,
|
||||
"scene_revision": 3,
|
||||
"timestamp": 12.5,
|
||||
"scene_json": { "...": "see below" }
|
||||
"timestamp": 42.0,
|
||||
"scene_json": { "...": "см. ниже" }
|
||||
}
|
||||
```
|
||||
|
||||
* `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.
|
||||
* `state_version` — дискретная версия supervisor. Она растёт при подтверждённом
|
||||
изменении владения; обновление позы удерживаемого предмета её не меняет.
|
||||
* `scene_revision` **не входит** в публичный контракт: клиент не передаёт и не
|
||||
получает его. Исполнитель сам хранит внутренний счётчик для защиты от
|
||||
внешнего изменения сцены во время выполнения.
|
||||
* `timestamp` — время получения снимка. Потребитель сам проверяет свежесть.
|
||||
|
||||
## `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`.
|
||||
Поля: `schema_version`, `contract_version`, `catalog_version`, `profile`,
|
||||
`experiment_id`, `domain`, `pose_conventions`, `components`, `destinations`,
|
||||
`tools`, `initial_state`, `cameras`, `available_skills`, `skill_contracts`,
|
||||
`action_rules`, `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.
|
||||
`components` содержит только реально созданные объекты выбранного
|
||||
эксперимента: всегда `fixture`, `body`; условно `cap`, `tray`, `screw_1`…
|
||||
`screw_10`. `tools` содержит только присутствующие инструменты.
|
||||
`destinations` содержит `work`, `hatch_seat`, `hole_1`…`hole_4`, гнездо
|
||||
`<tool>_socket` для каждого инструмента и только те участки герметика
|
||||
`side_i`, которые заданы в сцене. Для каждого участка публикуются начальная и
|
||||
конечная позы.
|
||||
|
||||
`initial_state` is logical only:
|
||||
### Позы
|
||||
|
||||
Единые правила: `frame_id="world"`, метры, кватернион `xyzw`.
|
||||
|
||||
```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
|
||||
"frame_id": "world",
|
||||
"position": {"x": 0.3, "y": 0.12, "z": 0.823},
|
||||
"orientation": {"x": 0.0, "y": 0.0, "z": 0.3826834, "w": 0.9238795},
|
||||
"source": "webots_supervisor",
|
||||
"stamp": 42.0
|
||||
}
|
||||
```
|
||||
|
||||
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.
|
||||
* `source="webots_supervisor"` — живая поза из supervisor. Пока предмет
|
||||
удерживается, публикуется его текущая поза, а не начальная.
|
||||
* `source="scenario_manifest"` — запасная начальная поза, если supervisor
|
||||
ещё не прислал объект (например, в unit-тестах).
|
||||
* `source="executor_geometry"` — производная цель. Она вычисляется теми же
|
||||
функциями `world_target`, `sealant_point` и `tcp_goal`, что использует
|
||||
исполнитель, поэтому поза в `scene_json` и цель движения не расходятся.
|
||||
|
||||
`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.
|
||||
Позы есть у `components` и `tools` в поле `pose`. У мест назначения:
|
||||
`pose` (поза объекта/рабочей точки) и `tcp_pose` (цель TCP). Участок
|
||||
герметика содержит `start`, `end`, `start_tcp`, `end_tcp`. Числа проверяются
|
||||
на конечность, кадр — на `world`, кватернион — на нормировку.
|
||||
|
||||
### Правила действий
|
||||
|
||||
`action_rules` — компактные машиночитаемые правила для каждого доступного
|
||||
навыка:
|
||||
|
||||
```json
|
||||
"grab": {
|
||||
"object_types": ["hatch", "screw", "tool"],
|
||||
"target_types": [],
|
||||
"objects": ["cap", "screw_1", "vacuum", "..."],
|
||||
"targets": [],
|
||||
"required_tools": [],
|
||||
"required_tool_by_kind": {"hatch": "vacuum", "screw": "magnetic_tool"},
|
||||
"preconditions": ["gripper is empty"],
|
||||
"result": "tool mounted or part attached to the mounted tool"
|
||||
}
|
||||
```
|
||||
|
||||
Правила строятся из выбранной сцены, поэтому отсутствующие ID и навыки не
|
||||
рекламируются. В частности:
|
||||
|
||||
* `grab(cap)` требует `vacuum`, `grab(screw_i)` — `magnetic_tool`;
|
||||
* `dispense` доступен только при наличии `sealant_tool` и участка `side_i`;
|
||||
* `dock_tool` требует предшествующий `transfer(<tool>, <tool>_socket)` в
|
||||
собственное гнездо;
|
||||
* `handoff_screw` только передаёт винт виртуальному оператору у отверстия;
|
||||
вставки и затяжки нет, отверстие остаётся пустым;
|
||||
* `tighten` не рекламируется, пока навык не реализован;
|
||||
* `move_to_storage` не применяется к инструменту и не ведёт `sealant_tool` в
|
||||
`hatch_seat`.
|
||||
|
||||
Сгенерированный пример для эксперимента 20:
|
||||
`docs/scene_json_experiment_20.json`. Он создаётся тем же `build_scene`, а не
|
||||
заменяет динамическую генерацию; тест сверяет пример с генератором.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -13,9 +13,9 @@ from typing import Any
|
||||
from iiwa_experement.scenario import Tool, ScenarioSpec
|
||||
|
||||
|
||||
SCENE_SCHEMA_VERSION = 1
|
||||
CONTRACT_VERSION = 1
|
||||
CATALOG_VERSION = 1
|
||||
SCENE_SCHEMA_VERSION = 2
|
||||
CONTRACT_VERSION = 2
|
||||
CATALOG_VERSION = 2
|
||||
|
||||
NATIVE_SKILLS = (
|
||||
'detect', 'approach', 'lower', 'grab', 'lift', 'transfer', 'align',
|
||||
@@ -41,6 +41,7 @@ TOOL_KINDS = {
|
||||
_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')
|
||||
CAMERA_IDS = ('top', 'hand')
|
||||
|
||||
|
||||
class CatalogError(ValueError):
|
||||
@@ -206,10 +207,9 @@ class SkillContract:
|
||||
outcomes: dict[str, str]
|
||||
|
||||
def as_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
'description': self.description,
|
||||
'outcomes': dict(self.outcomes),
|
||||
}
|
||||
# Descriptions live once in action_rules; the contract only needs the
|
||||
# event names so the graph and the validator agree on the edges.
|
||||
return {'outcomes': dict(self.outcomes)}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -220,15 +220,29 @@ class SkillCatalog:
|
||||
catalog_version: int = CATALOG_VERSION
|
||||
profile: str = 'grant'
|
||||
|
||||
def skill_names(self) -> tuple[str, ...]:
|
||||
return AVAILABLE_SKILLS
|
||||
def skill_names(self, spec: ScenarioSpec | None = None) -> tuple[str, ...]:
|
||||
"""Skills that the real executor can run in this selected scene."""
|
||||
names = list(AVAILABLE_SKILLS)
|
||||
if spec is not None:
|
||||
if not spec.sealant_sides or 'sealant_tool' not in spec.tool_names:
|
||||
names.remove('dispense')
|
||||
if not spec.tools:
|
||||
names.remove('dock_tool')
|
||||
if not spec.screws:
|
||||
names.remove('handoff_screw')
|
||||
if spec.cap_state == 'absent' and not spec.screws:
|
||||
names.remove('move_to_storage')
|
||||
return tuple(names)
|
||||
|
||||
def skill_contracts(self) -> dict[str, dict[str, Any]]:
|
||||
def skill_contracts(
|
||||
self,
|
||||
spec: ScenarioSpec | None = None,
|
||||
) -> dict[str, dict[str, Any]]:
|
||||
return {
|
||||
name: SkillContract(
|
||||
name, SKILL_DESCRIPTIONS[name], SKILL_OUTCOMES[name],
|
||||
).as_dict()
|
||||
for name in AVAILABLE_SKILLS
|
||||
for name in self.skill_names(spec)
|
||||
}
|
||||
|
||||
def outcomes(self, skill: str) -> dict[str, str]:
|
||||
@@ -322,6 +336,261 @@ class SkillCatalog:
|
||||
if side not in targets:
|
||||
raise CatalogError(f'sealant side {side!r} is not a destination')
|
||||
|
||||
# ------------------------------------------------------------------ rules
|
||||
def _socket_ids(self, spec: ScenarioSpec) -> list[str]:
|
||||
return [self.tool_socket_id(tool.name) for tool in spec.tools]
|
||||
|
||||
def _screw_ids(self, spec: ScenarioSpec) -> list[str]:
|
||||
return [screw.name for screw in spec.screws]
|
||||
|
||||
def _component_kinds(self, spec: ScenarioSpec) -> list[str]:
|
||||
return sorted({
|
||||
self.component_kind(object_id, spec)
|
||||
for object_id in self.component_ids(spec)
|
||||
})
|
||||
|
||||
def _destination_kinds(self, spec: ScenarioSpec) -> list[str]:
|
||||
return sorted({
|
||||
self.destination_kind(destination_id, spec)
|
||||
for destination_id in self.destination_ids(spec)
|
||||
})
|
||||
|
||||
def action_rules(self, spec: ScenarioSpec) -> dict[str, dict[str, Any]]:
|
||||
"""Compact machine-readable action rules for the selected scene.
|
||||
|
||||
The lists contain only ids that are present in this experiment, so the
|
||||
external planner cannot advertise an action on a missing object.
|
||||
"""
|
||||
components = list(self.component_ids(spec))
|
||||
tools = list(self.tool_ids(spec))
|
||||
destinations = list(self.destination_ids(spec))
|
||||
screws = self._screw_ids(spec)
|
||||
sockets = self._socket_ids(spec)
|
||||
holes = [name for name in _ALLOWED_HOLE_IDS if name in destinations]
|
||||
sides = list(spec.sealant_sides)
|
||||
cap_present = spec.cap_state != 'absent'
|
||||
object_components = components + tools
|
||||
|
||||
def rule(
|
||||
object_types: list[str],
|
||||
target_types: list[str],
|
||||
objects: list[str],
|
||||
targets: list[str],
|
||||
*,
|
||||
required_tools: list[str] | None = None,
|
||||
required_tool_by_kind: dict[str, str] | None = None,
|
||||
preconditions: list[str] | None = None,
|
||||
result: str = '',
|
||||
targets_by_object_kind: dict[str, list[str]] | None = None,
|
||||
forbidden_targets: list[str] | None = None,
|
||||
) -> dict[str, Any]:
|
||||
entry: dict[str, Any] = {
|
||||
'object_types': object_types,
|
||||
'target_types': target_types,
|
||||
'objects': objects,
|
||||
'targets': targets,
|
||||
'required_tools': required_tools or [],
|
||||
'preconditions': preconditions or [],
|
||||
'result': result,
|
||||
}
|
||||
if required_tool_by_kind:
|
||||
entry['required_tool_by_kind'] = required_tool_by_kind
|
||||
if targets_by_object_kind:
|
||||
entry['targets_by_object_kind'] = targets_by_object_kind
|
||||
if forbidden_targets:
|
||||
entry['forbidden_targets'] = forbidden_targets
|
||||
return entry
|
||||
|
||||
rules: dict[str, dict[str, Any]] = {
|
||||
'detect': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec),
|
||||
object_components,
|
||||
destinations,
|
||||
result='fresh pose of a present object or destination',
|
||||
),
|
||||
'approach': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec) + ['component', 'tool'],
|
||||
object_components,
|
||||
destinations + components + tools,
|
||||
result='tcp above the target at approach height',
|
||||
),
|
||||
'lower': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec) + ['component', 'tool'],
|
||||
object_components,
|
||||
destinations + components + tools,
|
||||
result='tcp at the axial contact pose',
|
||||
),
|
||||
'lift': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec) + ['component', 'tool'],
|
||||
object_components,
|
||||
destinations + components + tools,
|
||||
result='tcp at the saved approach height',
|
||||
),
|
||||
'retreat': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec) + ['component', 'tool'],
|
||||
object_components,
|
||||
destinations + components + tools,
|
||||
result='tcp at work after an axial withdrawal',
|
||||
),
|
||||
'grab': rule(
|
||||
['hatch', 'screw', 'tool'], [],
|
||||
(['cap'] if cap_present else []) + screws + tools,
|
||||
[],
|
||||
required_tool_by_kind={
|
||||
'hatch': 'vacuum',
|
||||
'screw': 'magnetic_tool',
|
||||
},
|
||||
preconditions=[
|
||||
'tool socket is free for a tool',
|
||||
'gripper is empty',
|
||||
],
|
||||
result='tool mounted or part attached to the mounted tool',
|
||||
),
|
||||
'pick': rule(
|
||||
['hatch', 'screw', 'tool'], [],
|
||||
(['cap'] if cap_present else []) + screws + tools,
|
||||
[],
|
||||
required_tool_by_kind={
|
||||
'hatch': 'vacuum',
|
||||
'screw': 'magnetic_tool',
|
||||
},
|
||||
preconditions=[
|
||||
'tool socket is free for a tool',
|
||||
'gripper is empty',
|
||||
],
|
||||
result='alias of grab',
|
||||
),
|
||||
'transfer': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec),
|
||||
object_components,
|
||||
destinations,
|
||||
preconditions=['object is held or tool is mounted'],
|
||||
result='tcp above the destination at transfer height',
|
||||
targets_by_object_kind={
|
||||
'tool': ['work'] + sockets,
|
||||
'hatch': ['work', 'hatch_seat'],
|
||||
'screw': ['work'] + holes,
|
||||
},
|
||||
),
|
||||
'move_to_storage': rule(
|
||||
['hatch', 'screw'],
|
||||
['workspace', 'seat', 'hole'],
|
||||
(['cap'] if cap_present else []) + screws,
|
||||
['work'] + (['hatch_seat'] if cap_present else []) + holes,
|
||||
preconditions=[
|
||||
'a hatch or screw is held',
|
||||
'this alias is not valid for a tool',
|
||||
],
|
||||
result='held part lowered at work, hatch_seat or a hole',
|
||||
forbidden_targets=['hatch_seat'] if 'sealant_tool' in tools else [],
|
||||
),
|
||||
'align': rule(
|
||||
self._component_kinds(spec) + ['tool'],
|
||||
self._destination_kinds(spec),
|
||||
object_components,
|
||||
destinations,
|
||||
preconditions=['a part is held or a tool is mounted'],
|
||||
result='orientation aligned above the target',
|
||||
),
|
||||
'release': rule(
|
||||
['hatch', 'screw', 'tool'],
|
||||
['seat', 'hole', 'tool_socket'],
|
||||
(['cap'] if cap_present else []) + screws + tools,
|
||||
(['hatch_seat'] if cap_present else []) + holes + sockets,
|
||||
preconditions=[
|
||||
'release(tool) requires the matching <tool>_socket',
|
||||
'an explicit transfer to the socket precedes dock_tool',
|
||||
],
|
||||
result=(
|
||||
'screw handed to the virtual operator at a hole; '
|
||||
'no insertion or tightening'
|
||||
),
|
||||
),
|
||||
'capture': rule(
|
||||
['camera'], [], list(CAMERA_IDS), [],
|
||||
result='a fresh frame is available',
|
||||
),
|
||||
'verify': rule(
|
||||
['hatch', 'screw'],
|
||||
['seat', 'hole'],
|
||||
(['cap'] if cap_present else []) + screws,
|
||||
(['hatch_seat'] if cap_present else []) + holes,
|
||||
result='measured object state matches the expected target',
|
||||
),
|
||||
'inspect': rule(
|
||||
['hatch', 'screw'],
|
||||
['seat', 'hole'],
|
||||
(['cap'] if cap_present else []) + screws,
|
||||
(['hatch_seat'] if cap_present else []) + holes,
|
||||
result='alias of verify',
|
||||
),
|
||||
'stop': rule(['service_object'], [], ['robot'], [],
|
||||
result='motion stopped, grasp preserved'),
|
||||
'wait_scene_clear': rule(
|
||||
['service_object'], [], ['workspace'], [],
|
||||
result='continue only after a bounded clear condition',
|
||||
),
|
||||
'inspect_state': rule(
|
||||
self._component_kinds(spec) + ['tool'], [],
|
||||
object_components, [],
|
||||
result='empty, holding, placed or unknown ownership',
|
||||
),
|
||||
'notify_operator': rule(
|
||||
['service_object'], [], ['operator'], [],
|
||||
result='operator notified; no motion',
|
||||
),
|
||||
}
|
||||
if sides and 'sealant_tool' in tools:
|
||||
rules['dispense'] = rule(
|
||||
['tool'],
|
||||
['contour'],
|
||||
['sealant_tool'],
|
||||
sides,
|
||||
required_tools=['sealant_tool'],
|
||||
preconditions=[
|
||||
'sealant_tool is mounted',
|
||||
'target is an existing contour side_i',
|
||||
],
|
||||
result='continuous LIN segment along the named contour side',
|
||||
)
|
||||
if tools:
|
||||
rules['dock_tool'] = rule(
|
||||
['tool'],
|
||||
['tool_socket'],
|
||||
tools,
|
||||
sockets,
|
||||
preconditions=[
|
||||
'the named tool is mounted',
|
||||
'transfer(<tool>, <tool>_socket) precedes dock_tool',
|
||||
"target is that tool's own socket",
|
||||
],
|
||||
result='tool returned to its own socket',
|
||||
)
|
||||
if screws:
|
||||
rules['handoff_screw'] = rule(
|
||||
['screw'],
|
||||
['hole'],
|
||||
screws,
|
||||
holes,
|
||||
required_tools=['magnetic_tool'],
|
||||
preconditions=['the screw is held by the magnetic tool'],
|
||||
result=(
|
||||
'screw handed to the virtual operator at the hole; '
|
||||
'the hole stays empty, no insertion or tightening'
|
||||
),
|
||||
)
|
||||
return {
|
||||
name: rules[name]
|
||||
for name in self.skill_names(spec)
|
||||
if name in rules
|
||||
}
|
||||
|
||||
|
||||
DEFAULT_CATALOG = SkillCatalog()
|
||||
|
||||
|
||||
@@ -13,6 +13,7 @@ from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
|
||||
from std_msgs.msg import String
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from iiwa_experement.geometry import GeometrySpec, load_geometry
|
||||
from iiwa_experement.scenario import ScenarioSpec, load_scenario
|
||||
|
||||
from .catalog import SkillCatalog, DEFAULT_CATALOG
|
||||
@@ -48,9 +49,11 @@ class SceneStore:
|
||||
catalog: SkillCatalog = DEFAULT_CATALOG,
|
||||
*,
|
||||
freshness_sec: float = DEFAULT_FRESHNESS_SEC,
|
||||
geometry: GeometrySpec | None = None,
|
||||
) -> None:
|
||||
self.spec = spec
|
||||
self.catalog = catalog
|
||||
self.geometry = geometry
|
||||
self.freshness_sec = float(freshness_sec)
|
||||
self.scene_ready = False
|
||||
self.ready_message = 'scene not ready'
|
||||
@@ -103,8 +106,13 @@ class SceneStore:
|
||||
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,
|
||||
build_scene(
|
||||
self.spec,
|
||||
self.state,
|
||||
self.catalog,
|
||||
self.geometry,
|
||||
stamp=float(self.received_at),
|
||||
),
|
||||
state_version=self._last_version,
|
||||
stamp=float(self.received_at),
|
||||
experiment_id=self.spec.experiment_id,
|
||||
@@ -125,19 +133,24 @@ class SceneContextNode(Node):
|
||||
self.declare_parameter(
|
||||
'scene_config', str(package_share / 'config' / 'scenes.yaml'),
|
||||
)
|
||||
self.declare_parameter(
|
||||
'geometry_path', str(package_share / 'config' / 'geometry.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))
|
||||
geometry_path = Path(str(self.get_parameter('geometry_path').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),
|
||||
geometry=load_geometry(geometry_path),
|
||||
)
|
||||
self._last_published: tuple[int, int] | None = None
|
||||
self._last_published: tuple[int, ...] | None = None
|
||||
self._last_publish_time = -1.0
|
||||
self._ready_client = self.create_client(Trigger, READY_SERVICE)
|
||||
self._ready_request_pending = False
|
||||
@@ -199,7 +212,7 @@ class SceneContextNode(Node):
|
||||
envelope, _ = self.store.snapshot(self._now())
|
||||
if envelope is None:
|
||||
return
|
||||
key = (int(envelope['scene_revision']), int(envelope['state_version']))
|
||||
key = (int(envelope['state_version']),)
|
||||
now = self._now()
|
||||
period = max(
|
||||
0.05, float(self.get_parameter('publish_period_sec').value),
|
||||
|
||||
@@ -1,13 +1,27 @@
|
||||
"""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.
|
||||
The snapshot is a live view of the selected experiment: only objects that the
|
||||
Webots supervisor really created are listed, every pose carries its frame,
|
||||
source and validity stamp, and derived goals use the same geometry module as
|
||||
the graph executor.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import Any, Mapping
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from iiwa_experement.geometry import (
|
||||
GeometrySpec,
|
||||
Transform,
|
||||
load_geometry,
|
||||
object_pose_transform,
|
||||
sealant_point,
|
||||
tcp_goal,
|
||||
world_target,
|
||||
)
|
||||
from iiwa_experement.scenario import ScenarioSpec
|
||||
|
||||
from .catalog import (
|
||||
@@ -41,6 +55,41 @@ SERVICE_OBJECTS = {
|
||||
'operator': 'виртуальный оператор',
|
||||
}
|
||||
|
||||
POSE_CONVENTIONS = {
|
||||
'frame_id': 'world',
|
||||
'length_unit': 'm',
|
||||
'orientation': 'quaternion_xyzw',
|
||||
}
|
||||
|
||||
_OBJECT_DEFS = {
|
||||
'fixture': 'grant_fixture',
|
||||
'body': 'grant_body',
|
||||
'tray': 'grant_tray',
|
||||
'cap': 'grant_cap',
|
||||
}
|
||||
|
||||
|
||||
def _default_geometry() -> GeometrySpec:
|
||||
"""Locate geometry.yaml in an installed or source checkout."""
|
||||
candidates: list[Path] = []
|
||||
try:
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
candidates.append(
|
||||
Path(get_package_share_directory('iiwa_experement'))
|
||||
/ 'config' / 'geometry.yaml'
|
||||
)
|
||||
except Exception:
|
||||
pass
|
||||
candidates.append(
|
||||
Path(__file__).resolve().parents[2]
|
||||
/ 'iiwa_experement' / 'config' / 'geometry.yaml'
|
||||
)
|
||||
for path in candidates:
|
||||
if path.is_file():
|
||||
return load_geometry(path)
|
||||
raise FileNotFoundError('geometry.yaml is unavailable for the scene snapshot')
|
||||
|
||||
|
||||
def _state_mapping(supervisor_state: Mapping[str, Any] | None) -> dict[str, Any]:
|
||||
if supervisor_state is None:
|
||||
@@ -50,6 +99,69 @@ def _state_mapping(supervisor_state: Mapping[str, Any] | None) -> dict[str, Any]
|
||||
return dict(supervisor_state)
|
||||
|
||||
|
||||
def _transform(entry: Any, child: str) -> Transform | None:
|
||||
if not isinstance(entry, Mapping):
|
||||
return None
|
||||
xyz = entry.get('xyz')
|
||||
rotation = entry.get('rotation')
|
||||
if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
|
||||
return None
|
||||
if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
|
||||
return None
|
||||
if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation):
|
||||
return None
|
||||
try:
|
||||
values = np.asarray(rotation, dtype=float)
|
||||
translation = np.asarray(xyz, dtype=float)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
if not np.all(np.isfinite(values)) or not np.all(np.isfinite(translation)):
|
||||
return None
|
||||
return Transform(
|
||||
xyz=tuple(float(value) for value in translation),
|
||||
rotation=tuple(tuple(float(value) for value in row) for row in values),
|
||||
parent='world',
|
||||
child=child,
|
||||
)
|
||||
|
||||
|
||||
def _pose_json(
|
||||
transform: Transform | None,
|
||||
*,
|
||||
source: str,
|
||||
stamp: float,
|
||||
) -> dict[str, Any] | None:
|
||||
if transform is None:
|
||||
return None
|
||||
position = np.asarray(transform.xyz, dtype=float)
|
||||
matrix = np.asarray(transform.rotation, dtype=float)
|
||||
if position.shape != (3,) or matrix.shape != (3, 3):
|
||||
raise ValueError('pose must contain a 3-vector and a 3x3 rotation')
|
||||
if not np.all(np.isfinite(position)) or not np.all(np.isfinite(matrix)):
|
||||
raise ValueError('pose contains a non-finite number')
|
||||
quaternion = Rotation.from_matrix(matrix).as_quat()
|
||||
norm = float(np.linalg.norm(quaternion))
|
||||
if not np.isfinite(norm) or norm < 1e-9:
|
||||
raise ValueError('pose quaternion is not normalizable')
|
||||
quaternion = quaternion / norm
|
||||
return {
|
||||
'frame_id': 'world',
|
||||
'position': {
|
||||
'x': float(position[0]),
|
||||
'y': float(position[1]),
|
||||
'z': float(position[2]),
|
||||
},
|
||||
'orientation': {
|
||||
'x': float(quaternion[0]),
|
||||
'y': float(quaternion[1]),
|
||||
'z': float(quaternion[2]),
|
||||
'w': float(quaternion[3]),
|
||||
},
|
||||
'source': source,
|
||||
'stamp': float(stamp),
|
||||
}
|
||||
|
||||
|
||||
def _flags(state: Mapping[str, Any]) -> list[str]:
|
||||
flags: list[str] = []
|
||||
mounted = state.get('mounted_tool')
|
||||
@@ -116,29 +228,183 @@ def _hole_state(
|
||||
}
|
||||
|
||||
|
||||
def _object_pose(
|
||||
state: Mapping[str, Any],
|
||||
definition: str,
|
||||
fallback: Transform,
|
||||
*,
|
||||
stamp: float,
|
||||
) -> tuple[Transform, str]:
|
||||
live = _transform((state.get('objects') or {}).get(definition), definition)
|
||||
if live is not None:
|
||||
return live, 'webots_supervisor'
|
||||
return fallback, 'scenario_manifest'
|
||||
|
||||
|
||||
def _component_pose(
|
||||
spec: ScenarioSpec,
|
||||
geometry: GeometrySpec,
|
||||
state: Mapping[str, Any],
|
||||
object_id: str,
|
||||
kind: str,
|
||||
stamp: float,
|
||||
) -> tuple[Transform | None, str]:
|
||||
if kind == 'fixture':
|
||||
fallback = object_pose_transform(
|
||||
spec.fixture.x, spec.fixture.y, spec.fixture.z,
|
||||
spec.fixture.yaw_deg, child='fixture',
|
||||
)
|
||||
return _object_pose(state, 'grant_fixture', fallback, stamp=stamp)
|
||||
if kind == 'body':
|
||||
fallback = object_pose_transform(
|
||||
spec.body.x, spec.body.y, spec.body.z,
|
||||
spec.body.yaw_deg, child='body',
|
||||
)
|
||||
return _object_pose(state, 'grant_body', fallback, stamp=stamp)
|
||||
if kind == 'tray':
|
||||
assert spec.tray_pose is not None
|
||||
fallback = object_pose_transform(
|
||||
spec.tray_pose.x, spec.tray_pose.y, spec.tray_pose.z,
|
||||
spec.tray_pose.yaw_deg, child='tray',
|
||||
)
|
||||
return _object_pose(state, 'grant_tray', fallback, stamp=stamp)
|
||||
if kind == 'hatch':
|
||||
live = _transform((state.get('cap') or {}).get('pose'), 'cap')
|
||||
if live is not None:
|
||||
return live, 'webots_supervisor'
|
||||
return world_target(spec, geometry, 'cap'), 'scenario_manifest'
|
||||
if kind == 'screw':
|
||||
live = _transform(
|
||||
(state.get('screws') or {}).get(object_id, {}).get('pose'),
|
||||
object_id,
|
||||
)
|
||||
if live is not None:
|
||||
return live, 'webots_supervisor'
|
||||
return world_target(spec, geometry, f'screw:{object_id}'), 'scenario_manifest'
|
||||
return None, 'scenario_manifest'
|
||||
|
||||
|
||||
def _tool_pose(
|
||||
spec: ScenarioSpec,
|
||||
geometry: GeometrySpec,
|
||||
state: Mapping[str, Any],
|
||||
tool_name: str,
|
||||
stamp: float,
|
||||
) -> tuple[Transform | None, str]:
|
||||
live = _transform(
|
||||
(state.get('tools') or {}).get(tool_name, {}).get('pose'),
|
||||
tool_name,
|
||||
)
|
||||
if live is not None:
|
||||
return live, 'webots_supervisor'
|
||||
fallback = world_target(spec, geometry, f'tool:{tool_name}')
|
||||
return _object_pose(state, f'grant_{tool_name}', fallback, stamp=stamp)
|
||||
|
||||
|
||||
def _destination_poses(
|
||||
spec: ScenarioSpec,
|
||||
geometry: GeometrySpec,
|
||||
destination_id: str,
|
||||
kind: str,
|
||||
stamp: float,
|
||||
) -> dict[str, Any]:
|
||||
source = 'executor_geometry'
|
||||
if kind == 'workspace':
|
||||
work = geometry.safe_poses['work']
|
||||
pose = _pose_json(work, source=source, stamp=stamp)
|
||||
return {'pose': pose, 'tcp_pose': pose}
|
||||
if kind == 'seat':
|
||||
target = world_target(spec, geometry, 'cap_seat')
|
||||
return {
|
||||
'pose': _pose_json(target, source=source, stamp=stamp),
|
||||
'tcp_pose': _pose_json(
|
||||
tcp_goal(target, geometry.cap_tcp_to_node),
|
||||
source=source, stamp=stamp,
|
||||
),
|
||||
}
|
||||
if kind == 'hole':
|
||||
target = world_target(spec, geometry, f'hole:{destination_id}')
|
||||
return {
|
||||
'pose': _pose_json(target, source=source, stamp=stamp),
|
||||
'tcp_pose': _pose_json(
|
||||
tcp_goal(target, geometry.screw_tcp_to_node),
|
||||
source=source, stamp=stamp,
|
||||
),
|
||||
}
|
||||
if kind == 'tool_socket':
|
||||
tool = destination_id[: -len('_socket')]
|
||||
target = world_target(spec, geometry, f'tool:{tool}')
|
||||
return {
|
||||
'pose': _pose_json(target, source=source, stamp=stamp),
|
||||
'tcp_pose': _pose_json(
|
||||
tcp_goal(target, geometry.tool_tcp_to_nodes[tool]),
|
||||
source=source, stamp=stamp,
|
||||
),
|
||||
}
|
||||
if kind == 'contour':
|
||||
start = sealant_point(geometry, spec, destination_id, 0.0)
|
||||
end = sealant_point(geometry, spec, destination_id, 1.0)
|
||||
tip = geometry.tool_work_tips['sealant_tool']
|
||||
return {
|
||||
'start': _pose_json(start, source=source, stamp=stamp),
|
||||
'end': _pose_json(end, source=source, stamp=stamp),
|
||||
'start_tcp': _pose_json(
|
||||
tcp_goal(start, tip), source=source, stamp=stamp,
|
||||
),
|
||||
'end_tcp': _pose_json(
|
||||
tcp_goal(end, tip), source=source, stamp=stamp,
|
||||
),
|
||||
}
|
||||
return {}
|
||||
|
||||
|
||||
def build_scene(
|
||||
spec: ScenarioSpec,
|
||||
supervisor_state: Mapping[str, Any],
|
||||
catalog: SkillCatalog,
|
||||
geometry: GeometrySpec | None = None,
|
||||
*,
|
||||
stamp: float | None = None,
|
||||
) -> dict[str, Any]:
|
||||
"""Build the coordinate-free ``scene_json`` object for one snapshot."""
|
||||
"""Build the live ``scene_json`` object for one snapshot."""
|
||||
state = _state_mapping(supervisor_state)
|
||||
catalog.validate_ids(spec)
|
||||
if geometry is None:
|
||||
geometry = _default_geometry()
|
||||
if stamp is None:
|
||||
try:
|
||||
stamp = float(state.get('simulation_time', 0.0))
|
||||
except (TypeError, ValueError):
|
||||
stamp = 0.0
|
||||
stamp = float(stamp)
|
||||
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
|
||||
]
|
||||
components = []
|
||||
for object_id in catalog.component_ids(spec):
|
||||
kind = catalog.component_kind(object_id, spec)
|
||||
entry = build_component_entry(catalog, spec, object_id, state)
|
||||
pose, source = _component_pose(
|
||||
spec, geometry, state, object_id, kind, stamp,
|
||||
)
|
||||
entry['pose'] = _pose_json(pose, source=source, stamp=stamp)
|
||||
components.append(entry)
|
||||
|
||||
destinations = []
|
||||
for destination_id in catalog.destination_ids(spec):
|
||||
kind = catalog.destination_kind(destination_id, spec)
|
||||
entry = destination_entry(catalog, spec, destination_id, state)
|
||||
entry.update(_destination_poses(
|
||||
spec, geometry, destination_id, kind, stamp,
|
||||
))
|
||||
destinations.append(entry)
|
||||
|
||||
tools = []
|
||||
for tool in spec.tools:
|
||||
entry = tool_entry(catalog, spec, tool, state)
|
||||
pose, source = _tool_pose(spec, geometry, state, tool.name, stamp)
|
||||
entry['pose'] = _pose_json(pose, source=source, stamp=stamp)
|
||||
tools.append(entry)
|
||||
|
||||
initial_state = {
|
||||
'tool_mounted': mounted if isinstance(mounted, str) else None,
|
||||
@@ -148,6 +414,7 @@ def build_scene(
|
||||
'tools': _tool_state(catalog, spec, state),
|
||||
'holes': _hole_state(spec, state),
|
||||
'scene_ready': bool(state.get('ready', True)),
|
||||
'stamp': stamp,
|
||||
}
|
||||
|
||||
return {
|
||||
@@ -157,13 +424,15 @@ def build_scene(
|
||||
'profile': catalog.profile,
|
||||
'experiment_id': int(spec.experiment_id),
|
||||
'domain': 'grant_assembly',
|
||||
'pose_conventions': dict(POSE_CONVENTIONS),
|
||||
'components': components,
|
||||
'destinations': destinations,
|
||||
'tools': tools,
|
||||
'initial_state': initial_state,
|
||||
'cameras': DEFAULT_CAMERAS,
|
||||
'available_skills': list(catalog.skill_names()),
|
||||
'skill_contracts': catalog.skill_contracts(),
|
||||
'available_skills': list(catalog.skill_names(spec)),
|
||||
'skill_contracts': catalog.skill_contracts(spec),
|
||||
'action_rules': catalog.action_rules(spec),
|
||||
'service_objects': dict(SERVICE_OBJECTS),
|
||||
'recovery_attempts_per_component': 1,
|
||||
}
|
||||
@@ -172,14 +441,11 @@ def build_scene(
|
||||
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 = {
|
||||
@@ -192,7 +458,6 @@ def build_envelope(
|
||||
else int(scene.get('experiment_id', 0))
|
||||
),
|
||||
'state_version': int(state_version),
|
||||
'scene_revision': int(scene_revision),
|
||||
'timestamp': float(stamp),
|
||||
'scene_json': dict(scene),
|
||||
}
|
||||
|
||||
@@ -11,6 +11,8 @@
|
||||
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>iiwa_experement</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-scipy</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
@@ -12,6 +12,7 @@ setup(
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/doc', glob('docs/*.md')),
|
||||
('share/' + package_name + '/doc', glob('docs/*.json')),
|
||||
],
|
||||
install_requires=['setuptools', 'PyYAML'],
|
||||
tests_require=['pytest'],
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
"""Readiness, freshness and scene_revision rules of the snapshot node."""
|
||||
"""Readiness, freshness and internal scene version rules of the snapshot node."""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from iiwa_experement.geometry import load_geometry
|
||||
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
|
||||
@@ -12,6 +13,7 @@ from iiwa_scene_context.snapshot import build_scene
|
||||
|
||||
PACKAGE = Path(__file__).resolve().parents[1]
|
||||
SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
|
||||
GEOMETRY = load_geometry(PACKAGE.parent / 'iiwa_experement' / 'config' / 'geometry.yaml')
|
||||
|
||||
|
||||
def state(version: int, *, mounted=None, held=None):
|
||||
@@ -29,7 +31,7 @@ def state(version: int, *, mounted=None, held=None):
|
||||
|
||||
def make_store():
|
||||
spec = load_scenario(1, SCENES)
|
||||
return SceneStore(spec, freshness_sec=2.0)
|
||||
return SceneStore(spec, freshness_sec=2.0, geometry=GEOMETRY)
|
||||
|
||||
|
||||
def test_snapshot_requires_scene_ready_and_fresh_state():
|
||||
@@ -49,22 +51,23 @@ def test_snapshot_requires_scene_ready_and_fresh_state():
|
||||
assert 'stale' in reason
|
||||
|
||||
|
||||
def test_revision_advances_on_state_version_but_not_on_pose_updates():
|
||||
def test_internal_version_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
|
||||
assert store.scene_revision == 1
|
||||
assert 'scene_revision' not in store.snapshot(10.1)[0]
|
||||
|
||||
# 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
|
||||
assert store.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
|
||||
assert store.scene_revision == 2
|
||||
|
||||
store.update(state(3, mounted='vacuum', held=None), received_at=11.5)
|
||||
assert store.snapshot(11.6)[0]['scene_revision'] == 3
|
||||
assert store.scene_revision == 3
|
||||
|
||||
|
||||
def test_late_subscriber_qos_is_transient_local_and_depth_one():
|
||||
@@ -91,7 +94,8 @@ def test_scene_service_returns_json_or_a_reason():
|
||||
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
|
||||
assert '"scene_revision"' not in result.message
|
||||
assert '"pose_conventions"' in result.message
|
||||
|
||||
|
||||
def test_snapshot_scene_is_the_catalog_scene_not_the_state():
|
||||
@@ -99,9 +103,12 @@ def test_snapshot_scene_is_the_catalog_scene_not_the_state():
|
||||
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)
|
||||
expected = build_scene(
|
||||
store.spec, store.state, store.catalog, store.geometry,
|
||||
stamp=store.received_at,
|
||||
)
|
||||
assert envelope['scene_json'] == expected
|
||||
assert 'pose' not in envelope['scene_json']
|
||||
assert 'objects' not in envelope['scene_json']
|
||||
|
||||
|
||||
def test_scene_topic_is_republished_to_keep_the_timestamp_fresh():
|
||||
|
||||
@@ -1,10 +1,13 @@
|
||||
"""Scene snapshots expose only present ids and logical state."""
|
||||
"""Scene snapshots expose only present ids, live poses and action rules."""
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from iiwa_experement.geometry import load_geometry
|
||||
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
|
||||
@@ -13,9 +16,10 @@ 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'
|
||||
GEOMETRY = load_geometry(ROOT / 'iiwa_experement' / 'config' / 'geometry.yaml')
|
||||
FORBIDDEN_KEYS = {
|
||||
'pose', 'poses', 'xyz', 'rotation', 'tcp', 'objects', 'simulation_time',
|
||||
'last_command_result', 'last_error', 'attached', 'version',
|
||||
'simulation_time', 'last_command_result', 'last_error',
|
||||
'attached', 'scene_revision',
|
||||
}
|
||||
|
||||
|
||||
@@ -51,6 +55,7 @@ def state_for(spec, *, version=3, mounted=None, held=None, cap=None,
|
||||
'tools': tools,
|
||||
'screws': screw_entries,
|
||||
'holes': hole_entries,
|
||||
'simulation_time': 12.5,
|
||||
}
|
||||
|
||||
|
||||
@@ -68,10 +73,16 @@ def walk_keys(value):
|
||||
yield from walk_keys(item)
|
||||
|
||||
|
||||
def quaternion_norm(orientation):
|
||||
return math.sqrt(sum(
|
||||
float(orientation[axis]) ** 2 for axis in ('x', 'y', 'z', 'w')
|
||||
))
|
||||
|
||||
|
||||
@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)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
components = ids(scene['components'])
|
||||
destinations = ids(scene['destinations'])
|
||||
assert 'fixture' in components and 'body' in components
|
||||
@@ -97,11 +108,46 @@ def test_scene_lists_only_present_objects_and_goals(experiment_id):
|
||||
assert len([name for name in components if name.startswith('screw_')]) >= 10
|
||||
|
||||
|
||||
@pytest.mark.parametrize('experiment_id', range(1, 21))
|
||||
def test_every_experiment_scene_matches_the_created_scene(experiment_id):
|
||||
spec = spec_for(experiment_id)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
expected_components = {'fixture', 'body'}
|
||||
if spec.cap_state != 'absent':
|
||||
expected_components.add('cap')
|
||||
if spec.tray_pose is not None:
|
||||
expected_components.add('tray')
|
||||
expected_components.update(
|
||||
f'screw_{index}' for index in range(1, 11)
|
||||
)
|
||||
assert set(ids(scene['components'])) == expected_components
|
||||
assert set(ids(scene['tools'])) == set(spec.tool_names)
|
||||
assert set(ids(scene['destinations'])) == {
|
||||
'work', 'hatch_seat', 'hole_1', 'hole_2', 'hole_3', 'hole_4',
|
||||
*[f'{tool.name}_socket' for tool in spec.tools],
|
||||
*spec.sealant_sides,
|
||||
}
|
||||
assert set(scene['available_skills']) == set(scene['action_rules'])
|
||||
for rule in scene['action_rules'].values():
|
||||
assert set(rule['objects']) <= set(scene['service_objects']) | set(
|
||||
ids(scene['components'])
|
||||
) | set(ids(scene['tools'])) | set(scene['cameras'])
|
||||
assert set(rule['targets']) <= set(ids(scene['destinations'])) | set(
|
||||
ids(scene['components'])
|
||||
) | set(ids(scene['tools']))
|
||||
if 'dispense' in scene['available_skills']:
|
||||
assert scene['action_rules']['dispense']['targets'] == list(
|
||||
spec.sealant_sides
|
||||
)
|
||||
else:
|
||||
assert not spec.sealant_sides
|
||||
|
||||
|
||||
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)
|
||||
first = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
second = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
assert first['components'] == second['components']
|
||||
assert first['destinations'] == second['destinations']
|
||||
tool_ids = [tool['id'] for tool in first['tools']]
|
||||
@@ -111,19 +157,70 @@ def test_each_present_tool_has_a_named_socket_and_stable_ids():
|
||||
assert tool['socket'] in ids(first['destinations'])
|
||||
|
||||
|
||||
def test_scene_json_has_no_coordinates_or_supervisor_internals():
|
||||
def test_scene_json_has_live_poses_and_no_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)
|
||||
scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY)
|
||||
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
|
||||
assert scene['contract_version'] == 2
|
||||
assert scene['catalog_version'] == 2
|
||||
assert scene['schema_version'] == 2
|
||||
assert scene['pose_conventions'] == {
|
||||
'frame_id': 'world',
|
||||
'length_unit': 'm',
|
||||
'orientation': 'quaternion_xyzw',
|
||||
}
|
||||
|
||||
for entry in scene['components'] + scene['tools']:
|
||||
pose = entry['pose']
|
||||
assert pose['frame_id'] == 'world'
|
||||
assert pose['stamp'] == 12.5
|
||||
assert pose['source'] in ('webots_supervisor', 'scenario_manifest')
|
||||
assert abs(quaternion_norm(pose['orientation']) - 1.0) < 1e-9
|
||||
assert all(math.isfinite(value) for value in pose['position'].values())
|
||||
|
||||
for entry in scene['destinations']:
|
||||
if entry['kind'] == 'contour':
|
||||
assert entry['start']['frame_id'] == 'world'
|
||||
assert entry['end']['frame_id'] == 'world'
|
||||
assert entry['start_tcp']['frame_id'] == 'world'
|
||||
assert entry['end_tcp']['frame_id'] == 'world'
|
||||
else:
|
||||
assert entry['pose']['frame_id'] == 'world'
|
||||
assert entry['tcp_pose']['frame_id'] == 'world'
|
||||
|
||||
|
||||
def test_live_supervisor_pose_overrides_the_manifest_pose():
|
||||
spec = spec_for(20)
|
||||
state = state_for(spec)
|
||||
state['objects'] = {
|
||||
'grant_body': {
|
||||
'xyz': [0.31, 0.13, 0.82],
|
||||
'rotation': [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
|
||||
},
|
||||
}
|
||||
scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY)
|
||||
body = next(entry for entry in scene['components'] if entry['id'] == 'body')
|
||||
assert body['pose']['position'] == {'x': 0.31, 'y': 0.13, 'z': 0.82}
|
||||
assert body['pose']['source'] == 'webots_supervisor'
|
||||
|
||||
|
||||
def test_held_object_pose_is_updated_and_marked_live():
|
||||
spec = spec_for(20)
|
||||
state = state_for(spec, mounted='vacuum', held='cap', cap={
|
||||
'state': 'held',
|
||||
'pose': {
|
||||
'xyz': [0.4, -0.2, 0.9],
|
||||
'rotation': [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
|
||||
},
|
||||
})
|
||||
scene = build_scene(spec, state, DEFAULT_CATALOG, GEOMETRY)
|
||||
cap = next(entry for entry in scene['components'] if entry['id'] == 'cap')
|
||||
assert cap['pose']['position'] == {'x': 0.4, 'y': -0.2, 'z': 0.9}
|
||||
assert cap['pose']['source'] == 'webots_supervisor'
|
||||
|
||||
|
||||
def test_logical_state_reflects_the_live_supervisor_snapshot():
|
||||
@@ -139,6 +236,7 @@ def test_logical_state_reflects_the_live_supervisor_snapshot():
|
||||
holes={'hole_1': {'state': 'empty', 'screw': 'screw_1'}},
|
||||
),
|
||||
DEFAULT_CATALOG,
|
||||
GEOMETRY,
|
||||
)
|
||||
initial = scene['initial_state']
|
||||
assert initial['tool_mounted'] == 'vacuum'
|
||||
@@ -151,25 +249,83 @@ def test_logical_state_reflects_the_live_supervisor_snapshot():
|
||||
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)
|
||||
def test_scene_json_carries_server_skill_contracts_and_scene_rules():
|
||||
spec = spec_for(20)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
assert 'grab' in scene['available_skills']
|
||||
assert 'tighten' not in scene['available_skills']
|
||||
assert set(scene['skill_contracts']) == set(scene['available_skills'])
|
||||
assert set(scene['action_rules']) == set(scene['available_skills'])
|
||||
assert scene['skill_contracts']['grab']['outcomes']['grasp_failed']
|
||||
assert 'grasp_failed' in scene['skill_contracts']['pick']['outcomes']
|
||||
|
||||
grab = scene['action_rules']['grab']
|
||||
assert grab['required_tool_by_kind']['hatch'] == 'vacuum'
|
||||
assert grab['required_tool_by_kind']['screw'] == 'magnetic_tool'
|
||||
assert set(grab['objects']) == {
|
||||
'cap',
|
||||
*[f'screw_{index}' for index in range(1, 11)],
|
||||
'vacuum', 'magnetic_tool', 'sealant_tool', 'torque_tool',
|
||||
}
|
||||
|
||||
dispense = scene['action_rules']['dispense']
|
||||
assert dispense['required_tools'] == ['sealant_tool']
|
||||
assert dispense['targets'] == ['side_1', 'side_2', 'side_3', 'side_4']
|
||||
|
||||
dock = scene['action_rules']['dock_tool']
|
||||
assert 'transfer(<tool>, <tool>_socket) precedes dock_tool' in (
|
||||
dock['preconditions']
|
||||
)
|
||||
assert 'handoff_screw' in scene['action_rules']
|
||||
assert 'no insertion or tightening' in (
|
||||
scene['action_rules']['handoff_screw']['result']
|
||||
)
|
||||
assert 'sealant_tool' not in scene['action_rules']['move_to_storage']['objects']
|
||||
assert 'hatch_seat' in scene['action_rules']['move_to_storage']['forbidden_targets']
|
||||
|
||||
|
||||
def test_smaller_scene_does_not_advertise_unavailable_actions():
|
||||
spec = spec_for(1)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
assert 'dispense' not in scene['available_skills']
|
||||
assert 'handoff_screw' not in scene['available_skills']
|
||||
assert 'dispense' not in scene['action_rules']
|
||||
assert 'side_1' not in ids(scene['destinations'])
|
||||
assert not any(
|
||||
entry['id'].startswith('screw_') for entry in scene['components']
|
||||
)
|
||||
|
||||
|
||||
def test_experiment_20_example_is_the_generated_live_snapshot():
|
||||
example_path = PACKAGE / 'docs' / 'scene_json_experiment_20.json'
|
||||
example = json.loads(example_path.read_text(encoding='utf-8'))
|
||||
spec = spec_for(20)
|
||||
generated = build_scene(
|
||||
spec,
|
||||
state_for(spec),
|
||||
DEFAULT_CATALOG,
|
||||
GEOMETRY,
|
||||
stamp=42.0,
|
||||
)
|
||||
assert example == generated
|
||||
assert example['experiment_id'] == 20
|
||||
assert len(example['components']) == 14
|
||||
assert len(example['tools']) == 4
|
||||
assert len(example['destinations']) == 14
|
||||
assert [entry['id'] for entry in example['destinations']].count(
|
||||
'side_1'
|
||||
) == 1
|
||||
|
||||
|
||||
def test_envelope_keeps_transport_separate_from_scene():
|
||||
spec = spec_for(20)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG)
|
||||
scene = build_scene(spec, state_for(spec), DEFAULT_CATALOG, GEOMETRY)
|
||||
envelope = build_envelope(
|
||||
scene, scene_revision=7, state_version=4, stamp=123.5,
|
||||
scene, state_version=4, stamp=123.5,
|
||||
)
|
||||
assert envelope['experiment_id'] == 20
|
||||
assert envelope['state_version'] == 4
|
||||
assert envelope['scene_revision'] == 7
|
||||
assert 'scene_revision' not in envelope
|
||||
assert envelope['timestamp'] == 123.5
|
||||
assert envelope['scene_json'] == scene
|
||||
assert envelope['scene_json'] is not envelope
|
||||
|
||||
@@ -13,13 +13,14 @@ from fastapi import APIRouter, HTTPException
|
||||
from pydantic import BaseModel, Field
|
||||
from std_srvs.srv import Trigger
|
||||
import webots_ros2_msgs.srv
|
||||
from iiwa_msgs.srv import GetRunStatus
|
||||
from iiwa_msgs.srv import GetRunStatus, SubmitPlan
|
||||
|
||||
from .ros_node import get_bridge
|
||||
|
||||
|
||||
SCENE_SERVICE = '/iiwa_scene_context/get_scene'
|
||||
SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit'
|
||||
SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit_plan'
|
||||
LEGACY_SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit'
|
||||
CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel'
|
||||
STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status'
|
||||
|
||||
@@ -30,7 +31,6 @@ _active_run_id: str | None = None
|
||||
|
||||
class RunRequest(BaseModel):
|
||||
run_id: str = Field(..., min_length=1, max_length=128)
|
||||
scene_revision: int = Field(..., ge=0)
|
||||
response: dict[str, Any] | str = Field(
|
||||
..., description='The untouched Qwen3 graph response',
|
||||
)
|
||||
@@ -54,15 +54,14 @@ def _status(run_id: str) -> dict[str, Any] | None:
|
||||
return payload if isinstance(payload, dict) else None
|
||||
|
||||
|
||||
def _rejection(run_id: str) -> tuple[int, str, dict[str, Any] | None]:
|
||||
status = _status(run_id)
|
||||
reason = '' if status is None else str(status.get('reason', ''))
|
||||
lowered = reason.lower()
|
||||
if 'busy' in lowered:
|
||||
return 409, reason or 'another graph is already active', status
|
||||
if 'not_ready' in lowered:
|
||||
return 503, reason or 'the executor is not ready yet', status
|
||||
return 422, reason or 'graph was rejected', status
|
||||
def _http_status(code: str) -> int:
|
||||
if code in ('busy', 'conflict', 'stale_scene'):
|
||||
return 409
|
||||
if code == 'not_ready':
|
||||
return 503
|
||||
if code == 'invalid':
|
||||
return 422
|
||||
return 422
|
||||
|
||||
|
||||
@router.get('/scene', summary='Текущий scene_json для внешнего планировщика')
|
||||
@@ -87,30 +86,37 @@ def get_scene():
|
||||
@router.post('/runs', summary='Передать готовый граф Qwen3 исполнителю')
|
||||
def submit_run(request: RunRequest):
|
||||
global _active_run_id
|
||||
payload = {
|
||||
'run_id': request.run_id,
|
||||
'scene_revision': int(request.scene_revision),
|
||||
'response': request.response,
|
||||
}
|
||||
graph = (
|
||||
request.response
|
||||
if isinstance(request.response, str)
|
||||
else json.dumps(request.response, ensure_ascii=False)
|
||||
)
|
||||
try:
|
||||
response = get_bridge().call_service(
|
||||
webots_ros2_msgs.srv.SetString,
|
||||
SubmitPlan,
|
||||
SUBMIT_SERVICE,
|
||||
webots_ros2_msgs.srv.SetString.Request(
|
||||
value=json.dumps(payload, ensure_ascii=False),
|
||||
SubmitPlan.Request(
|
||||
run_id=request.run_id,
|
||||
response=graph,
|
||||
),
|
||||
)
|
||||
except (RuntimeError, TimeoutError) as error:
|
||||
raise HTTPException(503, str(error)) from error
|
||||
if not response.success:
|
||||
code, reason, status = _rejection(request.run_id)
|
||||
raise HTTPException(code, detail=reason)
|
||||
if not response.accepted:
|
||||
raise HTTPException(
|
||||
_http_status(response.code),
|
||||
detail={
|
||||
'code': response.code,
|
||||
'reason': response.reason,
|
||||
'run_id': request.run_id,
|
||||
},
|
||||
)
|
||||
with _lock:
|
||||
_active_run_id = request.run_id
|
||||
return {
|
||||
'status': 'accepted',
|
||||
'status': 'duplicate' if response.code == 'duplicate' else 'accepted',
|
||||
'code': response.code,
|
||||
'run_id': request.run_id,
|
||||
'scene_revision': request.scene_revision,
|
||||
}
|
||||
|
||||
|
||||
@@ -122,7 +128,7 @@ def get_run(run_id: str):
|
||||
raise HTTPException(404, f'run {run_id!r} was not submitted')
|
||||
if status.get('state') in (
|
||||
'completed', 'replan_required', 'operator_required', 'failed',
|
||||
'canceled', 'rejected',
|
||||
'canceled',
|
||||
):
|
||||
with _lock:
|
||||
if _active_run_id == run_id:
|
||||
|
||||
@@ -14,7 +14,6 @@ from iiwa_web import main, planner
|
||||
def envelope():
|
||||
return {
|
||||
'experiment_id': 20,
|
||||
'scene_revision': 4,
|
||||
'state_version': 2,
|
||||
'timestamp': 10.0,
|
||||
'scene_json': {'experiment_id': 20, 'components': []},
|
||||
@@ -24,7 +23,9 @@ def envelope():
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.calls = []
|
||||
self.submit_success = True
|
||||
self.submit_accepted = True
|
||||
self.submit_code = 'accepted'
|
||||
self.submit_reason = 'graph admitted'
|
||||
self.cancel_success = True
|
||||
self.scene_success = True
|
||||
self.status_payload = {
|
||||
@@ -45,7 +46,12 @@ class FakeBridge:
|
||||
message=json.dumps(envelope()) if self.scene_success else 'scene not ready',
|
||||
)
|
||||
if srv_name == planner.SUBMIT_SERVICE:
|
||||
return SimpleNamespace(success=self.submit_success)
|
||||
return SimpleNamespace(
|
||||
accepted=self.submit_accepted,
|
||||
code=self.submit_code,
|
||||
reason=self.submit_reason,
|
||||
status_json='{}',
|
||||
)
|
||||
if srv_name == planner.CANCEL_SERVICE:
|
||||
return SimpleNamespace(success=self.cancel_success)
|
||||
if srv_name == planner.STATUS_SERVICE:
|
||||
@@ -68,11 +74,12 @@ def bridge(monkeypatch):
|
||||
return fake, TestClient(app)
|
||||
|
||||
|
||||
def test_scene_returns_the_envelope(bridge):
|
||||
def test_scene_returns_the_envelope_without_scene_revision(bridge):
|
||||
fake, client = bridge
|
||||
response = client.get('/planner/scene')
|
||||
assert response.status_code == 200
|
||||
assert response.json()['scene_revision'] == 4
|
||||
assert response.json()['state_version'] == 2
|
||||
assert 'scene_revision' not in response.json()
|
||||
assert fake.calls[0][0] == planner.SCENE_SERVICE
|
||||
|
||||
|
||||
@@ -84,11 +91,10 @@ def test_scene_not_ready_and_service_failure(bridge):
|
||||
assert client.get('/planner/scene').status_code == 503
|
||||
|
||||
|
||||
def test_submit_accepts_without_waiting_for_completion(bridge):
|
||||
def test_submit_accepts_without_scene_revision_or_waiting_for_completion(bridge):
|
||||
fake, client = bridge
|
||||
response = client.post('/planner/runs', json={
|
||||
'run_id': 'r1',
|
||||
'scene_revision': 4,
|
||||
'response': {
|
||||
'status': 'ready',
|
||||
'entry': 'a',
|
||||
@@ -97,39 +103,55 @@ def test_submit_accepts_without_waiting_for_completion(bridge):
|
||||
})
|
||||
assert response.status_code == 200
|
||||
assert response.json()['status'] == 'accepted'
|
||||
assert 'scene_revision' not in response.json()
|
||||
service, request = fake.calls[0]
|
||||
assert service == planner.SUBMIT_SERVICE
|
||||
payload = json.loads(request.value)
|
||||
assert payload['run_id'] == 'r1'
|
||||
assert payload['scene_revision'] == 4
|
||||
assert payload['response']['entry'] == 'a'
|
||||
assert request.run_id == 'r1'
|
||||
assert json.loads(request.response)['entry'] == 'a'
|
||||
assert planner._active_run_id == 'r1'
|
||||
|
||||
|
||||
def test_submit_conflict_and_not_ready_mapping(bridge):
|
||||
def test_duplicate_submit_does_not_start_a_second_motion(bridge):
|
||||
fake, client = bridge
|
||||
fake.submit_success = False
|
||||
fake.status_payload = {
|
||||
'found': True, 'run_id': 'r1', 'state': 'rejected',
|
||||
'reason': 'busy: another run is active',
|
||||
}
|
||||
fake.submit_code = 'duplicate'
|
||||
response = client.post('/planner/runs', json={
|
||||
'run_id': 'r1', 'scene_revision': 4,
|
||||
'run_id': 'r1',
|
||||
'response': {'status': 'ready', 'entry': 'a', 'plan': []},
|
||||
})
|
||||
assert response.status_code == 200
|
||||
assert response.json()['status'] == 'duplicate'
|
||||
|
||||
|
||||
@pytest.mark.parametrize('code,http_status', [
|
||||
('busy', 409),
|
||||
('conflict', 409),
|
||||
('stale_scene', 409),
|
||||
('not_ready', 503),
|
||||
('invalid', 422),
|
||||
('rejected', 422),
|
||||
('replan_required', 422),
|
||||
])
|
||||
def test_submit_errors_include_the_exact_code_and_reason(bridge, code, http_status):
|
||||
fake, client = bridge
|
||||
fake.submit_accepted = False
|
||||
fake.submit_code = code
|
||||
fake.submit_reason = f'{code} because of a concrete reason'
|
||||
response = client.post('/planner/runs', json={
|
||||
'run_id': 'r1',
|
||||
'response': {'status': 'blocked', 'entry': '', 'plan': []},
|
||||
})
|
||||
assert response.status_code == 409
|
||||
assert 'busy' in response.json()['detail']
|
||||
assert response.status_code == http_status
|
||||
detail = response.json()['detail']
|
||||
assert detail['code'] == code
|
||||
assert detail['reason'] == f'{code} because of a concrete reason'
|
||||
assert detail['run_id'] == 'r1'
|
||||
|
||||
fake.status_payload['reason'] = 'not_ready: scene is not ready'
|
||||
response = client.post('/planner/runs', json={
|
||||
'run_id': 'r2', 'scene_revision': 4,
|
||||
'response': {'status': 'blocked', 'entry': '', 'plan': []},
|
||||
})
|
||||
assert response.status_code == 503
|
||||
|
||||
def test_submit_service_failure_is_503(bridge):
|
||||
fake, client = bridge
|
||||
fake.raise_on.add(planner.SUBMIT_SERVICE)
|
||||
assert client.post('/planner/runs', json={
|
||||
'run_id': 'r3', 'scene_revision': 4,
|
||||
'run_id': 'r3',
|
||||
'response': {'status': 'blocked', 'entry': '', 'plan': []},
|
||||
}).status_code == 503
|
||||
|
||||
@@ -139,6 +161,7 @@ def test_status_returns_404_for_unknown_run(bridge):
|
||||
response = client.get('/planner/runs/r1')
|
||||
assert response.status_code == 200
|
||||
assert response.json()['state'] == 'running'
|
||||
assert 'scene_revision' not in response.json()
|
||||
fake.status_payload = None
|
||||
assert client.get('/planner/runs/nope').status_code == 404
|
||||
|
||||
|
||||
Reference in New Issue
Block a user