From 5dc8d651f3806723d9ac17dd00111f37a447c0e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 11 Nov 2026 00:28:04 +1000 Subject: [PATCH] feat(web): expose planner scene and graph runs over HTTP and MCP --- src/iiwa_web/iiwa_web/main.py | 32 +++-- src/iiwa_web/iiwa_web/planner.py | 174 ++++++++++++++++++++++++ src/iiwa_web/package.xml | 4 + src/iiwa_web/test/test_planner.py | 211 ++++++++++++++++++++++++++++++ 4 files changed, 409 insertions(+), 12 deletions(-) create mode 100644 src/iiwa_web/iiwa_web/planner.py create mode 100644 src/iiwa_web/test/test_planner.py diff --git a/src/iiwa_web/iiwa_web/main.py b/src/iiwa_web/iiwa_web/main.py index 1cb8381..56afa87 100644 --- a/src/iiwa_web/iiwa_web/main.py +++ b/src/iiwa_web/iiwa_web/main.py @@ -10,13 +10,14 @@ from std_srvs.srv import Trigger from .dynamic_router import build_dynamic_router from .ros_node import CobotWebNode, get_bridge, set_bridge -from . import runner, trajectory, positions +from . import runner, trajectory, positions, planner stop_router = APIRouter(tags=["stop"]) @stop_router.post("/stop", summary="Остановить всё: runner, траекторию и планировщик") def stop_all(): + planner.cancel_active_run() runner.stop_if_running() trajectory.send_stop_trajectory() @@ -41,6 +42,22 @@ def stop_all(): } +def add_application_routes(app: FastAPI, endpoints_path, joint_limits_path) -> None: + """Add the same routers to the schema app and the serving app.""" + app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) + app.include_router(runner.router) + app.include_router(trajectory.router) + app.include_router(positions.router) + app.include_router(planner.router) + app.include_router(stop_router) + + +def build_schema_app(endpoints_path=None, joint_limits_path=None) -> FastAPI: + schema_app = FastAPI() + add_application_routes(schema_app, endpoints_path, joint_limits_path) + return schema_app + + def main(): rclpy.init() node = CobotWebNode() @@ -54,12 +71,7 @@ def main(): positions.init() - _schema_app = FastAPI() - _schema_app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) - _schema_app.include_router(runner.router) - _schema_app.include_router(trajectory.router) - _schema_app.include_router(positions.router) - _schema_app.include_router(stop_router) + _schema_app = build_schema_app(endpoints_path, joint_limits_path) mcp = FastMCP.from_fastapi(app=_schema_app) mcp_http = mcp.http_app(path='/') @@ -71,11 +83,7 @@ def main(): yield app = FastAPI(lifespan=lifespan) - app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) - app.include_router(runner.router) - app.include_router(trajectory.router) - app.include_router(positions.router) - app.include_router(stop_router) + add_application_routes(app, endpoints_path, joint_limits_path) app.mount("/mcp", mcp_http) uvicorn.run(app, host=host, port=port) diff --git a/src/iiwa_web/iiwa_web/planner.py b/src/iiwa_web/iiwa_web/planner.py new file mode 100644 index 0000000..21d4706 --- /dev/null +++ b/src/iiwa_web/iiwa_web/planner.py @@ -0,0 +1,174 @@ +"""Thin HTTP/MCP adapter for the scene snapshot and graph executor services. + +This module contains no motion logic and no local scene revision. It only +forwards JSON to the ROS services using the existing web bridge. +""" +from __future__ import annotations + +import json +import threading +from typing import Any + +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 .ros_node import get_bridge + + +SCENE_SERVICE = '/iiwa_scene_context/get_scene' +SUBMIT_SERVICE = '/iiwa_llm_plan_executor/submit' +CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel' +STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status' + +router = APIRouter(prefix='/planner', tags=['planner']) +_lock = threading.Lock() +_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', + ) + + +def _status(run_id: str) -> dict[str, Any] | None: + try: + response = get_bridge().call_service( + GetRunStatus, + STATUS_SERVICE, + GetRunStatus.Request(run_id=run_id), + ) + except (RuntimeError, TimeoutError): + return None + if not response.found: + return None + try: + payload = json.loads(response.status_json) + except json.JSONDecodeError: + return 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 + + +@router.get('/scene', summary='Текущий scene_json для внешнего планировщика') +def get_scene(): + try: + response = get_bridge().call_service( + Trigger, SCENE_SERVICE, Trigger.Request(), + ) + except (RuntimeError, TimeoutError) as error: + raise HTTPException(503, str(error)) from error + if not response.success: + raise HTTPException(409, response.message or 'scene is not ready') + try: + payload = json.loads(response.message) + except json.JSONDecodeError as error: + raise HTTPException(502, f'invalid scene JSON: {error}') from error + if not isinstance(payload, dict): + raise HTTPException(502, 'scene envelope must be a JSON object') + return payload + + +@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, + } + try: + response = get_bridge().call_service( + webots_ros2_msgs.srv.SetString, + SUBMIT_SERVICE, + webots_ros2_msgs.srv.SetString.Request( + value=json.dumps(payload, ensure_ascii=False), + ), + ) + 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) + with _lock: + _active_run_id = request.run_id + return { + 'status': 'accepted', + 'run_id': request.run_id, + 'scene_revision': request.scene_revision, + } + + +@router.get('/runs/{run_id}', summary='Статус одного запуска графа') +def get_run(run_id: str): + global _active_run_id + status = _status(run_id) + if status is None: + raise HTTPException(404, f'run {run_id!r} was not submitted') + if status.get('state') in ( + 'completed', 'replan_required', 'operator_required', 'failed', + 'canceled', 'rejected', + ): + with _lock: + if _active_run_id == run_id: + _active_run_id = None + return status + + +@router.post('/runs/{run_id}/cancel', summary='Отменить запуск графа') +def cancel_run(run_id: str): + global _active_run_id + try: + response = get_bridge().call_service( + webots_ros2_msgs.srv.SetString, + CANCEL_SERVICE, + webots_ros2_msgs.srv.SetString.Request( + value=json.dumps({'run_id': run_id}), + ), + ) + except (RuntimeError, TimeoutError) as error: + raise HTTPException(503, str(error)) from error + if not response.success: + status = _status(run_id) + if status is None: + raise HTTPException(404, f'run {run_id!r} was not submitted') + raise HTTPException(409, status.get('reason') or 'run is not active') + with _lock: + if _active_run_id == run_id: + _active_run_id = None + return {'status': 'cancel_requested', 'run_id': run_id} + + +def cancel_active_run() -> None: + """Stop the accepted graph before the regular cobot/stop command.""" + global _active_run_id + with _lock: + run_id = _active_run_id + _active_run_id = None + if not run_id: + return + try: + get_bridge().call_service( + webots_ros2_msgs.srv.SetString, + CANCEL_SERVICE, + webots_ros2_msgs.srv.SetString.Request( + value=json.dumps({'run_id': run_id}), + ), + ) + except (RuntimeError, TimeoutError): + return diff --git a/src/iiwa_web/package.xml b/src/iiwa_web/package.xml index df14537..4368b80 100644 --- a/src/iiwa_web/package.xml +++ b/src/iiwa_web/package.xml @@ -15,6 +15,10 @@ tf2_py moveit_msgs iiwa_planning + iiwa_msgs + std_srvs + sensor_msgs + webots_ros2_msgs python3-yaml ament_copyright diff --git a/src/iiwa_web/test/test_planner.py b/src/iiwa_web/test/test_planner.py new file mode 100644 index 0000000..477edae --- /dev/null +++ b/src/iiwa_web/test/test_planner.py @@ -0,0 +1,211 @@ +"""HTTP adapter tests for scene, submit, status and cancellation.""" +from __future__ import annotations + +import json +from types import SimpleNamespace + +import pytest +from fastapi import APIRouter, FastAPI +from fastapi.testclient import TestClient + +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': []}, + } + + +class FakeBridge: + def __init__(self): + self.calls = [] + self.submit_success = True + self.cancel_success = True + self.scene_success = True + self.status_payload = { + 'found': True, + 'run_id': 'r1', + 'state': 'running', + 'reason': '', + } + self.raise_on = set() + + def call_service(self, srv_type, srv_name, request): + self.calls.append((srv_name, request)) + if srv_name in self.raise_on: + raise RuntimeError(f"{srv_name} unavailable") + if srv_name == planner.SCENE_SERVICE: + return SimpleNamespace( + success=self.scene_success, + message=json.dumps(envelope()) if self.scene_success else 'scene not ready', + ) + if srv_name == planner.SUBMIT_SERVICE: + return SimpleNamespace(success=self.submit_success) + if srv_name == planner.CANCEL_SERVICE: + return SimpleNamespace(success=self.cancel_success) + if srv_name == planner.STATUS_SERVICE: + if self.status_payload is None: + return SimpleNamespace(found=False, status_json='{}') + return SimpleNamespace( + found=bool(self.status_payload.get('found', True)), + status_json=json.dumps(self.status_payload), + ) + raise AssertionError(srv_name) + + +@pytest.fixture +def bridge(monkeypatch): + fake = FakeBridge() + monkeypatch.setattr(planner, 'get_bridge', lambda: fake) + planner._active_run_id = None + app = FastAPI() + app.include_router(planner.router) + return fake, TestClient(app) + + +def test_scene_returns_the_envelope(bridge): + fake, client = bridge + response = client.get('/planner/scene') + assert response.status_code == 200 + assert response.json()['scene_revision'] == 4 + assert fake.calls[0][0] == planner.SCENE_SERVICE + + +def test_scene_not_ready_and_service_failure(bridge): + fake, client = bridge + fake.scene_success = False + assert client.get('/planner/scene').status_code == 409 + fake.raise_on.add(planner.SCENE_SERVICE) + assert client.get('/planner/scene').status_code == 503 + + +def test_submit_accepts_without_waiting_for_completion(bridge): + fake, client = bridge + response = client.post('/planner/runs', json={ + 'run_id': 'r1', + 'scene_revision': 4, + 'response': { + 'status': 'ready', + 'entry': 'a', + 'plan': [], + }, + }) + assert response.status_code == 200 + assert response.json()['status'] == 'accepted' + 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 planner._active_run_id == 'r1' + + +def test_submit_conflict_and_not_ready_mapping(bridge): + fake, client = bridge + fake.submit_success = False + fake.status_payload = { + 'found': True, 'run_id': 'r1', 'state': 'rejected', + 'reason': 'busy: another run is active', + } + response = client.post('/planner/runs', json={ + 'run_id': 'r1', 'scene_revision': 4, + 'response': {'status': 'blocked', 'entry': '', 'plan': []}, + }) + assert response.status_code == 409 + assert 'busy' in response.json()['detail'] + + 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 + + fake.raise_on.add(planner.SUBMIT_SERVICE) + assert client.post('/planner/runs', json={ + 'run_id': 'r3', 'scene_revision': 4, + 'response': {'status': 'blocked', 'entry': '', 'plan': []}, + }).status_code == 503 + + +def test_status_returns_404_for_unknown_run(bridge): + fake, client = bridge + response = client.get('/planner/runs/r1') + assert response.status_code == 200 + assert response.json()['state'] == 'running' + fake.status_payload = None + assert client.get('/planner/runs/nope').status_code == 404 + + +def test_status_clears_the_active_run_on_terminal(bridge): + fake, client = bridge + planner._active_run_id = 'r1' + fake.status_payload = { + 'found': True, 'run_id': 'r1', 'state': 'completed', + 'reason': 'done', + } + assert client.get('/planner/runs/r1').json()['state'] == 'completed' + assert planner._active_run_id is None + + +def test_cancel_and_unknown_cancel(bridge): + fake, client = bridge + response = client.post('/planner/runs/r1/cancel') + assert response.status_code == 200 + service, request = fake.calls[0] + assert service == planner.CANCEL_SERVICE + assert json.loads(request.value)['run_id'] == 'r1' + + fake.cancel_success = False + fake.status_payload = None + assert client.post('/planner/runs/nope/cancel').status_code == 404 + + +def test_stop_cancels_the_active_graph_before_the_robot_stop(monkeypatch): + order = [] + fake = FakeBridge() + + def call_service(srv_type, srv_name, request): + order.append(srv_name) + return SimpleNamespace(success=True, message='stopped') + + fake.call_service = call_service + monkeypatch.setattr(main, 'get_bridge', lambda: fake) + monkeypatch.setattr(main.planner, 'cancel_active_run', lambda: order.append('cancel_graph')) + monkeypatch.setattr(main.runner, 'stop_if_running', lambda: order.append('runner')) + monkeypatch.setattr(main.trajectory, 'send_stop_trajectory', lambda: order.append('trajectory')) + result = main.stop_all() + assert result['status'] == 'stopped' + assert order == ['cancel_graph', 'runner', 'trajectory', 'cobot/stop'] + + +def test_schema_app_and_serving_app_share_the_planner_router(monkeypatch): + import asyncio + from fastmcp import FastMCP + + monkeypatch.setattr(main, 'build_dynamic_router', lambda *args: APIRouter()) + schema = main.build_schema_app(None, None) + paths = {route.path for route in schema.routes} + assert { + '/planner/scene', '/planner/runs', '/planner/runs/{run_id}', + '/planner/runs/{run_id}/cancel', '/stop', + } <= paths + + mcp = FastMCP.from_fastapi(app=schema) + tools = asyncio.run(mcp.list_tools()) + names = {tool.name for tool in tools} + assert 'get_scene_planner_scene_get' in names + assert 'submit_run_planner_runs_post' in names + assert 'get_run_planner_runs' in names + assert 'cancel_run_planner_runs' in names + + app = FastAPI() + main.add_application_routes(app, None, None) + served = {route.path for route in app.routes} + assert paths <= served