refactor(module2): simplify ArUco mission
This commit is contained in:
@@ -76,97 +76,68 @@ source ~/ros2_ws/install/setup.bash
|
||||
python3 module2/module_b.py --target 14 --sim
|
||||
```
|
||||
|
||||
1. Робот должен стоять **центром над известной меткой**. Начальная ориентация любая:
|
||||
она определяется по TF метки. Программа ждёт odometry, lidar, TF и ArUco.
|
||||
2. Программа печатает `ROUTE`. Эксперт фиксирует маршрут; **Enter** разрешает движение.
|
||||
3. После `TARGET_REACHED` робот стоит. Выставьте препятствие штатным инструментом симулятора.
|
||||
4. **Enter** строит и выводит обратный маршрут с учётом видимого препятствия.
|
||||
5. Ещё **Enter** разрешает возврат. Успех — `MISSION_FINISHED`.
|
||||
1. Поставьте РМК-2 центром над любой меткой. Код сам определит стартовый ID и
|
||||
начальную ориентацию.
|
||||
2. Код заранее выведет кратчайшие маршруты к цели и обратно. **Enter** запускает
|
||||
движение к цели.
|
||||
3. После `TARGET_REACHED` поставьте динамическое препятствие и нажмите **Enter**.
|
||||
4. Код ещё раз выведет обратный маршрут. Следующий **Enter** запускает возврат.
|
||||
5. Если лидар увидит преграду на ребре, ровер вернётся к предыдущей метке,
|
||||
исключит ребро и выведет `ROUTE_REPLANNED`.
|
||||
6. Успешный конец — `MISSION_FINISHED`. `Ctrl+C` публикует нулевую скорость.
|
||||
|
||||
Если за время подтверждения маршрут изменился, программа покажет новый и снова дождётся Enter.
|
||||
Время паузы между этапами не входит в `movement_seconds`.
|
||||
|
||||
Команды в том же терминале: `stop`, `resume`, `return`, `go`, `quit`.
|
||||
Ctrl+C немедленно инициирует остановку и завершение с ненулевым кодом.
|
||||
Для внешнего интерфейса: `/mvch/command` (`std_msgs/String`), `/mvch/emergency_stop`
|
||||
(`std_msgs/Bool`, true фиксирует стоп), `/mvch/status` (JSON в `std_msgs/String`).
|
||||
После потери датчиков программа останавливается и требует `resume`.
|
||||
|
||||
Автоматическая тренировка без подтверждений (только симулятор):
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14 --sim --auto --return-delay 10
|
||||
```
|
||||
|
||||
Цель координатами и конечная ориентация, в системе **поля**:
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --xy -2 2 --target-yaw 1.57 --sim
|
||||
```
|
||||
|
||||
На физическом роботе `--sim` не указывать:
|
||||
На физическом РМК-2 команда такая же, только `--sim` можно не писать:
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14
|
||||
```
|
||||
|
||||
Тогда используется подтверждённый документацией профиль: поле 5×5 (ID 0–24),
|
||||
ArUco `/RMC2/camera_bottom/aruco_id`, TF `/RMC2/tf` и `/RMC2/tf_static`, габарит
|
||||
RMC2 до 0.870 м. С `--sim` остаётся прежний профиль Webots 6×6 (ID 0–35),
|
||||
`/RMC2/aruco_id` и глобальные `/tf`/`/tf_static`.
|
||||
|
||||
Перед физическим заездом всё равно проверить частоту топиков, имена frame_id, шаг
|
||||
сетки, тормозной путь и аппаратную аварийную остановку. Документ не сообщает шаг
|
||||
между маркерами и точные имена TF-фреймов. Испытания в Webots не подтверждают
|
||||
готовность к реальному полю.
|
||||
Если значения отличаются, код менять не нужно:
|
||||
По умолчанию используется поле 5×5, ID 0–24 и шаг 1 м. Закрытые ячейки
|
||||
передаются через `--blocked`:
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14 --rows 5 --columns 5 --spacing 1.0 \
|
||||
--base-frame RMC2/base_link --odom-frame RMC2/odom
|
||||
python3 module2/module_b.py --target 14 --blocked 7 8 13 --sim
|
||||
```
|
||||
|
||||
У штатного Webots-драйвера нет таймаута последней команды: при SIGKILL/обрыве питания
|
||||
процесса программная остановка не гарантируется; остановить симулятор вручную.
|
||||
Основные параметры:
|
||||
|
||||
- `--target` — целевой ArUco ID;
|
||||
- `--blocked` — закрытые ячейки;
|
||||
- `--rows`, `--columns`, `--spacing` — геометрия сетки;
|
||||
- `--speed` — линейная скорость, по умолчанию 0.35 м/с, предел RMC2 0.5 м/с;
|
||||
- `--angular` — скорость поворота, по умолчанию 0.6 рад/с;
|
||||
- `--stop-distance` — дистанция срабатывания объезда, по умолчанию 0.65 м;
|
||||
- `--tolerance` — допустимое смещение от центра ArUco, по умолчанию 0.07 м;
|
||||
- `--scan-topic` — передний лидар, по умолчанию `/RMC2/scan_front`.
|
||||
|
||||
## Как устроен код
|
||||
|
||||
Весь модуль находится в одном файле `module2/module_b.py`: сверху параметры поля и
|
||||
скоростей, затем геометрия и Дейкстра, ниже ROS-узел, безопасность и логи.
|
||||
Читайте в таком порядке: `shortest_path` → `localize` → `drive_to` → `follow` →
|
||||
`main`. BFS строит кратчайший путь только по соседним свободным ячейкам.
|
||||
|
||||
Читайте в таком порядке: `Field.route` → `localize` → `scan_points` → `plan` → `drive` → `tick`.
|
||||
Схема поля: `0=(0,0)`, `1=(0,+1)`, `5=(-1,0)`. Старт может быть на любой
|
||||
метке. По TF видимой ArUco код связывает сетку с odometry, поэтому начальная
|
||||
ориентация тоже может быть любой. Между соседними метками ровер использует
|
||||
odometry, а на каждой следующей метке снова корректируется по ArUco.
|
||||
|
||||
Координаты реального поля: `0=(0,0)`, `5=(-1,0)`, `1=(0,1)`; в Webots 6×6:
|
||||
`0=(0,0)`, `6=(-1,0)`, `1=(0,1)`.
|
||||
Поворот `yaw` каждой метки — ориентация её TF-системы относительно поля.
|
||||
Измеренное положение метки в odom сравнивается с её известным положением на поле;
|
||||
так получаем преобразование `map_from_odom`. Между метками движение идёт по одометрии.
|
||||
У каждой промежуточной точки нужна свежая метка и попадание в допуск; «успех по таймауту» отсутствует.
|
||||
|
||||
Лидар переводится через TF с учётом `angle_min`, `angle_increment` и времени скана.
|
||||
У текущего симулятора `laser_merged` повёрнут относительно корпуса на **120°**.
|
||||
Планировщик исключает рёбра, проходящие слишком близко к точкам препятствий.
|
||||
Он не знает ID или место будущего препятствия заранее и сам ничего не спавнит.
|
||||
Если преграда появилась во время движения, робот тормозит, возвращается задним ходом
|
||||
к предыдущему маркеру по своему ребру и публикует новый маршрут `ROUTE_REPLANNED`.
|
||||
Если отступить безопасно нельзя, он стоит и завершает попытку по таймауту.
|
||||
|
||||
Профили сетки находятся в `REAL_FIELD` и `SIM_FIELD`, известные занятые клетки —
|
||||
в `BLOCKED_MARKERS`. Все длины — метры.
|
||||
Загруженная карта сама по себе не даёт RMC2 команды «ехать к marker ID». В выданном
|
||||
workspace RMC2 управляется через `/RMC2/cmd_vel`; Nav2 настроен для RMC1. Поэтому
|
||||
модуль Б напрямую использует ArUco, odometry и `/RMC2/scan_front`.
|
||||
|
||||
## Проверки и логи
|
||||
|
||||
```bash
|
||||
python3 module2/module_b.py --target 14 --dry-run # реальное поле 5x5
|
||||
python3 module2/module_b.py --target 35 --sim --dry-run # Webots 6x6
|
||||
python3 module2/module_b.py --target 14 --start 0 --dry-run
|
||||
python3 module2/module_b.py --target 24 --start 6 --blocked 7 8 --dry-run
|
||||
```
|
||||
|
||||
`reports/module_b_*.jsonl` содержат маршрут, движение, ошибки, препятствия,
|
||||
точность попадания по меткам и время. Логи исключены из Git.
|
||||
`module2/reports/module_b_*.jsonl` содержит обнаруженные маркеры, оба маршрута,
|
||||
препятствия, достигнутые точки и чистое время движения. Паузы на установку
|
||||
препятствия в это время не входят.
|
||||
|
||||
Условия финального КЗ: один запускаемый скрипт решения; маршруты в терминале и файле;
|
||||
явные `movement_start` / `movement_stop`; подтверждение эксперта перед каждым этапом.
|
||||
Фактические баллы определяют эксперты по заезду, а не наличие функций в коде.
|
||||
Перед реальным заездом достаточно проверить направление осей, фактический шаг
|
||||
между метками и дистанцию безопасной остановки. Скрипт не делает отдельный аудит
|
||||
топиков при старте: он просто стоит, пока не получит odometry, ArUco и её TF.
|
||||
|
||||
## Обновление VM с хоста
|
||||
|
||||
|
||||
Reference in New Issue
Block a user