diff --git a/src/iiwa_experement/doc/README.md b/src/iiwa_experement/doc/README.md new file mode 100644 index 0000000..2ac05b8 --- /dev/null +++ b/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`; это не тест нового +пакета. diff --git a/src/iiwa_experement/iiwa_experement/runtime_config.py b/src/iiwa_experement/iiwa_experement/runtime_config.py new file mode 100644 index 0000000..3b009c8 --- /dev/null +++ b/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 diff --git a/src/iiwa_experement/launch/experiment.launch.py b/src/iiwa_experement/launch/experiment.launch.py new file mode 100644 index 0000000..7f137e6 --- /dev/null +++ b/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), + ]) diff --git a/src/iiwa_experement/test/test_launch.py b/src/iiwa_experement/test/test_launch.py new file mode 100644 index 0000000..348f55a --- /dev/null +++ b/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 diff --git a/src/iiwa_experement/test/test_package_layout.py b/src/iiwa_experement/test/test_package_layout.py index c356f15..a930548 100644 --- a/src/iiwa_experement/test/test_package_layout.py +++ b/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 diff --git a/src/iiwa_experement/test/test_runtime_config.py b/src/iiwa_experement/test/test_runtime_config.py new file mode 100644 index 0000000..c26e19b --- /dev/null +++ b/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')