# Умная навигация по сетке 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. Сделайте скрипт исполняемым (если еще не сделали):
   ```bash
   chmod +x nav_test.py
   ```
3. Запустите основной скрипт навигации:
   ```bash
   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`. Это заставляет робота физически "наезжать" на центр маркера, даже если одометрия думает, что робот едет прямо. Это полностью нивелирует дрейф датчиков на длинных дистанциях!