grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

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

Browse Source
This commit is contained in:
Даниил Грабарь
2026-11-11 00:31:49 +10:00
parent 5dc8d651f3
commit e7d280c11c
5 changed files with 469 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_llm_plan_executor/config/planner.yaml
+15
View File
@@ -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
+102
View File
@@ -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
+209
View File
@@ -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
+139
View File
@@ -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