feat(llm-plan-executor): add graph engine and ROS submit/status/cancel node

src/iiwa_llm_plan_executor/docs/interfaces.md

@@ -89,6 +89,31 @@ Important pre-movement rules:
   and no insertion/tightening occurs.
 * No task is inferred from `experiment_id`; only the initial scene is selected.
 
+## ROS execution interfaces
+
+| Direction | Name | Type | 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. |
+
+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`.
+
+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`).
+
+
 ## Skill phases
 
 A skill may contain the mandatory geometric phases of one action. `approach`

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py

@@ -0,0 +1,585 @@
+"""ROS-free graph engine: admission, edge selection, terminals and status."""
+from __future__ import annotations
+
+import json
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Any, Callable, Mapping
+
+from iiwa_scene_context.catalog import DEFAULT_CATALOG, SkillCatalog
+
+from .schema import TERMINALS, PlanGraph, PlanSchemaError, parse_response
+from .validator import ValidationResult, validate_graph
+
+
+ACTIVE_STATES = ('admitted', 'running')
+TERMINAL_STATES = (
+    'completed', 'replan_required', 'operator_required', 'failed', 'canceled',
+)
+OWNERSHIP_SKILLS = ('grab', 'pick', 'release', 'dock_tool', 'handoff_screw')
+DEFAULT_MIN_START_DELAY_SEC = 3.0
+DEFAULT_SCENE_MAX_AGE_SEC = 2.0
+DEFAULT_MAX_STEPS = 512
+
+
+@dataclass(frozen=True)
+class AdmissionResult:
+    accepted: bool
+    code: str
+    message: str
+    run_id: str
+
+    def as_dict(self) -> dict[str, Any]:
+        return {
+            'accepted': self.accepted,
+            'code': self.code,
+            'message': self.message,
+            'run_id': self.run_id,
+        }
+
+
+@dataclass
+class RunRecord:
+    run_id: str
+    state: str = 'new'
+    reason: str = ''
+    scene_revision: int = 0
+    response: str = ''
+    node_id: str | None = None
+    skill: str | None = None
+    event: str | None = None
+    next: str | None = None
+    mounted_tool: str | None = None
+    held_object: str | None = None
+    terminal: str | None = None
+    events: list[dict[str, Any]] = field(default_factory=list)
+    context: Any = None
+    cancel_requested: bool = False
+    done: threading.Event = field(default_factory=threading.Event)
+    thread: threading.Thread | None = None
+    details: dict[str, Any] = field(default_factory=dict)
+
+
+@dataclass(frozen=True)
+class RunStatus:
+    found: bool
+    run_id: str
+    state: str
+    reason: str
+    node_id: str | None
+    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 = {
+            'found': self.found,
+            'run_id': self.run_id,
+            'state': self.state,
+            'reason': self.reason,
+            'node_id': self.node_id,
+            '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)
+
+
+@dataclass(frozen=True)
+class CancelResult:
+    accepted: bool
+    message: str
+    run_id: str
+
+
+class GraphEngine:
+    def __init__(
+        self,
+        scene_provider: Callable[[], Mapping[str, Any] | None],
+        context_factory: Callable[[Mapping[str, Any], Mapping[str, Any]], Any],
+        skills: Any,
+        *,
+        catalog: SkillCatalog = DEFAULT_CATALOG,
+        clock: Callable[[], float] | None = None,
+        on_plan: Callable[[dict[str, Any]], None] | None = None,
+        on_status: Callable[[dict[str, Any]], None] | None = None,
+        on_replan: Callable[[dict[str, Any] | None], None] | None = None,
+        on_stop: Callable[[str], None] | None = None,
+        robot_fresh: Callable[[], tuple[bool, str]] | None = None,
+        threaded: bool = True,
+        min_start_delay_sec: float = DEFAULT_MIN_START_DELAY_SEC,
+        scene_max_age_sec: float = DEFAULT_SCENE_MAX_AGE_SEC,
+        max_steps: int = DEFAULT_MAX_STEPS,
+    ) -> None:
+        self.scene_provider = scene_provider
+        self.context_factory = context_factory
+        self.skills = skills
+        self.catalog = catalog
+        self._clock = clock or time.monotonic
+        self.on_plan = on_plan
+        self.on_status = on_status
+        self.on_replan = on_replan
+        self.on_stop = on_stop
+        self.robot_fresh = robot_fresh
+        self.threaded = bool(threaded)
+        self.min_start_delay_sec = float(min_start_delay_sec)
+        self.scene_max_age_sec = float(scene_max_age_sec)
+        self.max_steps = int(max_steps)
+        self.scene_ready = False
+        self.ready_at: float | None = None
+        self.motion_ready = False
+        self.readiness_reason = 'not ready'
+        self._runs: dict[str, RunRecord] = {}
+        self._lock = threading.RLock()
+
+    # ------------------------------------------------------------------ readiness
+    def update_readiness(
+        self,
+        *,
+        scene_ready: bool,
+        ready_at: float | None,
+        motion_ready: bool,
+        reason: str = '',
+    ) -> None:
+        self.scene_ready = bool(scene_ready)
+        self.ready_at = None if ready_at is None else float(ready_at)
+        self.motion_ready = bool(motion_ready)
+        self.readiness_reason = reason or (
+            'ready' if self.scene_ready and self.motion_ready else 'not ready'
+        )
+
+    def _not_ready(self, now: float) -> str | None:
+        if not self.scene_ready:
+            return f'scene is not ready: {self.readiness_reason}'
+        if not self.motion_ready:
+            return f'motion is not ready: {self.readiness_reason}'
+        if self.ready_at is None:
+            return 'scene_ready time is unknown'
+        remaining = self.min_start_delay_sec - (now - self.ready_at)
+        if remaining > 0.0:
+            return (
+                'start delay has not elapsed '
+                f'({remaining:.2f} s of {self.min_start_delay_sec:.1f} s left)'
+            )
+        return None
+
+    # ------------------------------------------------------------------ admission
+    def submit(
+        self,
+        run_id: str,
+        response_json: str | Mapping[str, Any],
+        scene_revision: int,
+    ) -> AdmissionResult:
+        run_id = str(run_id).strip()
+        if not run_id:
+            return AdmissionResult(False, 'rejected', 'run_id must not be empty', '')
+        raw_response = (
+            response_json
+            if isinstance(response_json, str)
+            else json.dumps(response_json, ensure_ascii=False)
+        )
+        with self._lock:
+            existing = self._runs.get(run_id)
+            if existing is not None:
+                if existing.state in ACTIVE_STATES:
+                    return AdmissionResult(
+                        True, 'duplicate',
+                        f'run {run_id!r} is already active; 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',
+                    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,
+                )
+            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,
+                )
+            if self.threaded:
+                thread = threading.Thread(
+                    target=self._execute,
+                    args=(run_id, graph, envelope),
+                    name=f'graph-run-{run_id}',
+                    daemon=True,
+                )
+                record.thread = thread
+                thread.start()
+            return AdmissionResult(True, 'accepted', 'graph admitted', run_id)
+
+    # ------------------------------------------------------------------ helpers
+    def _envelope(self) -> Mapping[str, Any] | None:
+        try:
+            envelope = self.scene_provider()
+        except Exception:
+            return None
+        if not isinstance(envelope, Mapping):
+            return None
+        if not isinstance(envelope.get('scene_json'), Mapping):
+            return None
+        if 'scene_revision' not in envelope:
+            return None
+        return envelope
+
+    def _scene_age(self, envelope: Mapping[str, Any], now: float) -> str | None:
+        try:
+            stamp = float(envelope.get('timestamp'))
+        except (TypeError, ValueError):
+            return 'scene snapshot has no timestamp'
+        if now - stamp > self.scene_max_age_sec:
+            return (
+                f'scene snapshot is stale ({now - stamp:.2f} s > '
+                f'{self.scene_max_age_sec:.2f} s)'
+            )
+        return None
+
+    def _context(self, run: RunRecord, envelope: Mapping[str, Any]) -> Any:
+        context = self.context_factory(envelope, envelope['scene_json'])
+        initial = (envelope.get('scene_json') or {}).get('initial_state') or {}
+        if hasattr(context, 'mounted_tool'):
+            context.mounted_tool = initial.get('tool_mounted') or None
+        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))
+        run.context = context
+        return context
+
+    def _status_payload(self, run: RunRecord, **extra) -> dict[str, Any]:
+        context = run.context
+        if context is not None:
+            if hasattr(context, 'mounted_tool'):
+                run.mounted_tool = context.mounted_tool
+            if hasattr(context, 'held_object'):
+                run.held_object = context.held_object
+        payload = {
+            'run_id': run.run_id,
+            'state': run.state,
+            'reason': run.reason,
+            'node_id': run.node_id,
+            '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,
+            'timestamp': self._clock(),
+        }
+        payload.update(extra)
+        return payload
+
+    def _emit_status(self, run: RunRecord, **extra) -> None:
+        payload = self._status_payload(run, **extra)
+        if self.on_status is not None:
+            try:
+                self.on_status(payload)
+            except Exception:
+                pass
+
+    def _record_event(
+        self,
+        run: RunRecord,
+        node,
+        event: str,
+        next_id: str,
+        outcome: Any,
+    ) -> None:
+        entry = {
+            'run_id': run.run_id,
+            'node_id': node.id,
+            'skill': node.skill,
+            'event': event,
+            'next': next_id,
+            'scene_revision': run.scene_revision,
+            'mounted_tool': run.mounted_tool,
+            'held_object': run.held_object,
+            'details': getattr(outcome, 'details', {}) or {},
+            'timestamp': self._clock(),
+        }
+        run.events.append(entry)
+        self._emit_status(run, event_detail=entry)
+
+    def _stop_motion(self, reason: str) -> None:
+        if self.on_stop is not None:
+            try:
+                self.on_stop(reason)
+            except Exception:
+                pass
+
+    def _terminal(self, run: RunRecord, state: str, reason: str) -> None:
+        run.state = state
+        run.reason = reason
+        run.terminal = state
+        if state in ('failed', 'canceled'):
+            self._stop_motion(reason)
+        self._emit_status(run, terminal_reason=reason)
+        run.done.set()
+
+    # ------------------------------------------------------------------ execution
+    def run_accepted(self, run_id: str, graph: PlanGraph | None = None,
+                     envelope: Mapping[str, Any] | None = None) -> None:
+        """Synchronously execute an already admitted run (tests, tools)."""
+        run = self._runs.get(str(run_id).strip())
+        if run is None or run.state != 'admitted':
+            return
+        graph = graph or parse_response(run.response)
+        envelope = envelope or self._envelope()
+        if envelope is None:
+            self._terminal(run, 'failed', 'scene snapshot is unavailable')
+            return
+        self._execute(run.run_id, graph, envelope)
+
+    def _execute(self, run_id: str, graph: PlanGraph, first_envelope: Mapping[str, Any]) -> None:
+        with self._lock:
+            run = self._runs[run_id]
+            run.state = 'running'
+        try:
+            context = self._context(run, first_envelope)
+            envelope = first_envelope
+            node_id = graph.entry
+            steps = 0
+            while True:
+                if run.cancel_requested:
+                    self._terminal(run, 'canceled', 'cancel requested')
+                    return
+                if steps >= self.max_steps:
+                    self._terminal(
+                        run, 'failed',
+                        f'executed step limit of {self.max_steps} reached',
+                    )
+                    return
+                now = self._clock()
+                if self.robot_fresh is not None:
+                    try:
+                        fresh, message = self.robot_fresh()
+                    except Exception as error:
+                        fresh, message = False, str(error)
+                    if not fresh:
+                        self._terminal(run, 'failed', f'robot state: {message}')
+                        return
+                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
+                revision = int(current.get('scene_revision', -1))
+                if revision != run.scene_revision:
+                    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
+                    )
+                    if not acknowledged:
+                        self._terminal(
+                            run, 'failed',
+                            'scene_revision changed externally '
+                            f'({run.scene_revision} -> {revision}); motion stopped',
+                        )
+                        return
+                    run.scene_revision = revision
+                    if hasattr(context, 'scene_revision'):
+                        context.scene_revision = revision
+                    envelope = current
+                node = graph.node(node_id)
+                outcome = self.skills.execute(node, context)
+                event = str(outcome.event)
+                transitions = {
+                    transition.event: transition.next for transition in node.transitions
+                }
+                run.node_id = node.id
+                run.skill = node.skill
+                run.event = event
+                if event not in transitions:
+                    run.next = None
+                    self._record_event(run, node, event, '', outcome)
+                    self._terminal(
+                        run, 'failed',
+                        f'unknown skill outcome {event!r} for node {node.id!r}',
+                    )
+                    return
+                next_id = transitions[event]
+                run.next = next_id
+                self._record_event(run, node, event, next_id, outcome)
+                steps += 1
+                if run.cancel_requested:
+                    self._terminal(run, 'canceled', 'cancel requested')
+                    return
+                if next_id in TERMINALS:
+                    if next_id == 'replan_required':
+                        fresh_scene = self._envelope()
+                        if self.on_replan is not None:
+                            try:
+                                self.on_replan(
+                                    None if fresh_scene is None else dict(fresh_scene),
+                                )
+                            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
+        except Exception as error:
+            self._terminal(run, 'failed', f'execution error: {error}')
+
+    # ------------------------------------------------------------------ queries
+    def get_status(self, run_id: str) -> RunStatus:
+        run_id = str(run_id).strip()
+        with self._lock:
+            run = self._runs.get(run_id)
+            if run is None:
+                return RunStatus(
+                    False, run_id, 'unknown', 'run_id was not submitted',
+                    None, None, None, None, 0, None, None, None, (),
+                )
+            return RunStatus(
+                True,
+                run.run_id,
+                run.state,
+                run.reason,
+                run.node_id,
+                run.skill,
+                run.event,
+                run.next,
+                run.scene_revision,
+                run.terminal,
+                run.mounted_tool,
+                run.held_object,
+                tuple(run.events),
+            )
+
+    def cancel(self, run_id: str) -> CancelResult:
+        run_id = str(run_id).strip()
+        with self._lock:
+            run = self._runs.get(run_id)
+            if run is None:
+                return CancelResult(False, 'run_id is unknown', run_id)
+            if run.state not in ACTIVE_STATES:
+                return CancelResult(
+                    False, f'run is not active (state={run.state})', run_id,
+                )
+            run.cancel_requested = True
+            if run.state == 'admitted' and not self.threaded:
+                self._terminal(run, 'canceled', 'cancel requested')
+            self._stop_motion('cancel requested')
+            self._emit_status(run, cancel_requested=True)
+            return CancelResult(True, 'cancel requested', run_id)
+
+    def wait(self, run_id: str, timeout: float | None = None) -> bool:
+        run = self._runs.get(str(run_id).strip())
+        if run is None:
+            return False
+        return run.done.wait(timeout)
+
+    def active_run_id(self) -> str | None:
+        with self._lock:
+            for run in self._runs.values():
+                if run.state in ACTIVE_STATES:
+                    return run.run_id
+        return None

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py

