feat(llm-plan-executor): add end-to-end planner launch and operator guide

src/iiwa_experement/doc/README.md

@@ -11,7 +11,10 @@
 передаёт винты виртуальному оператору и выполняет имитацию затяжки затяжным
 инструментом. Винты не вставляются: после передачи отверстие остаётся пустым,
 а `torque_tool` подходит к отверстию, поворачивается вокруг своей оси и
-отходит. Режим `view` (по умолчанию) оставляет сцену неподвижной.
+отходит. Режим `planner` готовит ту же записываемую сцену (supervisor
+принимает захваты), но **не** запускает `reference_runner`: готовый граф
+передаётся снаружи в `iiwa_llm_plan_executor`. Режим `view` (по умолчанию)
+оставляет сцену неподвижной.
 
 ## Сборка
 

src/iiwa_llm_plan_executor/config/planner.yaml

@@ -0,0 +1,15 @@
+# Defaults for `ros2 launch iiwa_llm_plan_executor planner.launch.py`.
+# Qwen3 is called outside this project; this file only configures Webots and
+# the two new nodes.
+start_delay_sec: 3.0
+scene_max_age_sec: 2.0
+max_steps: 512
+scene_freshness_sec: 2.0
+close_inspection_objects:
+  - cap
+top_camera_topic: /d455_top/image_raw
+hand_camera_topic: /camera/color
+frame_timeout_sec: 2.0
+trace_dir: ~/.ros/iiwa_llm_plan_executor/runs
+planning_group: iiwa_arm
+pose_link: tcp

src/iiwa_llm_plan_executor/doc/README.md

