feat(web): expose planner scene and graph runs over HTTP and MCP

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)

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

src/iiwa_web/package.xml

@@ -15,6 +15,10 @@
   <exec_depend>tf2_py</exec_depend>
   <exec_depend>moveit_msgs</exec_depend>
   <exec_depend>iiwa_planning</exec_depend>
+  <exec_depend>iiwa_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>webots_ros2_msgs</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>ament_copyright</test_depend>

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