webots-module-b/README.md

RMC-2 Navigation System

Система автономной навигации для робота RMC-2 на основе ROS2 с использованием ArUco-маркеров и одометрии.

Описание

Проект реализует полноценную систему навигации по полю 6×6 клеток, размеченному ArUco-маркерами (DICT_6X6_50). Робот строит маршрут с учётом заблокированных клеток, выполняет движение по линии с визуальной коррекцией и точное центрирование над целевым маркером.

Файлы проекта

navigator_node.py — основной узел навигации

Главный ROS2-узел, реализующий полный цикл навигации:

• Построение маршрута — BFS-алгоритм для поиска кратчайшего пути по сетке 6×6
• Движение по линии — комбинированное управление по одометрии и визуальной обратной связи
• Cross-track коррекция — расчёт бокового отклонения от идеальной траектории
• Визуальная память — затухающее сохранение последней визуальной ошибки (K_VISUAL_DECAY = 0.92)
• Финальное центрирование — точное позиционирование над маркером по камере

Архитектура узла

RMC2Navigator (Node)
├── Подписки:
│   ├── /RMC2/aruco_id (String) — текущий маркер под роботом
│   ├── /RMC2/camera_bottom/image_color (Image) — видеопоток
│   └── /RMC2/odometry (Odometry) — данные одометрии
├── Публикации:
│   └── /RMC2/cmd_vel (Twist) — команды скорости
├── Таймер: 20 Гц (control_loop)
└── Поток ввода: консольный ввод команд пользователя

States (конечный автомат)

Состояние	Описание
IDLE	Ожидание команды
TURN_TO_TARGET	Поворот на целевой угол
DRIVE_STRAIGHT	Движение прямо с коррекцией
CENTER_MARKER	Визуальное центрирование над маркером
ALIGN_ORIENTATION	Финальное выравнивание ориентации

Ключевые коэффициенты

K_VISUAL       = 0.8    # П-коэффициент визуальной коррекции
K_VISUAL_DECAY = 0.92   # Затухание визуальной ошибки
K_HEADING      = 0.6    # Удержание курса
K_CROSS_P      = 2.0    # Odom cross-track
MAX_ANGULAR    = 0.35   # Макс. угловая скорость (рад/с)
K_TURN         = 1.5    # Поворот на месте
DRIVE_SPEED    = 0.3    # Скорость движения (м/с)
CELL_SIZE_M    = 0.65   # Размер клетки (м)

Алгоритм cross-track ошибки

Метод _cross_track_error() вычисляет знаковое расстояние от текущей позиции до идеальной линии движения:

dx = x - start_x
dy = y - start_y
cross_track = -dx * sin(target_yaw) + dy * cos(target_yaw)

Знак ошибки: + — справа от линии, − — слева.

---

aruco_centering.py — модуль центрирования

Библиотека и ROS2-узел для точного центрирования над ArUco-маркером.

Класс ArucoCentering

Работает независимо от ROS2, требует только OpenCV:

• detect_markers(image) — обнаружение всех маркеров на кадре
• get_marker_center(corners) — вычисление центра маркера
• get_centering_error(image, target_id) — ошибка позиционирования (пиксели)
• get_marker_orientation(corners) — угол ориентации маркера (радианы)
• compute_velocity(error_x, error_y) — расчёт скорости для центрирования

Класс ArucoCenteringNode

ROS2-узел для автономного центрирования:

• Подписка: /RMC2/camera_bottom/image_color
• Публикация: /RMC2/cmd_vel
• Параметр: target_marker_id (−1 = любой маркер)

Настройки

CAMERA_WIDTH = 640       # Ширина кадра (пикс)
CAMERA_HEIGHT = 480      # Высота кадра (пикс)
CENTER_TOLERANCE = 20    # Допуск центрирования (пикс)
CENTERING_KP = 0.002     # Пропорциональный коэффициент
MAX_CENTER_SPEED = 0.1   # Макс. скорость (м/с)

---

Преимущества системы

1. Гибридная навигация

Комбинирование одометрии и компьютерного зрения обеспечивает устойчивость:

• Одометрия — долгосрочная стабильность
• Камера — быстрая коррекция отклонений

2. Визуальная память

Коэффициент K_VISUAL_DECAY = 0.92 сохраняет последнюю визуальную ошибку при временной потере маркера, предотвращая резкие колебания.

3. Адаптивное управление

• Автоматическое снижение усиления при малых ошибках (< 5°)
• Ограничение угловой скорости (MAX_ANGULAR)
• Выбор ближайшего эквивалента целевого угла (±2π)

4. Обход препятствий

Динамическая блокировка клеток через консольную команду b <id> с автоматическим перестроением маршрута.

5. Кроссплатформенность OpenCV

Поддержка старых (< 4.7) и новых (≥ 4.7) версий OpenCV через детектирование API:

# Старый API
cv2.aruco.Dictionary_get()
cv2.aruco.DetectorParameters_create()

# Новый API
cv2.aruco.getPredefinedDictionary()
cv2.aruco.DetectorParameters()
cv2.aruco.ArucoDetector()

6. Многозадачность

• Главный цикл: 20 Гц (таймер ROS2)
• Ввод пользователя: отдельный поток
• Обработка изображений: асинхронный колбэк

7. Отладочные инструменты

• Логирование с троттлингом
• Сохранение кадров (/tmp/aruco_debug_gray.png)
• Детальные DEBUG-сообщения с координатами и углами

---

Зависимости

# ROS2 пакеты
rclpy
geometry_msgs
sensor_msgs
std_msgs
nav_msgs
cv_bridge

# Python библиотеки
opencv-contrib-python  # ArUco + cv2
numpy

---

Запуск

Основной узел навигации

python3 navigator_node.py

После запуска:

1. Дождитесь сообщения "Узел навигации RMC2 запущен"
2. Введите ID целевого маркера в консоль
3. Для блокировки клетки: b <id> (например, b 15)
4. Для разблокировки: повторная команда b <id>

Узел центрирования (отдельно)

python3 aruco_centering.py

---

Структура поля

ID маркеров (6×6):
     col 0   1   2   3   4   5
row 5   0   1   2   3   4   5
    4   6   7   8   9  10  11
    3  12  13  14  15  16  17
    2  18  19  20  21  22  23
    1  24  25  26  27  28  29
row 0  30  31  32  33  34  35

Формулы:

• row = marker_id % 6
• col = marker_id // 6
• x = col * 1.0
• y = 5 - row * 1.0

---

Пример маршрута

Маршрут: 0 → 1 → 2 → 8 → 14 → 15
Еду к точке 1 (waypoint 1/5), курс 90.0°
Курс взят (90.0°). Еду 1 кл. прямо.
[VIS-MEM] err_x=+0.120, yaw=+2.3°, tw.z=-0.096
Промежуточный waypoint 1 достигнут
...
Финальный маркер 15 в кадре (err_y=-0.15).
Центрирование завершено (err=0.02,-0.03). Выравниваю ориентацию.
Все точки маршрута пройдены!

---

Лицензия

MIT