@@ -0,0 +1,102 @@
+# Executing external Qwen3 graphs in Webots
+
+This package accepts a **ready-made** JSON graph.  It does not call Qwen3, does
+not evaluate the model and does not compare runs with a reference sequence.
+The experiment number selects only the initial Webots scene.
+
+## Start
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+ros2 launch iiwa_llm_plan_executor planner.launch.py experiment_id:=20
+```
+
+The launch starts the standard `iiwa_bringup` simulation, the writable Grant
+supervisor (`mode=planner`), the scene context and the graph executor.  It
+never starts `reference_runner` and never starts a second `iiwa_web` server:
+the web server already comes from `iiwa_bringup` when `web.enabled` is true in
+`cobot-setting.yaml`.  For a ROS-only run set `web.enabled: false` in your
+settings before launch.
+
+Useful arguments: `rviz:=true`, `config:=/path/planner.yaml`.
+`simulate` must remain `true`; the physical FRI robot is not connected.
+
+## Read the scene and submit a graph
+
+Wait until the launch log shows the scene is ready (the snapshot is only
+served after `/iiwa_experement/scene_ready`).
+
+ROS:
+
+```bash
+ros2 service call /iiwa_scene_context/get_scene std_srvs/srv/Trigger {}
+ros2 topic echo /iiwa_scene_context/scene_json --qos-durability transient_local
+```
+
+Give `scene_json` and your instruction to Qwen3 outside this project.  Submit
+the answer, the `scene_revision` from the same snapshot and a client-chosen
+`run_id`:
+
+```bash
+ros2 service call /iiwa_llm_plan_executor/submit webots_ros2_msgs/srv/SetString \
+  "{value: '{\"run_id\":\"demo-1\",\"scene_revision\":1,\"response\":{\"status\":\"ready\",\"entry\":\"detect\",\"plan\":[...]}}'}"
+
+ros2 topic echo /iiwa_llm_plan_executor/plan --qos-durability transient_local
+ros2 topic echo /iiwa_llm_plan_executor/status
+```
+
+The accepted graph is published **unchanged** in
+`/iiwa_llm_plan_executor/plan` before the first movement.  A rejected graph
+never appears there; read the reason from
+`/iiwa_llm_plan_executor/status` or:
+
+```bash
+ros2 service call /iiwa_llm_plan_executor/get_status iiwa_msgs/srv/GetRunStatus \
+  "{run_id: demo-1}"
+ros2 service call /iiwa_llm_plan_executor/cancel webots_ros2_msgs/srv/SetString \
+  "{value: '{\"run_id\":\"demo-1\"}'}"
+```
+
+HTTP (existing `iiwa_web` server; default port 8007):
+
+```bash
+curl http://localhost:8007/planner/scene
+curl -X POST http://localhost:8007/planner/runs \
+  -H 'Content-Type: application/json' \
+  -d '{"run_id":"demo-1","scene_revision":1,"response":"<Qwen JSON string>"}'
+curl http://localhost:8007/planner/runs/demo-1
+curl -X POST http://localhost:8007/planner/runs/demo-1/cancel
+```
+
+The same four operations are MCP tools built from the same router
+(`get_scene_planner_scene_get`, `submit_run_planner_runs_post`,
+`get_run_planner_runs`, `cancel_run_planner_runs`).
+
+## Nodes and terminals
+
+Only one graph is active per robot.  Repeating the same `run_id` is
+idempotent; a different `run_id` while active is rejected (`busy`).
+
+Status events contain the run id, node id, skill, result event, the chosen
+next node, the scene revision, the mounted tool, the held object and, at the
+end, the terminal.  Terminals:
+
+* `completed` — the submitted graph and its checked postconditions finished.
+* `replan_required` — the graph stopped and a fresh snapshot was emitted; the
+  external client decides whether to call Qwen3 again.
+* `operator_required` — a human decision is needed; the robot is stopped.
+* `failed` / `canceled` — safety stop (stale state, limits, collision, cancel,
+  step limit); the grasp state is preserved in the status.
+
+The JSONL log with every event is written to
+`~/.ros/iiwa_llm_plan_executor/runs/<run_id>.jsonl`.
+
+## Safety scope
+
+The executor checks J7 (±90° around `work`) and the downward TCP tilt (≤10°)
+on **every** planned point, seeds IK from current joints, enforces these as
+MoveIt path constraints, checks scene/joint freshness and verifies the final
+pose.  It never falls back to an unconstrained `MoveToPose` goal.  This is a
+Webots verification, not a physical-robot validation and not a Qwen3 quality
+evaluation.

src/iiwa_llm_plan_executor/launch/planner.launch.py

@@ -0,0 +1,209 @@
+"""Launch one Webots scene with the scene context and the graph executor.
+
+The experiment only selects the initial scene.  The reference runner is never
+started in this mode; the graph arrives from outside through
+``/iiwa_llm_plan_executor/submit``.
+"""
+from __future__ import annotations
+
+from pathlib import Path
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from iiwa_experement.runtime_config import (
+    validate_experiment_id,
+    validate_mode,
+)
+
+
+EXPERIMENT_LAUNCH = ('launch', 'experiment.launch.py')
+PLANNER_MODE = 'planner'
+DEFAULT_CONFIG = {
+    'start_delay_sec': 3.0,
+    'scene_max_age_sec': 2.0,
+    'max_steps': 512,
+    'scene_freshness_sec': 2.0,
+    'close_inspection_objects': ['cap'],
+    'top_camera_topic': '/d455_top/image_raw',
+    'hand_camera_topic': '/camera/color',
+    'frame_timeout_sec': 2.0,
+    'trace_dir': '~/.ros/iiwa_llm_plan_executor/runs',
+    'planning_group': 'iiwa_arm',
+    'pose_link': 'tcp',
+}
+
+
+def load_config(path: Path | str | None) -> dict:
+    data = dict(DEFAULT_CONFIG)
+    if path is not None:
+        source = Path(path)
+        if source.is_file():
+            loaded = yaml.safe_load(source.read_text(encoding='utf-8')) or {}
+            if isinstance(loaded, dict):
+                data.update(loaded)
+    return data
+
+
+def validate_arguments(
+    experiment_id_raw: str,
+    mode_raw: str,
+    simulate_raw: str,
+) -> tuple[int, str]:
+    """Validate everything before Webots is started."""
+    experiment_id = validate_experiment_id(experiment_id_raw)
+    mode = validate_mode(mode_raw)
+    if mode != PLANNER_MODE:
+        raise ValueError(
+            f'mode must be {PLANNER_MODE!r} for the graph executor, got {mode!r}'
+        )
+    simulate = str(simulate_raw).strip().lower()
+    if simulate not in ('true', '1', 'yes'):
+        raise ValueError(
+            'this executor stage only supports Webots (simulate:=true); '
+            'the physical FRI robot is not connected'
+        )
+    return experiment_id, mode
+
+
+def runtime_actions(
+    *,
+    experiment_id: int,
+    scene_config: str,
+    base_setting: str,
+    rviz: str,
+    config: dict,
+) -> list:
+    experiment = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([
+            FindPackageShare('iiwa_experement'), *EXPERIMENT_LAUNCH,
+        ])),
+        launch_arguments={
+            'experiment_id': str(experiment_id),
+            'mode': PLANNER_MODE,
+            'scene_config': str(scene_config),
+            'base_setting': str(base_setting),
+            'rviz': str(rviz),
+        }.items(),
+    )
+    scene_context = Node(
+        package='iiwa_scene_context',
+        executable='scene_context',
+        name='iiwa_scene_context',
+        output='screen',
+        parameters=[{
+            'experiment_id': int(experiment_id),
+            'scene_config': str(scene_config),
+            'freshness_sec': float(config.get('scene_freshness_sec', 2.0)),
+            'use_sim_time': True,
+        }],
+    )
+    executor = Node(
+        package='iiwa_llm_plan_executor',
+        executable='llm_plan_executor',
+        name='iiwa_llm_plan_executor',
+        output='screen',
+        parameters=[{
+            'experiment_id': int(experiment_id),
+            'scene_config': str(scene_config),
+            'start_delay_sec': float(config.get('start_delay_sec', 3.0)),
+            'scene_max_age_sec': float(config.get('scene_max_age_sec', 2.0)),
+            'max_steps': int(config.get('max_steps', 512)),
+            'close_inspection_objects': list(
+                config.get('close_inspection_objects', ['cap'])
+            ),
+            'top_camera_topic': str(
+                config.get('top_camera_topic', '/d455_top/image_raw')
+            ),
+            'hand_camera_topic': str(
+                config.get('hand_camera_topic', '/camera/color')
+            ),
+            'frame_timeout_sec': float(config.get('frame_timeout_sec', 2.0)),
+            'trace_dir': str(
+                config.get('trace_dir', '~/.ros/iiwa_llm_plan_executor/runs')
+            ),
+            'planning_group': str(config.get('planning_group', 'iiwa_arm')),
+            'pose_link': str(config.get('pose_link', 'tcp')),
+            'use_sim_time': True,
+        }],
+    )
+    return [experiment, scene_context, executor]
+
+
+def _setup(context, *args, **kwargs):
+    del args, kwargs
+    raw_id = LaunchConfiguration('experiment_id').perform(context)
+    raw_mode = LaunchConfiguration('mode').perform(context)
+    raw_simulate = LaunchConfiguration('simulate').perform(context)
+    try:
+        experiment_id, _ = validate_arguments(raw_id, raw_mode, raw_simulate)
+    except ValueError as error:
+        raise RuntimeError(f'Invalid planner launch arguments: {error}') from error
+    config_path = LaunchConfiguration('config').perform(context)
+    config = load_config(config_path)
+    print(
+        f'[iiwa_llm_plan_executor] experiment {experiment_id} planner mode: '
+        f'no reference sequence; waiting for an external graph'
+    )
+    return runtime_actions(
+        experiment_id=experiment_id,
+        scene_config=str(LaunchConfiguration('scene_config').perform(context)),
+        base_setting=str(LaunchConfiguration('base_setting').perform(context)),
+        rviz=str(LaunchConfiguration('rviz').perform(context)).lower(),
+        config=config,
+    )
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'experiment_id',
+            default_value='',
+            description='Experiment 1..20; selects only the initial scene.',
+        ),
+        DeclareLaunchArgument(
+            'mode',
+            default_value='planner',
+            description='Must remain planner for externally supplied graphs.',
+        ),
+        DeclareLaunchArgument(
+            'simulate',
+            default_value='true',
+            description='Must remain true; the physical robot is not connected.',
+        ),
+        DeclareLaunchArgument(
+            'rviz',
+            default_value='false',
+            description='true = start RViz together with Webots.',
+        ),
+        DeclareLaunchArgument(
+            'scene_config',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_experement'), 'config', 'scenes.yaml',
+            ]),
+            description='Scenario manifest used to build the initial scene.',
+        ),
+        DeclareLaunchArgument(
+            'base_setting',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_config'), 'config', 'setting.yaml',
+            ]),
+            description='Base settings for the temporary setting.yaml.',
+        ),
+        DeclareLaunchArgument(
+            'config',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_llm_plan_executor'),
+                'config',
+                'planner.yaml',
+            ]),
+            description='Executor defaults: delays, limits, cameras, logging.',
+        ),
+        OpaqueFunction(function=_setup),
+    ])

src/iiwa_llm_plan_executor/test/test_launch.py

@@ -0,0 +1,139 @@
+"""Launch validation and wiring for the planner scene."""
+from __future__ import annotations
+
+import importlib.util
+from pathlib import Path
+
+import pytest
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch_ros.actions import Node
+
+from iiwa_experement.runtime_config import EXPERIMENT_IDS, validate_experiment_id
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LAUNCH = PACKAGE / 'launch' / 'planner.launch.py'
+CONFIG = PACKAGE / 'config' / 'planner.yaml'
+
+
+def load_launch_module():
+    spec = importlib.util.spec_from_file_location('planner_launch', LAUNCH)
+    module = importlib.util.module_from_spec(spec)
+    assert spec.loader is not None
+    spec.loader.exec_module(module)
+    return module
+
+
+def flat_parameters(node: Node):
+    def flatten(value):
+        if hasattr(value, 'text'):
+            return [value.text]
+        if isinstance(value, (list, tuple)):
+            result = []
+            for item in value:
+                result.extend(flatten(item))
+            return result
+        return [value]
+
+    values = []
+    for parameter_set in node._Node__parameters:
+        for value in parameter_set.values():
+            values.extend(flatten(value))
+    return values
+
+
+def test_valid_experiment_ids_and_planner_mode():
+    module = load_launch_module()
+    for experiment_id in EXPERIMENT_IDS:
+        parsed, mode = module.validate_arguments(
+            str(experiment_id), 'planner', 'true',
+        )
+        assert parsed == experiment_id
+        assert mode == 'planner'
+    assert validate_experiment_id(' 20 ') == 20
+
+
+@pytest.mark.parametrize('raw', ['', '0', '21', 'abc', '1.0'])
+def test_invalid_experiment_ids_fail_before_webots(raw):
+    module = load_launch_module()
+    with pytest.raises(ValueError):
+        module.validate_arguments(raw, 'planner', 'true')
+
+
+def test_unknown_mode_or_physical_robot_is_rejected_before_webots():
+    module = load_launch_module()
+    with pytest.raises(ValueError, match='mode'):
+        module.validate_arguments('20', 'execute', 'true')
+    with pytest.raises(ValueError, match='simulate'):
+        module.validate_arguments('20', 'planner', 'false')
+
+
+def test_runtime_actions_start_scene_context_and_executor_only():
+    module = load_launch_module()
+    config = module.load_config(CONFIG)
+    actions = module.runtime_actions(
+        experiment_id=20,
+        scene_config='/tmp/scenes.yaml',
+        base_setting='/tmp/setting.yaml',
+        rviz='false',
+        config=config,
+    )
+    includes = [action for action in actions if isinstance(action, IncludeLaunchDescription)]
+    nodes = [action for action in actions if isinstance(action, Node)]
+    assert len(includes) == 1
+    assert len(nodes) == 2
+    arguments = dict(includes[0].launch_arguments)
+    assert arguments['experiment_id'] == '20'
+    assert arguments['mode'] == 'planner'
+    assert arguments['scene_config'] == '/tmp/scenes.yaml'
+    assert arguments['base_setting'] == '/tmp/setting.yaml'
+
+    packages = {node.node_package for node in nodes}
+    executables = {node.node_executable for node in nodes}
+    assert packages == {'iiwa_scene_context', 'iiwa_llm_plan_executor'}
+    assert executables == {'scene_context', 'llm_plan_executor'}
+    assert 'reference_runner' not in executables
+    assert 'iiwa_web' not in packages
+
+    scene_node = next(node for node in nodes if node.node_executable == 'scene_context')
+    executor_node = next(
+        node for node in nodes if node.node_executable == 'llm_plan_executor'
+    )
+    scene_values = flat_parameters(scene_node)
+    executor_values = flat_parameters(executor_node)
+    assert 20 in scene_values
+    assert 20 in executor_values
+    assert 3.0 in executor_values
+    assert 512 in executor_values
+    assert True in executor_values
+    assert any('iiwa_arm' in str(value) for value in executor_values)
+
+
+def test_config_contains_the_documented_defaults():
+    module = load_launch_module()
+    config = module.load_config(CONFIG)
+    assert config['start_delay_sec'] == 3.0
+    assert config['scene_max_age_sec'] == 2.0
+    assert config['max_steps'] == 512
+    assert config['close_inspection_objects'] == ['cap']
+    assert config['trace_dir'].endswith('iiwa_llm_plan_executor/runs')
+
+
+def test_launch_description_declares_all_arguments_and_validates_in_opaque():
+    module = load_launch_module()
+    description = module.generate_launch_description()
+    arguments = {
+        entity.name: entity.default_value
+        for entity in description.entities
+        if isinstance(entity, DeclareLaunchArgument)
+    }
+    assert set(arguments) == {
+        'experiment_id', 'mode', 'simulate', 'rviz', 'scene_config',
+        'base_setting', 'config',
+    }
+    assert [item.text for item in arguments['experiment_id']] == ['']
+    assert [item.text for item in arguments['mode']] == ['planner']
+    assert [item.text for item in arguments['simulate']] == ['true']
+    assert len([
+        entity for entity in description.entities if isinstance(entity, OpaqueFunction)
+    ]) == 1