Умная навигация по сетке ArUco-маркеров (RMC2)

Данный проект представляет собой надежную систему автономной навигации для робота с дифференциальным приводом. Робот перемещается по координатной сетке (поле 6х6), узлы которой обозначены метками ArUco. Навигация осуществляется с использованием одометрии и визуальной обратной связи от направленной вниз камеры.

Главные преимущества

1. Гибридная система управления (Сенсорный фьюжн)
   • Одометрия используется для удержания базового курса (Heading) и плавных поворотов.
   • Компьютерное зрение (ArUco) используется для коррекции бокового сноса (Cross-track error) и предотвращения накопления ошибки одометрии.
2. Память визуальной ошибки (Visual Error Decay)
   • Когда робот проезжает над маркером, он вычисляет визуальное смещение. Если маркер выходит из поля зрения, система "запоминает" ошибку и плавно сводит её на нет (коэффициент K_VISUAL_DECAY). Это избавляет робота от резких рывков при потере маркера из кадра.
3. Динамическое построение маршрутов (Pathfinding)
   • Встроенный алгоритм BFS (Поиск в ширину) автоматически прокладывает кратчайший маршрут по клеткам.
   • Поддержка динамических препятствий: пользователь может блокировать и разблокировать узлы сетки (команда b <id>), и робот автоматически построит маршрут в обход.
4. Прецизионное центрирование (Visual Servoing)
   • Встроенный модуль визуального сервопривода обеспечивает финальную парковку. Робот использует П-регулятор по осям X и Y камеры для идеального позиционирования над целевой клеткой (погрешность до пары пикселей/миллиметров).
5. Универсальность и отказоустойчивость OpenCV
   • Код автоматически определяет версию библиотеки cv2 и использует корректный API (поддерживаются как старые версии < 4.7, так и новые >= 4.7), что снимает головную боль при развертывании на разных версиях Ubuntu/ROS2.

---

Архитектура проекта

Весь функционал собран в модулях, главный из которых:

Узел навигатора (nav_test.py)

Главный ROS2 узел (RMC2Navigator), управляющий конечным автоматом (State Machine) робота. Состояния (States):

• IDLE: Режим ожидания. Опрос консоли в отдельном потоке для получения цели от пользователя.
• TURN_TO_TARGET: Поворот на месте к следующей точке маршрута (waypoint).
• DRIVE_STRAIGHT: Движение вперед по отрезку с одновременным удержанием курса и визуальным подруливанием над промежуточными маркерами.
• CENTER_MARKER: Точная остановка и выравнивание камеры строго над центром финального маркера (используются функции работы с ArUco).
• ALIGN_ORIENTATION: Корректировка финального угла (Yaw) робота после центрирования.

---

ROS-Интерфейсы (Топики)

Узел подписывается и публикует данные в следующие топики:

Имя топика	Тип сообщения	Направление	Описание
/RMC2/cmd_vel	geometry_msgs/Twist	Publisher	Управляющие команды скорости на колеса
/RMC2/aruco_id	std_msgs/String	Subscriber	ID текущего/ближайшего маркера
/RMC2/camera_bottom/image_color	sensor_msgs/Image	Subscriber	Видеопоток с нижней камеры робота
/RMC2/odometry	nav_msgs/Odometry	Subscriber	Данные одометрии для расчета текущего угла (Yaw)

---

Инструкция по использованию

1. Запустите ROS2-окружение и симуляцию (или систему реального робота).
2. Сделайте скрипт исполняемым (если еще не сделали):

   chmod +x nav_test.py
   

3. Запустите основной скрипт навигации:

   python3 nav_test.py
   # или через ros2 run, если скрипт лежит в пакете:
   # ros2 run <имя_вашего_пакета> nav_test.py
   

4. Обратите внимание на терминал. Робот дождется, пока обнаружит стартовый маркер, и предложит ввести команду:
   • Введите число (например, 15), чтобы робот построил маршрут и поехал на клетку с этим маркером.
   • Введите b 8, чтобы добавить клетку 8 в "черный список" (заблокировать проезд через неё).
   • Введите b 8 повторно, чтобы разблокировать клетку.

---

Тонкая настройка (Параметры управления)

В начале файла nav_test.py вынесены ключевые коэффициенты управления, которые можно тюнинговать под вес и моторы вашего конкретного робота:

• K_VISUAL = 0.8 — Резкость реакции на боковое смещение от центра маркера при движении прямо.
• K_VISUAL_DECAY = 0.92 — Скорость "забывания" ошибки (1.0 = помнит всегда, 0.0 = забывает мгновенно).
• K_HEADING = 0.6 — П-регулятор удержания направления по одометрии (компасу).
• MAX_ANGULAR = 0.35 — Лимит угловой скорости в движении (рад/с).
• K_TURN = 1.5 — Агрессивность разворота на месте.
• DRIVE_SPEED = 0.3 — Крейсерская линейная скорость движения (м/с).

---

Как работает визуальная коррекция курса

Во время состояния DRIVE_STRAIGHT робот непрерывно анализирует кадры с камеры:

1. Если маркера в кадре нет, скорость поворота рассчитывается только по одометрии (K_HEADING * yaw_err).
2. Как только внизу появляется маркер маршрута, камера фиксирует нормализованное боковое отклонение X (от -1.0 до 1.0).
3. Алгоритм вычитает из одометрического регулятора значение K_VISUAL * err_x. Это заставляет робота физически "наезжать" на центр маркера, даже если одометрия думает, что робот едет прямо. Это полностью нивелирует дрейф датчиков на длинных дистанциях!