feat: launch grant experiment by id

src/iiwa_experement/doc/README.md

@@ -0,0 +1,121 @@
+# iiwa_experement
+
+Пакет запускает один из двадцати детерминированных опытов в Webots. Один
+запуск создаёт один свежий мир: шаблон `worlds/grant/iiwa-grant.wbt` копируется
+во временную директорию, в копии заменяются пути к PROTO, затем существующий
+`iiwa_bringup` запускает Webots, робота iiwa 7, контроллеры и камеры. После
+этого `scene_spawner` добавляет объекты выбранного сценария.
+
+Реализована только начальная сцена. Захват, вставка инструмента в `patron`,
+нанесение герметика, установка и затяжка винтов в пакет не входят.
+
+## Сборка
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install
+source install/setup.bash
+```
+
+## Запуск
+
+Обязательный параметр `experiment_id` принимает только целое число от 1 до 20:
+
+```bash
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=1
+```
+
+Проверенные примеры:
+
+```bash
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=6
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=11
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=16
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=19
+ros2 launch iiwa_experement experiment.launch.py experiment_id:=20
+```
+
+`experiment_id:=0`, `21`, `abc` или пустое значение останавливают launch до
+запуска Webots. Для другого опыта завершите текущий запуск (`Ctrl-C`) и
+запустите команду с новым номером: мир каждый раз создаётся заново.
+
+Дополнительные параметры:
+
+| Параметр | По умолчанию | Значение |
+| --- | --- | --- |
+| `rviz` | `false` | `true` запускает RViz вместе с Webots |
+| `scene_config` | установленный `config/scenes.yaml` | другой манифест сценариев |
+| `base_setting` | установленный `iiwa_config/config/setting.yaml` | базовые настройки для временной копии |
+
+Временные `iiwa-grant.wbt` и `setting.yaml` создаются в отдельной директории
+запуска и удаляются при его завершении.
+
+## Готовность сцены
+
+Сервис доступен с момента старта ноды, но сообщает успех только после
+последнего успешного вызова supervisor:
+
+```bash
+ros2 service call /iiwa_experement/scene_ready std_srvs/srv/Trigger {}
+```
+
+Успешный ответ содержит `success: true` и сообщение вида
+`experiment 11: 17 objects spawned`. Пока объекты добавляются или если спавн
+завершился ошибкой, `success` остаётся `false`, а в `message` находится причина
+и имя последнего объекта.
+
+## Состав сцен
+
+| Опыты | Люк | Лоток и винты | Рабочие инструменты |
+| --- | --- | --- | --- |
+| 1–5 | свободный | нет | присоска |
+| 6–9 | отсутствует | нет | герметик |
+| 10 | свободный | нет | герметик, присоска |
+| 11–13 | установлен | 10 винтов | магнитный |
+| 14–15 | установлен | 10 винтов | магнитный, затяжной |
+| 16–20 | свободный | 10 винтов | присоска, магнитный, герметик, затяжной |
+
+Во всех опытах `patron` уже находится на роботе: он включён в URDF через
+`urdf/tools/tool_active.xacro`. Рабочие инструменты лежат отдельно в четырёх
+гнёзах стойки. Свободный люк имеет физику; установленный люк зафиксирован.
+Винты и инструменты в начальной сцене статичны: будущий исполнитель должен
+освободить их перед захватом. Это исходное положение, а не реализованный
+механизм захвата.
+
+Координаты и наборы объектов заданы только в `config/scenes.yaml`; генератор
+не выбирает случайные позы. В опытах 11–20 всегда ровно десять винтов, даже
+если сценарий использует один, два или четыре. В опыте 19 лоток и все десять
+винтов смещены вместе.
+
+## Ресурсы
+
+- Шаблон мира и код: `src/iiwa_experement/`.
+- OBJ/MTL: `src/iiwa_description/objects/grant/`; они не копируются.
+- PROTO стола и ограждений: `src/iiwa_description/resource/protos/`.
+- Спецификации: `doc/experements.md`, `doc/experiment-scenes.md`.
+
+Старые `body_hatch*.wbt` и `GrantToolRack.proto` не восстанавливаются и не
+используются. Стойка собрана из `tool-magizine.obj` и отдельных объектов
+инструментов.
+
+## Проверки
+
+```bash
+colcon test --base-paths src --packages-select iiwa_experement
+colcon test-result --verbose
+```
+
+Тесты пакета проверяют:
+
+- установку мира, YAML, launch и документации;
+- все 20 сценариев, включая 10 винтов в опытах 11–20;
+- уникальные имена, позы и существование OBJ/MTL;
+- отказы при неверном ID, неизвестном инструменте, 9/11 винтах, повторяющемся
+  имени, винте вне лотка и несовпадении корпуса со стапелем;
+- отсутствие повторного вызова supervisor при тайм-ауте;
+- замену локальных PROTO на установленные абсолютные пути, отключение старого
+  случайного `object_spawner` и сохранение `patron` в URDF.
+
+Старый `src/iiwa_utils/test/test_body_hatch_worlds.py` относится к удалённым
+`worlds/grant/body_hatch*.wbt` и `GrantToolRack.proto`; это не тест нового
+пакета.

src/iiwa_experement/iiwa_experement/runtime_config.py

@@ -0,0 +1,130 @@
+"""Create the per-launch world and settings copies.
+
+The installed world template keeps the two local PROTO references for source
+tree use.  Webots must open a copy whose local EXTERNPROTO paths are absolute,
+because the temporary world lives outside the installed package.  The same
+launch also needs a settings copy that points at that world and disables the
+legacy random object spawner.
+"""
+from __future__ import annotations
+
+import re
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+EXPERIMENT_IDS = tuple(range(1, 21))
+LOCAL_PROTOS = (
+    'CollaborativeRobotTable.proto',
+    'WorkspaceLimiter.proto',
+)
+_EXTERNPROTO_RE = re.compile(r'(EXTERNPROTO\s+")([^"]+)(")')
+
+
+def validate_experiment_id(raw: str) -> int:
+    """Accept only a decimal experiment number in 1..20."""
+    text = str(raw).strip()
+    if not text.isdigit():
+        raise ValueError(f'experiment_id must be an integer in 1..20, got {raw!r}')
+    value = int(text)
+    if text != str(value):
+        raise ValueError(f'experiment_id must be a decimal integer, got {raw!r}')
+    if value not in EXPERIMENT_IDS:
+        raise ValueError(f'experiment_id {value} is outside 1..20')
+    return value
+
+
+def _absolute_proto(source: str, description_share: Path) -> str:
+    if source.startswith(('http://', 'https://')):
+        return source
+    proto_dir = description_share / 'resource' / 'protos'
+    candidate = proto_dir / Path(source).name
+    if not candidate.is_file():
+        raise FileNotFoundError(
+            f'EXTERNPROTO {source!r} was not found in {proto_dir}'
+        )
+    return str(candidate.resolve())
+
+
+def _materialize_world(
+    world_template: Path,
+    description_share: Path,
+    output_dir: Path,
+) -> Path:
+    if not world_template.is_file():
+        raise FileNotFoundError(f'world template does not exist: {world_template}')
+    text = world_template.read_text(encoding='utf-8')
+
+    def replace(match: re.Match[str]) -> str:
+        return match.group(1) + _absolute_proto(match.group(2), description_share) + match.group(3)
+
+    resolved = _EXTERNPROTO_RE.sub(replace, text)
+    for name in LOCAL_PROTOS:
+        expected = description_share / 'resource' / 'protos' / name
+        if str(expected.resolve()) not in resolved:
+            raise FileNotFoundError(
+                f'world template does not reference the installed {name}'
+            )
+    world_path = output_dir / world_template.name
+    world_path.write_text(resolved, encoding='utf-8')
+    return world_path
+
+
+def _mapping(value: Any, label: str) -> dict:
+    if not isinstance(value, dict):
+        raise ValueError(f'{label} must be a mapping')
+    return value
+
+
+def _materialize_setting(
+    base_setting: Path,
+    world_path: Path,
+    output_dir: Path,
+) -> Path:
+    if not base_setting.is_file():
+        raise FileNotFoundError(f'base settings file does not exist: {base_setting}')
+    data = _mapping(
+        yaml.safe_load(base_setting.read_text(encoding='utf-8')) or {},
+        'settings',
+    )
+    digital_twin = _mapping(data.get('digital_twin'), 'digital_twin')
+    webots = _mapping(digital_twin.get('webots'), 'digital_twin.webots')
+    objects = _mapping(webots.setdefault('objects', {}), 'digital_twin.webots.objects')
+    objects['enabled'] = False
+    objects['count'] = 0
+    webots['world'] = str(world_path.resolve())
+
+    tool = _mapping(data.setdefault('tool', {}), 'tool')
+    tool['active'] = 'patron'
+
+    setting_path = output_dir / 'setting.yaml'
+    setting_path.write_text(
+        yaml.safe_dump(data, sort_keys=False, allow_unicode=True),
+        encoding='utf-8',
+    )
+    return setting_path
+
+
+def prepare_runtime_files(
+    base_setting: Path | str,
+    world_template: Path | str,
+    description_share: Path | str,
+    output_dir: Path | str,
+) -> tuple[Path, Path]:
+    """Write an isolated world/settings pair and return ``(world, setting)``.
+
+    Both local PROTO references are resolved against the installed
+    ``iiwa_description`` share.  External https PROTO references are preserved
+    verbatim.  The base settings keep their cameras, robot transform and
+    controller paths; only the world path and the legacy object spawner change.
+    """
+    base_setting = Path(base_setting)
+    world_template = Path(world_template)
+    description_share = Path(description_share)
+    output_dir = Path(output_dir)
+    output_dir.mkdir(parents=True, exist_ok=True)
+    world_path = _materialize_world(world_template, description_share, output_dir)
+    setting_path = _materialize_setting(base_setting, world_path, output_dir)
+    return world_path, setting_path

src/iiwa_experement/launch/experiment.launch.py

@@ -0,0 +1,150 @@
+"""Launch one deterministic grant experiment in the Webots simulator."""
+from __future__ import annotations
+
+import shutil
+import tempfile
+from pathlib import Path
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+    RegisterEventHandler,
+)
+from launch.event_handlers import OnShutdown
+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 (
+    prepare_runtime_files,
+    validate_experiment_id,
+)
+
+
+BRINGUP_LAUNCH = ('launch', 'iiwa.launch.py')
+SCENE_SPAWNER = 'scene_spawner'
+
+
+def spawner_parameters(experiment_id: int, scene_config: str) -> dict:
+    """Parameters passed unchanged to the local scene_spawner node."""
+    return {
+        'experiment_id': int(experiment_id),
+        'scene_config': str(scene_config),
+    }
+
+
+def runtime_actions(
+    experiment_id: int,
+    scene_config: str,
+    setting_path: str,
+    rviz: str = 'false',
+    cleanup_dir: str | None = None,
+) -> list:
+    """Build the actions after validation has already succeeded."""
+    actions = [
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution([
+                    FindPackageShare('iiwa_bringup'), *BRINGUP_LAUNCH,
+                ])
+            ),
+            launch_arguments={
+                'simulate': 'true',
+                'rviz': str(rviz),
+                'setting': str(setting_path),
+            }.items(),
+        ),
+        Node(
+            package='iiwa_experement',
+            executable=SCENE_SPAWNER,
+            name=SCENE_SPAWNER,
+            output='screen',
+            parameters=[spawner_parameters(experiment_id, scene_config)],
+        ),
+    ]
+    if cleanup_dir is not None:
+        actions.append(RegisterEventHandler(
+            OnShutdown(on_shutdown=[OpaqueFunction(
+                function=_remove_directory,
+                kwargs={'directory': cleanup_dir},
+            )])
+        ))
+    return actions
+
+
+def _remove_directory(context, *args, directory: str, **kwargs):
+    del context, args, kwargs
+    shutil.rmtree(directory, ignore_errors=True)
+    return []
+
+
+def _setup(context, *args, **kwargs):
+    del args, kwargs
+    raw_id = LaunchConfiguration('experiment_id').perform(context)
+    try:
+        experiment_id = validate_experiment_id(raw_id)
+    except ValueError as error:
+        raise RuntimeError(f'Invalid experiment_id: {error}') from error
+
+    scene_config = str(LaunchConfiguration('scene_config').perform(context))
+    base_setting = str(LaunchConfiguration('base_setting').perform(context))
+    rviz = str(LaunchConfiguration('rviz').perform(context)).lower()
+
+    package_share = Path(get_package_share_directory('iiwa_experement'))
+    description_share = Path(get_package_share_directory('iiwa_description'))
+    world_template = package_share / 'worlds' / 'grant' / 'iiwa-grant.wbt'
+
+    output_dir = Path(tempfile.mkdtemp(prefix='iiwa_experement_'))
+    try:
+        world_path, setting_path = prepare_runtime_files(
+            base_setting=base_setting,
+            world_template=world_template,
+            description_share=description_share,
+            output_dir=output_dir,
+        )
+    except Exception:
+        shutil.rmtree(output_dir, ignore_errors=True)
+        raise
+
+    print(f'[iiwa_experement] experiment {experiment_id}: {world_path}')
+    return runtime_actions(
+        experiment_id=experiment_id,
+        scene_config=scene_config,
+        setting_path=str(setting_path),
+        rviz=rviz,
+        cleanup_dir=str(output_dir),
+    )
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'experiment_id',
+            default_value='',
+            description='Experiment number 1..20; required.',
+        ),
+        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; passed unchanged to scene_spawner.',
+        ),
+        DeclareLaunchArgument(
+            'base_setting',
+            default_value=PathJoinSubstitution([
+                FindPackageShare('iiwa_config'), 'config', 'setting.yaml',
+            ]),
+            description='Base settings used to build the temporary setting.yaml.',
+        ),
+        OpaqueFunction(function=_setup),
+    ])

src/iiwa_experement/test/test_launch.py

@@ -0,0 +1,95 @@
+"""Launch validation and action wiring."""
+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 validate_experiment_id
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+LAUNCH = PACKAGE / 'launch' / 'experiment.launch.py'
+
+
+def load_launch_module():
+    spec = importlib.util.spec_from_file_location('experiment_launch', LAUNCH)
+    module = importlib.util.module_from_spec(spec)
+    assert spec.loader is not None
+    spec.loader.exec_module(module)
+    return module
+
+
+@pytest.mark.parametrize('raw, expected', [('1', 1), ('20', 20), (' 11 ', 11)])
+def test_valid_experiment_ids(raw, expected):
+    assert validate_experiment_id(raw) == expected
+
+
+@pytest.mark.parametrize('raw', ['', '0', '21', 'abc', '-1', '1.0', '01', '  '])
+def test_invalid_experiment_ids_are_rejected(raw):
+    with pytest.raises(ValueError):
+        validate_experiment_id(raw)
+
+
+def test_runtime_actions_include_bringup_and_one_scene_spawner():
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=19,
+        scene_config='/tmp/custom scenes.yaml',
+        setting_path='/tmp/run/setting.yaml',
+        rviz='false',
+    )
+    includes = [a for a in actions if isinstance(a, IncludeLaunchDescription)]
+    nodes = [a for a in actions if isinstance(a, Node)]
+    assert len(includes) == 1
+    assert len(nodes) == 1
+    assert 'iiwa_bringup' in repr(includes[0].launch_description_source.location)
+    arguments = dict(includes[0].launch_arguments)
+    assert arguments['simulate'] == 'true'
+    assert arguments['rviz'] == 'false'
+    assert arguments['setting'] == '/tmp/run/setting.yaml'
+    node = nodes[0]
+    assert node.node_package == 'iiwa_experement'
+    assert node.node_executable == 'scene_spawner'
+    assert vars(node)['_Node__node_name'] == 'scene_spawner'
+    assert module.spawner_parameters(19, '/tmp/custom scenes.yaml') == {
+        'experiment_id': 19,
+        'scene_config': '/tmp/custom scenes.yaml',
+    }
+
+
+def test_runtime_actions_can_register_cleanup(tmp_path):
+    from launch.actions import RegisterEventHandler
+
+    module = load_launch_module()
+    actions = module.runtime_actions(
+        experiment_id=1,
+        scene_config='/tmp/scenes.yaml',
+        setting_path='/tmp/setting.yaml',
+        cleanup_dir=str(tmp_path / 'run'),
+    )
+    assert len(actions) == 3
+    assert isinstance(actions[-1], RegisterEventHandler)
+    target = tmp_path / 'run'
+    target.mkdir()
+    (target / 'world.wbt').write_text('temporary', encoding='utf-8')
+    module._remove_directory(None, directory=str(target))
+    assert not target.exists()
+
+
+def test_launch_description_declares_all_arguments_and_validates_in_opaque_function():
+    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', 'rviz', 'scene_config', 'base_setting'}
+    assert [substitution.text for substitution in arguments['experiment_id']] == ['']
+    assert [substitution.text for substitution in arguments['rviz']] == ['false']
+    assert 'scenes.yaml' in repr(arguments['scene_config'])
+    assert 'setting.yaml' in repr(arguments['base_setting'])
+    opaque = [e for e in description.entities if isinstance(e, OpaqueFunction)]
+    assert len(opaque) == 1

src/iiwa_experement/test/test_package_layout.py

@@ -42,8 +42,10 @@ def test_expected_resources_are_installed():
         'package.xml',
         'doc/experements.md',
         'doc/experiment-scenes.md',
+        'doc/README.md',
         'worlds/grant/iiwa-grant.wbt',
         'config/scenes.yaml',
+        'launch/experiment.launch.py',
     ):
         path = share / relative
         assert path.is_file(), path

src/iiwa_experement/test/test_runtime_config.py

@@ -0,0 +1,112 @@
+"""Temporary runtime files point at installed assets and keep patron."""
+from pathlib import Path
+
+import pytest
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from iiwa_experement.runtime_config import (
+    LOCAL_PROTOS,
+    prepare_runtime_files,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+CONFIG_SHARE = Path(get_package_share_directory('iiwa_config'))
+DESCRIPTION_SHARE = Path(get_package_share_directory('iiwa_description'))
+PACKAGE_SHARE = Path(get_package_share_directory('iiwa_experement'))
+BASE_SETTING = CONFIG_SHARE / 'config' / 'setting.yaml'
+WORLD_TEMPLATE = PACKAGE_SHARE / 'worlds' / 'grant' / 'iiwa-grant.wbt'
+
+
+def prepare(tmp_path, **overrides):
+    arguments = {
+        'base_setting': BASE_SETTING,
+        'world_template': WORLD_TEMPLATE,
+        'description_share': DESCRIPTION_SHARE,
+        'output_dir': tmp_path / 'run',
+    }
+    arguments.update(overrides)
+    return prepare_runtime_files(**arguments)
+
+
+def test_world_copy_resolves_only_local_protos(tmp_path):
+    world_path, _ = prepare(tmp_path)
+    text = world_path.read_text(encoding='utf-8')
+    for name in LOCAL_PROTOS:
+        absolute = (DESCRIPTION_SHARE / 'resource' / 'protos' / name).resolve()
+        assert absolute.is_file()
+        assert f'EXTERNPROTO "{absolute}"' in text
+    assert 'EXTERNPROTO "../../../iiwa_description' not in text
+    assert text.count('https://raw.githubusercontent.com/cyberbotics/webots/R2025a/') == 3
+    assert world_path == tmp_path / 'run' / 'iiwa-grant.wbt'
+
+
+def test_setting_copy_disables_random_objects_and_keeps_patron_and_camera(tmp_path):
+    world_path, setting_path = prepare(tmp_path)
+    data = yaml.safe_load(setting_path.read_text(encoding='utf-8'))
+    webots = data['digital_twin']['webots']
+    assert webots['world'] == str(world_path.resolve())
+    assert webots['objects']['enabled'] is False
+    assert webots['objects']['count'] == 0
+    assert data['tool']['active'] == 'patron'
+    assert webots['transform'] == '-0.25 0 0.79'
+    assert webots['rotation'] == '0 0 1 0'
+    assert webots['controller_timer'] == '50'
+    assert any(
+        str(camera).endswith('config/cameras/d455_top.yaml')
+        for camera in webots['cameras']
+    )
+    assert data['controller']['controller_path'].endswith('moveit/iiwa_controller.yaml')
+    assert data['digital_twin']['rviz']['config'].endswith('rviz_moveit.rviz')
+
+
+def test_prepared_setting_is_accepted_by_the_existing_loader(tmp_path):
+    _, setting_path = prepare(tmp_path)
+    from iiwa_utils.setting_loader import build_settings
+
+    settings = build_settings(str(setting_path), check_files=True)
+    assert settings.tool.active == 'patron'
+    assert settings.digital_twin.webots.objects.enabled is False
+    assert settings.digital_twin.webots.objects.count == 0
+    assert settings.digital_twin.webots.cameras
+    assert settings.digital_twin.webots.cameras[0].endswith('d455_top.yaml')
+
+
+def test_tool_active_xacro_and_generated_urdf_include_patron(tmp_path):
+    from iiwa_utils import converter
+    from iiwa_utils.setting_loader import build_settings
+
+    tool_active = DESCRIPTION_SHARE / 'urdf' / 'tools' / 'tool_active.xacro'
+    assert 'patron.xacro' in tool_active.read_text(encoding='utf-8')
+    _, setting_path = prepare(tmp_path)
+    settings = build_settings(str(setting_path), check_files=True)
+    urdf = converter.load_robot_description(
+        settings.robot.description,
+        settings.robot.name,
+        {
+            'simulate': 'true',
+            'initial_positions_file': settings.controller.moveit.initial_positions,
+            'joint_limits_file': settings.controller.moveit.joint_limits,
+        },
+    )
+    assert 'name="patron"' in urdf
+    assert 'link name="patron"' in urdf
+    assert 'patron.dae' in urdf
+
+
+def test_missing_world_template_is_reported(tmp_path):
+    with pytest.raises(FileNotFoundError, match='world template'):
+        prepare(tmp_path, world_template=tmp_path / 'missing.wbt')
+
+
+def test_missing_proto_is_reported(tmp_path):
+    empty_description = tmp_path / 'description'
+    (empty_description / 'resource' / 'protos').mkdir(parents=True)
+    with pytest.raises(FileNotFoundError, match='CollaborativeRobotTable.proto'):
+        prepare(tmp_path, description_share=empty_description)
+
+
+def test_missing_base_setting_is_reported(tmp_path):
+    with pytest.raises(FileNotFoundError, match='base settings'):
+        prepare(tmp_path, base_setting=tmp_path / 'missing.yaml')