157 lines
8.4 KiB
Markdown
157 lines
8.4 KiB
Markdown
# Модуль Д — складской кейс RMC1 + RMC2
|
|
|
|
Один запускаемый скрипт координирует два ровера в сценарии из конкурсного
|
|
задания:
|
|
|
|
1. RMC2 едет к стеллажу, поднимает его и отвозит в точку комплектации.
|
|
2. RMC2 опускает стеллаж и отъезжает на соседнюю свободную точку.
|
|
3. RMC1 подъезжает к стеллажу, YOLO находит заданную деталь, ARM95 берёт её.
|
|
4. RMC1 отвозит деталь к полке сдачи и кладёт её.
|
|
5. RMC2 возвращается под стеллаж и отвозит его на хранение.
|
|
6. Оба ровера последовательно возвращаются на свои стартовые точки.
|
|
7. В терминал и JSONL-лог выводится `MISSION_FINISHED`.
|
|
|
|
Решение специально последовательное: это медленнее параллельного управления,
|
|
зато уменьшает риск столкновения 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:
|
|
|
|
```bash
|
|
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`:
|
|
|
|
```bash
|
|
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:
|
|
|
|
```bash
|
|
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:
|
|
|
|
```bash
|
|
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 и движения:
|
|
|
|
```bash
|
|
python3 module5/module5.py --dry-run --target pliers
|
|
```
|
|
|
|
Тест платформ и лифта без YOLO/MoveIt:
|
|
|
|
```bash
|
|
python3 module5/module5.py --target hammer --skip-arm
|
|
```
|
|
|
|
`--auto` убирает только начальное подтверждение. На первой тренировке его не
|
|
использовать.
|
|
|
|
## Аварийная остановка
|
|
|
|
В терминале скрипта доступны `stop`, `resume`, `abort`. Внешнее управление:
|
|
|
|
```bash
|
|
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`:
|
|
|
|
```text
|
|
--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. Результат физического захвата, отсутствие падения детали и время до
|
|
трёх минут подтверждаются только контрольным заездом.
|