@@ -123,6 +123,9 @@ class MoveItPyPlanner:
         self._stopped = False
 
     # ------------------------------------------------------------------ setup
+    def ensure_ready(self) -> None:
+        self._ensure()
+
     def _ensure(self) -> None:
         if self._arm is not None:
             return

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py

@@ -0,0 +1,751 @@
+"""ROS facade for the graph engine: submit, plan, status, cancel, logs."""
+from __future__ import annotations
+
+import json
+import re
+import threading
+import time
+import uuid
+from pathlib import Path
+from typing import Any, Mapping
+
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from control_msgs.action import FollowJointTrajectory
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import (
+    DurabilityPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_sensor_data,
+)
+from sensor_msgs.msg import Image, JointState
+from std_msgs.msg import String
+from std_srvs.srv import Trigger
+import webots_ros2_msgs.srv
+
+from iiwa_experement.geometry import GeometrySpec, load_geometry
+from iiwa_experement.planning_scene import (
+    PlanningSceneBridge,
+    attached_cap,
+    attached_screw,
+    attached_tool,
+    cap_world_box,
+    placed_cap_pose,
+    screw_object_id,
+)
+from iiwa_experement.scenario import ScenarioSpec, load_scenario
+from iiwa_msgs.srv import GetRunStatus
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.node import SCENE_TOPIC
+
+from .engine import GraphEngine
+from .motion_policy import MotionPolicy, load_limits
+from .moveit_backend import MoveItBackend
+from .skill_context import SkillContext
+from .skills import SkillRegistry
+
+
+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'
+CANCEL_SERVICE = '/iiwa_llm_plan_executor/cancel'
+STATUS_SERVICE = '/iiwa_llm_plan_executor/get_status'
+PLAN_TOPIC = '/iiwa_llm_plan_executor/plan'
+STATUS_TOPIC = '/iiwa_llm_plan_executor/status'
+CONTROLLER_ACTION = '/iiwa_arm_controller/follow_joint_trajectory'
+DEFAULT_TRACE_DIR = '~/.ros/iiwa_llm_plan_executor/runs'
+
+
+def plan_qos() -> QoSProfile:
+    return QoSProfile(
+        depth=1,
+        reliability=ReliabilityPolicy.RELIABLE,
+        durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    )
+
+
+def _safe_name(value: str) -> str:
+    return re.sub(r'[^A-Za-z0-9_.-]+', '_', value)[:80] or 'run'
+
+
+class RunLog:
+    """Machine-readable JSONL log, one file per run id."""
+
+    def __init__(self, directory: str) -> None:
+        self.directory = Path(directory).expanduser()
+        self._lock = threading.Lock()
+
+    def write(self, payload: Mapping[str, Any]) -> None:
+        try:
+            self.directory.mkdir(parents=True, exist_ok=True)
+            run_id = _safe_name(str(payload.get('run_id', 'run')))
+            path = self.directory / f'{run_id}.jsonl'
+            with self._lock, path.open('a', encoding='utf-8') as stream:
+                stream.write(json.dumps(payload, ensure_ascii=False) + '\n')
+        except Exception:
+            pass
+
+
+class SupervisorFacade:
+    """Thread-safe supervisor client that never spins its own node."""
+
+    def __init__(self, node: Node, callback_group) -> None:
+        self.node = node
+        self._lock = threading.Condition()
+        self.latest: dict[str, Any] | None = None
+        self._client = node.create_client(
+            webots_ros2_msgs.srv.SetString,
+            COMMAND_SERVICE,
+            callback_group=callback_group,
+        )
+        node.create_subscription(
+            String, STATE_TOPIC, self._on_state, 10,
+            callback_group=callback_group,
+        )
+
+    def _on_state(self, message: String) -> None:
+        try:
+            payload = json.loads(message.data)
+        except Exception:
+            return
+        if not isinstance(payload, dict):
+            return
+        with self._lock:
+            self.latest = payload
+            self._lock.notify_all()
+
+    def state(self) -> dict[str, Any]:
+        with self._lock:
+            return dict(self.latest or {})
+
+    # Alias used by the skills.
+    def latest_state(self) -> dict[str, Any]:
+        return self.state()
+
+    def latest(self) -> dict[str, Any]:
+        return self.state()
+
+    def command(
+        self,
+        op: str,
+        object_id: str = '',
+        *,
+        target=None,
+        target_name: str = '',
+    ) -> dict[str, Any]:
+        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')
+        request_id = uuid.uuid4().hex
+        payload: dict[str, Any] = {
+            'request_id': request_id,
+            'op': op,
+            'object': object_id,
+            'target_name': target_name,
+        }
+        if target is not None:
+            payload['target'] = {
+                'xyz': [float(value) for value in target.xyz],
+                'rotation': [
+                    [float(value) for value in row] for row in target.rotation
+                ],
+            }
+        request = webots_ros2_msgs.srv.SetString.Request()
+        request.value = json.dumps(payload)
+        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')
+        try:
+            response = future.result()
+        except Exception as error:
+            raise RuntimeError(f'supervisor command {op} failed: {error}') from error
+        accepted = bool(response.success)
+        state = self._wait_for_result(request_id, timeout=3.0)
+        last = (state or {}).get('last_command_result') or {}
+        if last.get('request_id') != request_id:
+            raise RuntimeError(
+                f'supervisor did not publish the result of {op}'
+            )
+        if not accepted or not last.get('accepted', False):
+            raise RuntimeError(
+                last.get('error') or f'supervisor rejected {op}'
+            )
+        return state
+
+    def _wait_for_result(self, request_id: str, timeout: float) -> dict[str, Any]:
+        deadline = time.monotonic() + timeout
+        with self._lock:
+            while True:
+                state = dict(self.latest or {})
+                last = state.get('last_command_result') or {}
+                if last.get('request_id') == request_id:
+                    return state
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    return state
+                self._lock.wait(timeout=min(0.1, remaining))
+
+
+class SceneAdapter:
+    """Keep the MoveIt scene aligned with the supervisor's ownership state."""
+
+    def __init__(
+        self,
+        bridge: PlanningSceneBridge,
+        spec: ScenarioSpec,
+        geometry: GeometrySpec,
+    ) -> None:
+        self.bridge = bridge
+        self.spec = spec
+        self.geometry = geometry
+
+    def _sync(self) -> None:
+        self.bridge.assert_synchronized(*self.bridge.expected_sets())
+
+    def attach_tool(self, tool: str) -> None:
+        self.bridge.attach(attached_tool(tool, self.geometry))
+        self._sync()
+
+    def dock_tool(self, tool: str) -> None:
+        self.bridge.restore_world(f'grant_{tool}')
+        self._sync()
+
+    def attach_cap(self) -> None:
+        self.bridge.attach(attached_cap(self.geometry))
+        self._sync()
+
+    def place_cap(self, seat) -> None:
+        self.bridge.add_world(
+            'grant_cap',
+            cap_world_box(self.geometry),
+            placed_cap_pose(seat, self.spec.body.yaw_deg),
+        )
+        self._sync()
+
+    def attach_screw(self, screw_id: str) -> None:
+        self.bridge.attach(attached_screw(screw_id, self.geometry))
+        self._sync()
+
+    def handoff_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+    # Hooks used before entering a contact so the carried geometry cannot
+    # collide with itself or with the target it is about to touch.
+    def prepare_grab_tool(self, tool: str) -> None:
+        self.bridge.remove(f'grant_{tool}')
+        self._sync()
+
+    def restore_tool(self, tool: str) -> None:
+        self.bridge.restore_world(f'grant_{tool}')
+        self._sync()
+
+    def prepare_grab_cap(self, object_id: str) -> None:
+        del object_id
+        self.bridge.remove('grant_cap')
+        self._sync()
+
+    def prepare_grab_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+    def restore_part(self, object_id: str) -> None:
+        name = 'grant_cap' if object_id == 'cap' else screw_object_id(object_id)
+        self.bridge.restore_world(name)
+        self._sync()
+
+    def prepare_dock_tool(self, tool: str) -> None:
+        self.bridge.remove(f'grant_{tool}')
+        self._sync()
+
+    def prepare_place_cap(self) -> None:
+        self.bridge.remove('grant_cap')
+        self._sync()
+
+    def prepare_handoff_screw(self, screw_id: str) -> None:
+        self.bridge.remove(screw_object_id(screw_id))
+        self._sync()
+
+
+class PlannerExecutorNode(Node):
+    def __init__(self) -> None:
+        super().__init__('iiwa_llm_plan_executor')
+        package_share = Path(get_package_share_directory('iiwa_experement'))
+        executor_share = Path(
+            get_package_share_directory('iiwa_llm_plan_executor')
+        )
+        self.declare_parameter('experiment_id', 20)
+        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(
+            'motion_limits_path',
+            str(executor_share / 'config' / 'motion_limits.yaml'),
+        )
+        self.declare_parameter('start_delay_sec', 3.0)
+        self.declare_parameter('scene_max_age_sec', 2.0)
+        self.declare_parameter('max_steps', 512)
+        self.declare_parameter('planning_group', 'iiwa_arm')
+        self.declare_parameter('pose_link', 'tcp')
+        self.declare_parameter('close_inspection_objects', ['cap'])
+        self.declare_parameter('top_camera_topic', '/d455_top/image_raw')
+        self.declare_parameter('hand_camera_topic', '/camera/color')
+        self.declare_parameter('frame_timeout_sec', 2.0)
+        self.declare_parameter('controller_action', CONTROLLER_ACTION)
+        self.declare_parameter('trace_dir', DEFAULT_TRACE_DIR)
+
+        self.experiment_id = int(self.get_parameter('experiment_id').value)
+        self.spec = load_scenario(
+            self.experiment_id, str(self.get_parameter('scene_config').value),
+        )
+        self.geometry = load_geometry(
+            str(self.get_parameter('geometry_path').value),
+        )
+        self.limits = load_limits(
+            str(self.get_parameter('motion_limits_path').value),
+        )
+        self.policy = MotionPolicy(self.limits)
+        self.close_inspection = frozenset(
+            str(value) for value in self.get_parameter(
+                'close_inspection_objects',
+            ).value
+        )
+        self.frame_timeout = float(
+            self.get_parameter('frame_timeout_sec').value
+        )
+        self.scene_max_age = float(
+            self.get_parameter('scene_max_age_sec').value
+        )
+        self.top_topic = str(self.get_parameter('top_camera_topic').value)
+        self.hand_topic = str(self.get_parameter('hand_camera_topic').value)
+        self.log = RunLog(str(self.get_parameter('trace_dir').value))
+
+        self.callback_group = ReentrantCallbackGroup()
+        self._lock = threading.RLock()
+        self._latest_scene: dict[str, Any] | None = None
+        self._scene_ready_at: float | None = None
+        self._last_joint: tuple[float, ...] | None = None
+        self._last_joint_at: float | None = None
+        self._moveit_ready = False
+        self._bridge_ready = False
+        self._last_plan: dict[str, Any] | None = None
+        self._last_status: dict[str, Any] | None = None
+        self._frame_conditions = {
+            self.top_topic: threading.Condition(),
+            self.hand_topic: threading.Condition(),
+        }
+        self._frame_counts = {self.top_topic: 0, self.hand_topic: 0}
+        self._frame_stamps: dict[str, float] = {}
+
+        # ------------------------------------------------------------ interfaces
+        self._plan_publisher = self.create_publisher(String, PLAN_TOPIC, plan_qos())
+        self._status_publisher = self.create_publisher(String, STATUS_TOPIC, 10)
+        self.create_service(
+            webots_ros2_msgs.srv.SetString,
+            SUBMIT_SERVICE,
+            self._handle_submit,
+            callback_group=self.callback_group,
+        )
+        self.create_service(
+            webots_ros2_msgs.srv.SetString,
+            CANCEL_SERVICE,
+            self._handle_cancel,
+            callback_group=self.callback_group,
+        )
+        self.create_service(
+            GetRunStatus,
+            STATUS_SERVICE,
+            self._handle_get_status,
+            callback_group=self.callback_group,
+        )
+        self.create_subscription(
+            String,
+            SCENE_TOPIC,
+            self._on_scene,
+            plan_qos(),
+            callback_group=self.callback_group,
+        )
+        self.create_subscription(
+            JointState,
+            '/joint_states',
+            self._on_joints,
+            10,
+            callback_group=self.callback_group,
+        )
+        for topic in (self.top_topic, self.hand_topic):
+            self.create_subscription(
+                Image,
+                topic,
+                lambda message, name=topic: self._on_image(name, message),
+                qos_profile_sensor_data,
+                callback_group=self.callback_group,
+            )
+
+        self.supervisor = SupervisorFacade(self, self.callback_group)
+        self._scene_client = self.create_client(
+            Trigger, SCENE_SERVICE, callback_group=self.callback_group,
+        )
+        self._controller = ActionClient(
+            self, FollowJointTrajectory,
+            str(self.get_parameter('controller_action').value),
+            callback_group=self.callback_group,
+        )
+        self.bridge = PlanningSceneBridge(
+            node=self,
+            use_sim_time=True,
+            spin_fn=lambda timeout: time.sleep(max(0.0, float(timeout))),
+        )
+        self.adapter = SceneAdapter(self.bridge, self.spec, self.geometry)
+        self.motion = MoveItBackend(
+            self.geometry,
+            self.policy,
+            planner=self._make_planner(),
+            freshness_guard=self._robot_fresh,
+            scene_guard=self._scene_guard,
+            current_joints=lambda: self._last_joint,
+            on_event=self._on_motion_event,
+        )
+        self.skills = SkillRegistry(
+            self.motion,
+            self.supervisor,
+            catalog=DEFAULT_CATALOG,
+            scene_adapter=self.adapter,
+        )
+        self.engine = GraphEngine(
+            self._scene_provider,
+            self._context_factory,
+            self.skills,
+            catalog=DEFAULT_CATALOG,
+            clock=self._now,
+            on_plan=self._publish_plan,
+            on_status=self._publish_status,
+            on_replan=self._on_replan,
+            on_stop=self._on_stop,
+            robot_fresh=self._robot_fresh,
+            threaded=True,
+            min_start_delay_sec=float(
+                self.get_parameter('start_delay_sec').value
+            ),
+            scene_max_age_sec=self.scene_max_age,
+            max_steps=int(self.get_parameter('max_steps').value),
+        )
+        self.create_timer(
+            0.5, self._readiness_tick, callback_group=self.callback_group,
+        )
+        self.get_logger().info(
+            f'graph executor started for experiment {self.experiment_id}; '
+            f'waiting for scene {SCENE_TOPIC}'
+        )
+
+    # ------------------------------------------------------------------ setup
+    def _make_planner(self):
+        from .moveit_backend import MoveItPyPlanner
+
+        return MoveItPyPlanner(
+            self.geometry,
+            self.policy,
+            planning_group=str(self.get_parameter('planning_group').value),
+            pose_link=str(self.get_parameter('pose_link').value),
+        )
+
+    def _now(self) -> float:
+        return self.get_clock().now().nanoseconds / 1e9
+
+    def _on_joints(self, message: JointState) -> None:
+        if len(message.position) == 7:
+            self._last_joint = tuple(float(value) for value in message.position)
+            self._last_joint_at = self._now()
+
+    def _on_image(self, topic: str, message: Image) -> None:
+        condition = self._frame_conditions.get(topic)
+        if condition is None:
+            return
+        stamp = (
+            message.header.stamp.sec
+            + message.header.stamp.nanosec * 1e-9
+        )
+        if stamp <= 0.0:
+            stamp = self._now()
+        with condition:
+            self._frame_counts[topic] += 1
+            self._frame_stamps[topic] = stamp
+            condition.notify_all()
+
+    def _on_scene(self, message: String) -> None:
+        try:
+            envelope = json.loads(message.data)
+        except Exception:
+            return
+        if not isinstance(envelope, dict):
+            return
+        with self._lock:
+            self._latest_scene = envelope
+            if self._scene_ready_at is None:
+                stamp = envelope.get('timestamp')
+                self._scene_ready_at = (
+                    float(stamp) if isinstance(stamp, (int, float))
+                    else self._now()
+                )
+
+    def _scene_provider(self) -> dict[str, Any] | None:
+        with self._lock:
+            envelope = self._latest_scene
+        if envelope is None:
+            return None
+        try:
+            stamp = float(envelope.get('timestamp'))
+        except (TypeError, ValueError):
+            return None
+        if self._now() - stamp > self.scene_max_age:
+            return None
+        return envelope
+
+    def _robot_fresh(self) -> tuple[bool, str]:
+        if self._last_joint is None or self._last_joint_at is None:
+            return False, 'no /joint_states received yet'
+        age = self._now() - self._last_joint_at
+        if age > self.limits.joint_state_max_age_sec:
+            return False, f'/joint_states is stale ({age:.2f} s old)'
+        return True, 'ok'
+
+    def _scene_guard(self) -> tuple[bool, str]:
+        if not self._bridge_ready:
+            return False, 'planning scene bridge is not initialized'
+        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}'
+        return True, 'ok'
+
+    def _ensure_motion_ready(self) -> None:
+        if self._moveit_ready and self._bridge_ready:
+            return
+        try:
+            self.motion.planner.ensure_ready()
+            self._moveit_ready = True
+        except Exception as error:
+            self._moveit_ready = False
+            self.get_logger().warning(f'MoveIt is not ready yet: {error}')
+        if self._moveit_ready and not self._bridge_ready:
+            try:
+                self.bridge.initialize(self.spec, self.geometry)
+                self._bridge_ready = True
+            except Exception as error:
+                self._bridge_ready = False
+                self.get_logger().warning(
+                    f'planning scene bridge is not ready yet: {error}'
+                )
+
+    def _readiness_tick(self) -> None:
+        self._ensure_motion_ready()
+        controller_ready = self._controller.server_is_ready()
+        fresh, _ = self._robot_fresh()
+        scene = self._scene_provider()
+        scene_ready = scene is not None
+        motion_ready = (
+            self._moveit_ready and self._bridge_ready and controller_ready
+            and fresh
+        )
+        self.engine.update_readiness(
+            scene_ready=scene_ready,
+            ready_at=self._scene_ready_at,
+            motion_ready=motion_ready,
+            reason=(
+                'ready' if scene_ready and motion_ready else
+                f'scene={scene_ready} moveit={self._moveit_ready} '
+                f'bridge={self._bridge_ready} controller={controller_ready} '
+                f'joints={fresh}'
+            ),
+        )
+
+    # ------------------------------------------------------------------ frames
+    def _frame_provider(self, name: str) -> float | None:
+        topic = self.top_topic if name == 'top' else self.hand_topic
+        condition = self._frame_conditions.get(topic)
+        if condition is None:
+            return None
+        deadline = time.monotonic() + self.frame_timeout
+        with condition:
+            baseline = self._frame_counts[topic]
+            while self._frame_counts[topic] <= baseline:
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    return None
+                condition.wait(timeout=remaining)
+            return self._frame_stamps.get(topic)
+
+    # ------------------------------------------------------------------ engine
+    def _context_factory(self, envelope: Mapping[str, Any], scene_json) -> SkillContext:
+        del scene_json
+        context = SkillContext(
+            spec=self.spec,
+            geometry=self.geometry,
+            scene_revision=int(envelope.get('scene_revision', 0)),
+        )
+        context.frame_provider = self._frame_provider
+        context.scene_clear_provider = lambda: True
+        context.notifier = self._notify_operator
+        context.scene_adapter = self.adapter
+        context.close_inspection_objects = self.close_inspection
+        return context
+
+    def _publish_plan(self, payload: dict[str, Any]) -> None:
+        self._last_plan = dict(payload)
+        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')})"
+        )
+
+    def _publish_status(self, payload: dict[str, Any]) -> None:
+        self._last_status = dict(payload)
+        message = String()
+        message.data = json.dumps(payload, ensure_ascii=False)
+        self._status_publisher.publish(message)
+        self.log.write(payload)
+
+    def _on_replan(self, envelope: dict[str, Any] | None) -> None:
+        if envelope is not None:
+            self._last_status = dict(envelope)
+        self.log.write({
+            'run_id': (self._last_status or {}).get('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': (self._last_status or {}).get('run_id'),
+                'state': 'replan_scene',
+                'reason': 'fresh scene snapshot for the external planner',
+                'scene_revision': envelope.get('scene_revision'),
+                'scene': envelope,
+                'timestamp': self._now(),
+            })
+
+    def _on_stop(self, reason: str) -> None:
+        self.motion.stop(reason)
+
+    def _on_motion_event(self, payload: dict[str, Any]) -> None:
+        self.log.write({**payload, 'event': payload.get('event', 'motion')})
+
+    def _notify_operator(self, reason: str, details: dict[str, Any]) -> bool:
+        self.get_logger().warning(f'operator required: {reason}')
+        self._publish_status({
+            'run_id': (self._last_status or {}).get('run_id'),
+            'state': 'operator_notification',
+            'reason': reason,
+            'details': details,
+            'timestamp': self._now(),
+        })
+        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)
+        if result.accepted:
+            self.get_logger().info(
+                f'run {run_id}: {result.code} ({result.message})'
+            )
+        else:
+            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 response
+
+    def _handle_cancel(self, request, response):
+        try:
+            try:
+                payload = json.loads(request.value)
+                run_id = (
+                    str(payload.get('run_id', ''))
+                    if isinstance(payload, dict) else str(payload)
+                )
+            except json.JSONDecodeError:
+                run_id = str(request.value)
+            result = self.engine.cancel(run_id)
+            response.success = bool(result.accepted)
+        except Exception as error:
+            self.get_logger().error(f'cancel failed: {error}')
+            response.success = False
+        return response
+
+    def _handle_get_status(self, request, response):
+        status = self.engine.get_status(request.run_id)
+        response.found = bool(status.found)
+        response.status_json = status.to_json()
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    exit_code = 0
+    try:
+        node = PlannerExecutorNode()
+        executor = MultiThreadedExecutor(num_threads=4)
+        executor.add_node(node)
+        executor.spin()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    except Exception as error:
+        print(f'[iiwa_llm_plan_executor] {error}')
+        exit_code = 1
+    finally:
+        if node is not None:
+            try:
+                node.motion.stop('node shutdown')
+            except Exception:
+                pass
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    if exit_code:
+        raise SystemExit(exit_code)
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py

