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` | Финальное выравнивание ориентации |

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

```python
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()` вычисляет знаковое расстояние от текущей позиции до идеальной линии движения:

```python
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 = любой маркер)

#### Настройки

```python
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:

```python
# Старый 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-сообщения с координатами и углами

---

## Зависимости

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

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

---

## Запуск

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

```bash
python3 navigator_node.py
```

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

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

```bash
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