From 1e8856387b8c3c2c47cca86a4e1a385f9f382120 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 11 Nov 2026 00:26:37 +1000 Subject: [PATCH] feat(llm-plan-executor): add graph engine and ROS submit/status/cancel node --- .../iiwa_experement/planning_scene.py | 23 +- src/iiwa_llm_plan_executor/docs/interfaces.md | 25 + .../iiwa_llm_plan_executor/engine.py | 585 ++++++++++++++ .../iiwa_llm_plan_executor/moveit_backend.py | 3 + .../iiwa_llm_plan_executor/node.py | 751 ++++++++++++++++++ .../iiwa_llm_plan_executor/skills.py | 37 +- .../test/test_engine.py | 384 +++++++++ src/iiwa_llm_plan_executor/test/test_node.py | 213 +++++ src/iiwa_msgs/CMakeLists.txt | 1 + src/iiwa_msgs/srv/GetRunStatus.srv | 4 + 10 files changed, 2017 insertions(+), 9 deletions(-) create mode 100644 src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py create mode 100644 src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py create mode 100644 src/iiwa_llm_plan_executor/test/test_engine.py create mode 100644 src/iiwa_llm_plan_executor/test/test_node.py create mode 100644 src/iiwa_msgs/srv/GetRunStatus.srv diff --git a/src/iiwa_experement/iiwa_experement/planning_scene.py b/src/iiwa_experement/iiwa_experement/planning_scene.py index 14e5c57..c2a7306 100644 --- a/src/iiwa_experement/iiwa_experement/planning_scene.py +++ b/src/iiwa_experement/iiwa_experement/planning_scene.py @@ -13,7 +13,7 @@ import time import xml.etree.ElementTree as ET from dataclasses import dataclass from pathlib import Path -from typing import Iterable, Sequence +from typing import Callable, Iterable, Sequence import numpy as np import rclpy @@ -465,6 +465,7 @@ class PlanningSceneBridge: node: Node | None = None, use_sim_time: bool = True, srdf_path: str | None = None, + spin_fn: Callable[[float], None] | None = None, ) -> None: self._owns_node = node is None if node is None: @@ -473,6 +474,7 @@ class PlanningSceneBridge: 'use_sim_time', value=use_sim_time, )]) self.node = node + self._spin_fn = spin_fn self.geometry: GeometrySpec | None = None self.spec: ScenarioSpec | None = None self.base_to_world: Transform | None = None @@ -553,10 +555,17 @@ class PlanningSceneBridge: attached[entry.object.id] = copy.deepcopy(entry) scene.robot_state.attached_collision_objects = list(attached.values()) + def _spin_once(self, timeout_sec: float) -> None: + """Let the owning executor spin, or spin locally in the reference run.""" + if self._spin_fn is not None: + self._spin_fn(timeout_sec) + return + rclpy.spin_once(self.node, timeout_sec=timeout_sec) + def _spin(self, seconds: float = 0.05) -> None: end = time.monotonic() + seconds while time.monotonic() < end: - rclpy.spin_once(self.node, timeout_sec=0.01) + self._spin_once(0.01) def _publish(self, scene: PlanningScene) -> None: self._seen_world.clear() @@ -575,7 +584,7 @@ class PlanningSceneBridge: future = self._get_scene.call_async(request) end = time.monotonic() + 3.0 while time.monotonic() < end and not future.done(): - rclpy.spin_once(self.node, timeout_sec=0.05) + self._spin_once(0.05) if future.done(): try: matrix = future.result().scene.allowed_collision_matrix @@ -639,7 +648,7 @@ class PlanningSceneBridge: ) end = time.monotonic() + 0.5 while time.monotonic() < end: - rclpy.spin_once(self.node, timeout_sec=0.02) + self._spin_once(0.02) current = self._latest_joint_state if current is None: continue @@ -672,7 +681,7 @@ class PlanningSceneBridge: future = self._get_scene.call_async(request) end = time.monotonic() + 1.0 while time.monotonic() < end and not future.done(): - rclpy.spin_once(self.node, timeout_sec=0.05) + self._spin_once(0.05) if not future.done(): continue try: @@ -859,7 +868,7 @@ class PlanningSceneBridge: future = self._get_scene.call_async(request) end = time.monotonic() + timeout_sec while time.monotonic() < end and not future.done(): - rclpy.spin_once(self.node, timeout_sec=0.05) + self._spin_once(0.05) if not future.done(): return None try: @@ -891,7 +900,7 @@ class PlanningSceneBridge: last_world: set[str] = set() last_attached: set[str] = set() while time.monotonic() < end: - rclpy.spin_once(self.node, timeout_sec=0.05) + self._spin_once(0.05) # 1. Every expected id must have appeared in a monitored scene # (published by the MoveItPy or move_group monitor). served = ( diff --git a/src/iiwa_llm_plan_executor/docs/interfaces.md b/src/iiwa_llm_plan_executor/docs/interfaces.md index d4c7bc9..84e7b68 100644 --- a/src/iiwa_llm_plan_executor/docs/interfaces.md +++ b/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` diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/engine.py new file mode 100644 index 0000000..82a14e6 --- /dev/null +++ b/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 diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py index d852db7..703a178 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py +++ b/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 diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py new file mode 100644 index 0000000..895fc7f --- /dev/null +++ b/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() diff --git a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py b/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py index dae17bb..0b36efa 100644 --- a/src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py +++ b/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') diff --git a/src/iiwa_llm_plan_executor/test/test_engine.py b/src/iiwa_llm_plan_executor/test/test_engine.py new file mode 100644 index 0000000..12584c5 --- /dev/null +++ b/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' diff --git a/src/iiwa_llm_plan_executor/test/test_node.py b/src/iiwa_llm_plan_executor/test/test_node.py new file mode 100644 index 0000000..1a28b66 --- /dev/null +++ b/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 diff --git a/src/iiwa_msgs/CMakeLists.txt b/src/iiwa_msgs/CMakeLists.txt index 190078a..d59bb0d 100644 --- a/src/iiwa_msgs/CMakeLists.txt +++ b/src/iiwa_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/MotionPriority.srv" "srv/MoveToPose.srv" "srv/MoveToNamedPose.srv" + "srv/GetRunStatus.srv" "action/MoveToPose.action" "action/MoveToJoints.action" DEPENDENCIES geometry_msgs action_msgs diff --git a/src/iiwa_msgs/srv/GetRunStatus.srv b/src/iiwa_msgs/srv/GetRunStatus.srv new file mode 100644 index 0000000..9c7435b --- /dev/null +++ b/src/iiwa_msgs/srv/GetRunStatus.srv @@ -0,0 +1,4 @@ +string run_id +--- +bool found +string status_json