@@ -440,6 +440,10 @@ class SkillRegistry:
         tool = node.object
         if context.mounted_tool == tool:
             return SkillOutcome('success', {'reused': True, 'tool': tool})
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_grab_tool', None)
+        if prepare is not None:
+            prepare(tool)
         self._ensure_contact(context, node)
         try:
             self._command(
@@ -447,11 +451,16 @@ class SkillRegistry:
             )
         except SkillError as error:
             context.ownership_unknown = True
+            restore = getattr(adapter, 'restore_tool', None)
+            if restore is not None:
+                try:
+                    restore(tool)
+                except Exception:
+                    pass
             return SkillOutcome('grasp_failed', {'reason': str(error)})
         context.mounted_tool = tool
         context.held_object = None
         context.ownership_unknown = False
-        adapter = self._adapter(context)
         if adapter is not None:
             try:
                 adapter.attach_tool(tool)
@@ -471,11 +480,24 @@ class SkillRegistry:
         command: str,
         tool: str,
     ) -> SkillOutcome:
+        adapter = self._adapter(context)
+        if node.object == 'cap':
+            prepare = getattr(adapter, 'prepare_grab_cap', None)
+        else:
+            prepare = getattr(adapter, 'prepare_grab_screw', None)
+        if prepare is not None:
+            prepare(node.object)
         contact = self._ensure_contact(context, node)
         try:
             self._command(context, command, node.object, event='grasp_failed')
         except SkillError as error:
             context.ownership_unknown = True
