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