Решение специально последовательное: это медленнее параллельного управления, зато уменьшает риск столкновения RMC1 и RMC2. Маршруты строятся BFS по графу ArUco, занятые клетки исключаются. Для Webots используется сетка 6 × 6, для физического поля из документации — 5 × 5. Движение замыкается по odometry, scan_front вызывает остановку перед препятствием, команды скорости сглаживаются. Соседняя парковочная точка RMC2 автоматически берётся из построенного пути к комплектации, отдельный параметр для неё не нужен.

Учебный сценарий симулятора

Дефолты образуют тренировочный сценарий на штатной геометрии module5.launch.py. Стартовые позиции, стеллаж и стол взяты из сцены; свободная точка комплектации выбрана нами для теста:

Объект	ArUco ID	Координата поля
старт RMC2	0	`(0, 0)`
старт RMC1	15	`(-2, 3)`
выбранный стеллаж	34	`(-5, 4)`
комплектация	16	`(-2, 4)`
подход RMC1 к стеллажу	10	`(-1, 4)`
полка сдачи	5	`(0, 5)`
подход RMC1 к полке	4	`(0, 4)`
другие стеллажи	31, 35	занятые клетки

Это только удобный тренировочный пресет. На зачётной попытке ID точек сообщает эксперт — передайте их аргументами, не редактируя код. Подход RMC1 должен быть соседней свободной клеткой с рабочей стороны стеллажа/полки.

Запуск

Терминал 1:

source /opt/ros/jazzy/setup.bash
source ~/ros2_ws/install/setup.bash
ros2 launch ar_webots_fms_ros2 module5.launch.py

Терминал 2. Используется уже настроенная venv модуля В с YOLO и numpy<2:

cd ~/scripts/mvch
git pull --ff-only
source /opt/ros/jazzy/setup.bash
source ~/ros2_ws/install/setup.bash
source module3/.venv/bin/activate
python3 module5/module5.py --target 1

Перед движением программа показывает все точки и маршруты. После проверки нажмите Enter. Классы цели: 1=hammer, 2=wrench, 3=pliers.

Пример с точками эксперта для Webots:

python3 module5/module5.py --target wrench \
  --rmc1-start 15 --rmc2-start 0 \
  --rack 34 --assembly 16 --assembly-approach 10 \
  --delivery 5 --delivery-approach 4 --blocked 31,35

На физическом поле обязательно добавьте --real и явно передайте все семь точек эксперта. Без них скрипт откажется запускаться, чтобы случайно не применить симуляторный пресет 0–35 к реальным ID 0–24:

python3 module5/module5.py --real --target wrench \
  --rmc1-start "$RMC1_START" --rmc2-start "$RMC2_START" \
  --rack "$RACK" --assembly "$ASSEMBLY" \
  --assembly-approach "$ASSEMBLY_APPROACH" \
  --delivery "$DELIVERY" --delivery-approach "$DELIVERY_APPROACH" \
  --blocked "$BLOCKED" --dry-run

Сначала присвойте переменным фактические ID и проверьте этот --dry-run. Затем уберите только --dry-run. Реальный профиль также выбирает камеру /RMC1/arm95/svcam/right/image/compressed, лифт 0.05 м и системное время. Суффикс камеры противоречит указанному в PDF типу sensor_msgs/Image, поэтому сначала выполните ros2 topic info ... -v; если фактический тип CompressedImage, добавьте --camera-compressed.

Только маршруты, без ROS и движения:

python3 module5/module5.py --dry-run --target pliers

Тест платформ и лифта без YOLO/MoveIt:

python3 module5/module5.py --target hammer --skip-arm

--auto убирает только начальное подтверждение. На первой тренировке его не использовать.

Аварийная остановка

В терминале скрипта доступны stop, resume, abort. Внешнее управление:

ros2 topic pub /mvch/module5/emergency_stop std_msgs/msg/Bool '{data: true}' --once
ros2 topic pub /mvch/module5/emergency_stop std_msgs/msg/Bool '{data: false}' --once
ros2 topic pub /mvch/module5/command std_msgs/msg/String '{data: abort}' --once

Ctrl+C и abort публикуют нулевую скорость обоим роверам. Штатный драйвер Webots не имеет аппаратного watchdog: после SIGKILL дополнительно остановите симуляцию вручную.

Калибровка ARM95

Захват переиспользует проверенный код module3/module3.py и веса module3/models/latest.pt. При подходе к полке RMC1 разворачивается к её центру, а точка выкладки задаётся относительно Base_link:

--drop-arm-x 0.55
--drop-arm-y 0.0
--drop-arm-z 0.220
--drop-arm-yaw 1.5708

Эти четыре значения обязательно проверить в симуляторе и затем на реальном поле: геометрия полки может отличаться. Если ARM95 пока не откалиброван, --skip-arm позволяет отдельно проверить всю логистику RMC1/RMC2.

Для расчёта координат детали нужен CameraInfo. Если его нет в списке топиков, передайте калибровку как --camera-fx, --camera-fy, --camera-cx, --camera-cy. Имена frame камеры и базы проверьте через TF; базу можно заменить аргументом --base-frame.

Логи и критерии

Логи сохраняются в module5/logs/module5_*.jsonl и не попадают в Git. В них есть старт/конец, маршруты, достигнутые маркеры, остановки по лидару, команды лифта, класс/confidence YOLO и этапы манипулятора.

Практически закрываются: все перемещения двух РМК, подъём/возврат стеллажа, захват/сдача детали, один запускаемый скрипт, безопасность, логирование, Git и README. Результат физического захвата, отсутствие падения детали и время до трёх минут подтверждаются только контрольным заездом.