+            restore = getattr(adapter, 'restore_part', None)
+            if restore is not None:
+                try:
+                    restore(node.object)
+                except Exception:
+                    pass
             return SkillOutcome('grasp_failed', {'reason': str(error)})
         context.held_object = node.object
         context.mounted_tool = tool
@@ -483,7 +505,6 @@ class SkillRegistry:
         if node.object == 'cap':
             cap_pose = self._live_pose(context, node.object)
             context.cap_relative = contact.inverse().compose_relaxed(cap_pose)
-        adapter = self._adapter(context)
         if adapter is not None:
             try:
                 if node.object == 'cap':
@@ -587,6 +608,10 @@ class SkillRegistry:
                 ),
                 'moved': False,
             })
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_dock_tool', None)
+        if prepare is not None:
+            prepare(tool)
         if not context.approach_matches((tool, socket)):
             goal = self._destination_goal(context, tool, socket)
             safe = _above(goal, context.geometry.approach_gap)
@@ -615,6 +640,10 @@ class SkillRegistry:
             return SkillOutcome('failed', {'reason': 'hatch is not held'})
         target = node.target or 'hatch_seat'
         self._ensure_approach(context, node)
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_place_cap', None)
+        if prepare is not None:
+            prepare()
         if context.phase != 'contact':
             goal = self._destination_goal(context, 'cap', target)
             self._move(context, goal, 'lower')
@@ -655,6 +684,10 @@ class SkillRegistry:
         if context.mounted_tool != 'magnetic_tool':
             return SkillOutcome('failed', {'reason': 'magnetic_tool is not mounted'})
         self._ensure_approach(context, node)
+        adapter = self._adapter(context)
+        prepare = getattr(adapter, 'prepare_handoff_screw', None)
+        if prepare is not None:
+            prepare(screw)
         if context.phase != 'contact':
             goal = self._destination_goal(context, screw, hole)
             self._move(context, goal, 'lower')

src/iiwa_llm_plan_executor/test/test_engine.py

@@ -0,0 +1,384 @@
+"""Graph engine: admission, one-edge transitions, terminals and cancellation."""
+from __future__ import annotations
+
+import threading
+from pathlib import Path
+from types import SimpleNamespace
+
+import pytest
+
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.engine import GraphEngine
+from iiwa_llm_plan_executor.schema import parse_response
+from iiwa_scene_context.catalog import DEFAULT_CATALOG
+from iiwa_scene_context.snapshot import build_envelope, build_scene
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+SCENES = PACKAGE.parent / 'iiwa_experement' / 'config' / 'scenes.yaml'
+SAFE = {
+    'collision': 'operator_required',
+    'grasp_failed': 'operator_required',
+    'human_in_scene': 'operator_required',
+    'not_found': 'replan_required',
+    'failed': 'replan_required',
+    'not_ok': 'operator_required',
+    'timeout': 'operator_required',
+    'unknown': 'operator_required',
+}
+PROGRESS = {'success', 'ok', 'found', 'clear', 'empty', 'holding', 'placed'}
+
+
+class FakeContext:
+    def __init__(self, scene_json):
+        self.scene_json = scene_json
+        self.mounted_tool = None
+        self.held_object = None
+        self.scene_revision = 0
+        self.ownership_unknown = False
+
+
+class FakeSkills:
+    def __init__(self):
+        self.calls = []
+        self.events = {}
+        self.on_execute = None
+        self.block = False
+        self.started = threading.Event()
+        self.release = threading.Event()
+
+    def execute(self, node, context):
+        if self.on_execute is not None:
+            self.on_execute(node, context)
+        if self.block:
+            self.started.set()
+            self.release.wait(timeout=5.0)
+        self.calls.append(node.id)
+        return SimpleNamespace(event=self.events.get(node.id, 'success'), details={})
+
+
+def envelope(revision=1, stamp=5.0, experiment_id=1):
+    spec = load_scenario(experiment_id, SCENES)
+    tools = {tool.name: {'state': 'docked'} for tool in spec.tools}
+    scene = build_scene(
+        spec,
+        {
+            'version': revision,
+            'mounted_tool': None,
+            'held_object': None,
+            'ready': True,
+            'cap': {'state': spec.cap_state},
+            'tools': tools,
+            'screws': {},
+            'holes': {},
+        },
+        DEFAULT_CATALOG,
+    )
+    return build_envelope(
+        scene,
+        scene_revision=revision,
+        state_version=revision,
+        stamp=stamp,
+        experiment_id=experiment_id,
+    )
+
+
+def node(node_id, skill, obj='', target='', next_on=None):
+    events = DEFAULT_CATALOG.outcomes(skill)
+    next_on = next_on or {}
+    transitions = []
+    for event in events:
+        if event in next_on:
+            destination = next_on[event]
+        elif event in PROGRESS:
+            destination = 'completed'
+        else:
+            destination = SAFE.get(event, 'operator_required')
+        transitions.append({'event': event, 'next': destination})
+    return {
+        'id': node_id,
+        'skill': skill,
+        'object': obj,
+        'target': target,
+        'transitions': transitions,
+    }
+
+
+def graph(entry, *nodes):
+    return parse_response({
+        'status': 'ready',
+        'entry': entry,
+        'plan': list(nodes),
+    })
+
+
+STOP_GRAPH = lambda: graph(
+    'a',
+    node('a', 'stop', 'robot', next_on={'success': 'b'}),
+    node('b', 'stop', 'robot', next_on={'success': 'completed'}),
+)
+
+GRAB_GRAPH = lambda: graph(
+    'g',
+    node('g', 'grab', 'vacuum', next_on={'success': 't'}),
+    node('t', 'transfer', 'vacuum', 'work',
+         next_on={'success': 'completed'}),
+)
+
+
+def make_engine(skills=None, *, clock=5.0, provider=None, threaded=False,
+                max_steps=512, **kwargs):
+    skills = skills or FakeSkills()
+    clock_box = {'now': clock}
+    state = {'envelope': envelope(1, clock_box['now'])}
+    if provider is not None:
+        state['provider'] = provider
+    def default_provider():
+        return state['envelope']
+    events = {
+        'plans': [],
+        'statuses': [],
+        'replans': [],
+        'stops': [],
+    }
+    engine = GraphEngine(
+        state.get('provider', default_provider),
+        lambda env, scene: FakeContext(scene),
+        skills,
+        clock=lambda: clock_box['now'],
+        on_plan=lambda payload: events['plans'].append(payload),
+        on_status=lambda payload: events['statuses'].append(payload),
+        on_replan=lambda payload: events['replans'].append(payload),
+        on_stop=lambda reason: events['stops'].append(reason),
+        robot_fresh=lambda: (True, 'ok'),
+        threaded=threaded,
+        max_steps=max_steps,
+        **kwargs,
+    )
+    engine.update_readiness(scene_ready=True, ready_at=0.0, motion_ready=True)
+    return engine, skills, events, state, clock_box
+
+
+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)
+    assert rejected.accepted is False
+    assert rejected.code == 'not_ready'
+    assert 'start delay' in rejected.message
+
+    engine, _, _, _, clock = make_engine(clock=3.01)
+    accepted = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    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)
+    assert rejected.code == 'not_ready'
+
+
+def test_rejected_graphs_are_never_published_or_executed():
+    engine, skills, events, _, _ = make_engine()
+    blocked = engine.submit('r1', {'status': 'blocked', 'entry': '', 'plan': []}, 1)
+    assert blocked.code == 'blocked'
+    cap_without_vacuum = graph(
+        'g', node('g', 'grab', 'cap', next_on={'success': 'completed'}),
+    )
+    rejected = engine.submit('r2', cap_without_vacuum.as_dict(), 1)
+    assert rejected.accepted is False
+    assert rejected.code == 'replan_required'
+    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'
+
+
+def test_accepted_graph_is_published_once_before_the_first_motion():
+    engine, skills, events, _, _ = make_engine()
+    order = []
+
+    def on_plan(payload):
+        events['plans'].append(payload)
+        order.append(('plan', payload['run_id']))
+
+    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)
+    assert accepted.accepted
+    engine.run_accepted('r1')
+    assert order[0] == ('plan', 'r1')
+    assert order[1] == ('move', 'a')
+    assert len(events['plans']) == 1
+    status = engine.get_status('r1')
+    assert status.state == 'completed'
+    assert status.found is True
+
+
+def test_same_run_id_is_idempotent_and_a_second_run_is_busy():
+    engine, _, events, _, _ = make_engine()
+    first = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    assert first.accepted
+    duplicate = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    assert duplicate.accepted is True
+    assert duplicate.code == 'duplicate'
+    assert len(events['plans']) == 1
+    busy = engine.submit('r2', STOP_GRAPH().as_dict(), 1)
+    assert busy.accepted is False
+    assert busy.code == 'busy'
+    assert len(events['plans']) == 1
+
+
+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)
+    skills.events = {'a': 'success', 'b': 'success'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'completed'
+    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)
+
+
+def test_replan_returns_a_fresh_scene_and_operator_required_stops():
+    engine, skills, events, _, _ = make_engine()
+    replan_graph = graph(
+        'd',
+        node('d', 'detect', 'cap',
+             next_on={'found': 'replan_required'}),
+    )
+    engine.submit('r1', replan_graph.as_dict(), 1)
+    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
+
+    engine, skills, events, _, _ = make_engine()
+    operator_graph = graph(
+        'd',
+        node('d', 'detect', 'cap',
+             next_on={'found': 'operator_required'}),
+    )
+    engine.submit('r2', operator_graph.as_dict(), 1)
+    skills.events = {'d': 'found'}
+    engine.run_accepted('r2')
+    assert engine.get_status('r2').state == 'operator_required'
+
+
+def test_unknown_skill_outcome_stops_execution():
+    engine, skills, events, _, _ = make_engine()
+    engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    skills.events = {'a': 'bogus'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'failed'
+    assert 'unknown skill outcome' in status.reason
+    assert events['stops']
+
+
+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.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)
+    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():
+    engine, skills, events, state, _ = make_engine()
+    engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    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)
+
+    def own_change(node, context):
+        if node.id == 'g':
+            context.mounted_tool = 'vacuum'
+            state.update(envelope=envelope(2))
+
+    skills.on_execute = own_change
+    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_step_limit_stops_a_recovering_cycle():
+    engine, skills, _, _, _ = make_engine(max_steps=3)
+    cycle = graph(
+        'a',
+        node('a', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'b'}),
+        node('b', 'wait_scene_clear', 'workspace',
+             next_on={'clear': 'a'}),
+    )
+    engine.submit('r1', cycle.as_dict(), 1)
+    skills.events = {'a': 'clear', 'b': 'clear'}
+    engine.run_accepted('r1')
+    status = engine.get_status('r1')
+    assert status.state == 'failed'
+    assert 'step limit' in status.reason
+
+
+def test_cancellation_stops_the_active_graph_and_preserves_ownership():
+    engine, skills, events, _, _ = make_engine(threaded=True)
+    skills.block = True
+    skills.on_execute = (
+        lambda node, context: setattr(context, 'mounted_tool', node.object)
+        if node.skill == 'grab' else None
+    )
+    engine.submit('r1', GRAB_GRAPH().as_dict(), 1)
+    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']
+    skills.release.set()
+    assert engine.wait('r1', timeout=2.0)
+    status = engine.get_status('r1')
+    assert status.state == 'canceled'
+    assert status.mounted_tool == 'vacuum'
+
+
+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.run_accepted('r1')
+    assert engine.cancel('r1').accepted is False
+
+
+def test_status_query_marks_unknown_ids():
+    engine, _, _, _, _ = make_engine()
+    status = engine.get_status('nope')
+    assert status.found is False
+    assert status.state == 'unknown'
+
+
+def test_not_ready_or_unavailable_scene_is_rejected():
+    engine, _, _, state, _ = make_engine()
+    state['envelope'] = None
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    assert rejected.code == 'not_ready'
+
+
+def test_stale_scene_timestamp_is_rejected_before_execution():
+    engine, _, _, state, clock = make_engine(clock=10.0)
+    state['envelope'] = envelope(1, stamp=1.0)
+    rejected = engine.submit('r1', STOP_GRAPH().as_dict(), 1)
+    assert rejected.code == 'stale_scene'

src/iiwa_llm_plan_executor/test/test_node.py

@@ -0,0 +1,213 @@
+"""ROS wrapper helpers: request parsing, plan/status publication and logs."""
+from __future__ import annotations
+
+import json
+from pathlib import Path
+from types import SimpleNamespace
+
+from iiwa_experement.geometry import load_geometry
+from iiwa_experement.scenario import load_scenario
+from iiwa_llm_plan_executor.engine import AdmissionResult, RunStatus
+from iiwa_llm_plan_executor.node import (
+    PlannerExecutorNode,
+    RunLog,
+    _safe_name,
+    plan_qos,
+)
+from iiwa_msgs.srv import GetRunStatus
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+ASSETS = PACKAGE.parent / 'iiwa_experement' / 'config'
+
+
+class FakePublisher:
+    def __init__(self):
+        self.messages = []
+
+    def publish(self, message):
+        self.messages.append(message.data)
+
+
+class FakeEngine:
+    def __init__(self, admission=None, status=None, cancel=None):
+        self.admission = admission or AdmissionResult(True, 'accepted', 'ok', 'r1')
+        self.status = status
+        self.cancel_result = cancel
+        self.submitted = []
+        self.cancelled = []
+
+    def submit(self, run_id, graph, revision):
+        self.submitted.append((run_id, graph, revision))
+        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, (),
+        )
+
+    def cancel(self, run_id):
+        self.cancelled.append(run_id)
+        return self.cancel_result or SimpleNamespace(
+            accepted=True, message='cancel requested',
+        )
+
+
+def fake_node(*, engine=None):
+    node = object.__new__(PlannerExecutorNode)
+    node.engine = engine or FakeEngine()
+    node._plan_publisher = FakePublisher()
+    node._status_publisher = FakePublisher()
+    node._last_plan = None
+    node._last_status = None
+    node.log = RunLog('/tmp/iiwa-plan-log-test')
+    node.get_logger = lambda: SimpleNamespace(
+        info=lambda *a, **k: None,
+        warning=lambda *a, **k: None,
+        error=lambda *a, **k: None,
+    )
+    node._now = lambda: 10.0
+    node._lock = __import__('threading').RLock()
+    return node
+
+
+def test_plan_qos_is_transient_local_depth_one():
+    qos = plan_qos()
+    assert qos.durability.name == 'TRANSIENT_LOCAL'
+    assert qos.depth == 1
+
+
+def test_safe_name_and_run_log_are_machine_readable(tmp_path):
+    assert _safe_name('run/one 1') == 'run_one_1'
+    log = RunLog(str(tmp_path))
+    log.write({'run_id': 'r1', 'event': 'plan_accepted', 'scene_revision': 1})
+    log.write({'run_id': 'r1', 'event': 'completed', 'terminal': 'completed'})
+    lines = (tmp_path / 'r1.jsonl').read_text().splitlines()
+    assert [json.loads(line)['event'] for line in lines] == [
+        'plan_accepted', 'completed',
+    ]
+
+
+def test_plan_is_published_once_and_kept_for_late_subscribers():
+    node = fake_node()
+    payload = {'run_id': 'r1', 'scene_revision': 2, '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'
+
+
+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,
+        'response': {'status': 'ready', 'entry': 'a', 'plan': []},
+    }))
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_submit(node, request, response)
+    assert response.success is True
+    assert node.engine.submitted[0][0] == 'r1'
+    assert node.engine.submitted[0][1] == {
+        'status': 'ready', 'entry': 'a', 'plan': [],
+    }
+    assert node.engine.submitted[0][2] == 3
+
+
+def test_rejected_submit_returns_false_and_publishes_the_reason():
+    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,
+        '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']
+
+
+def test_invalid_submit_json_is_rejected_without_calling_the_engine():
+    node = fake_node()
+    request = SimpleNamespace(value='{broken')
+    response = SimpleNamespace(success=True)
+    PlannerExecutorNode._handle_submit(node, request, response)
+    assert response.success is False
+    assert node.engine.submitted == []
+
+
+def test_status_service_returns_found_and_json():
+    node = fake_node()
+    request = GetRunStatus.Request(run_id='r1')
+    response = GetRunStatus.Response()
+    PlannerExecutorNode._handle_get_status(node, request, response)
+    assert response.found is True
+    payload = json.loads(response.status_json)
+    assert payload['run_id'] == 'r1'
+    assert payload['state'] == 'completed'
+    assert payload['mounted_tool'] == 'vacuum'
+
+
+def test_cancel_accepts_json_and_plain_run_ids():
+    node = fake_node()
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_cancel(
+        node, SimpleNamespace(value=json.dumps({'run_id': 'r1'})), response,
+    )
+    assert response.success is True
+    response = SimpleNamespace(success=False)
+    PlannerExecutorNode._handle_cancel(
+        node, SimpleNamespace(value='r2'), response,
+    )
+    assert node.engine.cancelled == ['r1', 'r2']
+
+
+def test_scene_provider_enforces_freshness():
+    node = fake_node()
+    node._now = lambda: 10.0
+    node.scene_max_age = 2.0
+    node._latest_scene = {
+        'timestamp': 9.0, 'scene_revision': 1, 'scene_json': {},
+    }
+    assert PlannerExecutorNode._scene_provider(node) is not None
+    node._latest_scene['timestamp'] = 7.0
+    assert PlannerExecutorNode._scene_provider(node) is None
+
+
+def test_context_factory_sets_sources_and_inspection_configuration():
+    node = fake_node()
+    node.spec = load_scenario(20, ASSETS / 'scenes.yaml')
+    node.geometry = load_geometry(ASSETS / 'geometry.yaml')
+    node.adapter = 'adapter'
+    node.close_inspection = frozenset({'cap'})
+    node._frame_provider = lambda name: 1.0
+    node._notify_operator = lambda reason, details: True
+    context = PlannerExecutorNode._context_factory(
+        node, {'scene_revision': 4}, {},
+    )
+    assert context.scene_revision == 4
+    assert context.scene_adapter == 'adapter'
+    assert context.close_inspection_objects == frozenset({'cap'})
+    assert context.capture_frame('top') == 1.0
+    assert context.notify('x', {}) is True
+
+
+def test_robot_freshness_uses_the_joint_limit_age():
+    node = fake_node()
+    node.limits = SimpleNamespace(joint_state_max_age_sec=1.0)
+    node._last_joint = None
+    node._last_joint_at = None
+    assert PlannerExecutorNode._robot_fresh(node)[0] is False
+    node._last_joint = (0.0,) * 7
+    node._last_joint_at = 9.5
+    assert PlannerExecutorNode._robot_fresh(node)[0] is True
+    node._last_joint_at = 8.0
+    assert PlannerExecutorNode._robot_fresh(node)[0] is False