first commit

README.md

@@ -0,0 +1,229 @@
+# 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

aruco_centering.py

@@ -0,0 +1,262 @@
+#!/usr/bin/env python3
+"""
+Модуль точного центрирования над ArUco-маркером.
+
+Использует OpenCV для обнаружения ArUco-маркера в изображении
+с нижней камеры и вычисления смещения робота от центра маркера.
+Это позволяет точно центрировать робота над маркером.
+
+Может работать как отдельный ROS2-узел или как библиотека.
+"""
+
+import math
+import numpy as np
+
+try:
+    import cv2
+    from cv2 import aruco
+    OPENCV_AVAILABLE = True
+except ImportError:
+    OPENCV_AVAILABLE = False
+    print("⚠️  OpenCV не найден! Центрирование по камере недоступно.")
+    print("   Установите: pip3 install opencv-contrib-python")
+
+try:
+    import rclpy
+    from rclpy.node import Node
+    from sensor_msgs.msg import Image
+    from geometry_msgs.msg import Twist
+    from cv_bridge import CvBridge
+    ROS_AVAILABLE = True
+except ImportError:
+    ROS_AVAILABLE = False
+
+
+# ============== НАСТРОЙКИ КАМЕРЫ ==============
+CAMERA_WIDTH = 640     # Ширина изображения (пикс)
+CAMERA_HEIGHT = 480    # Высота изображения (пикс)
+CENTER_TOLERANCE = 20  # Допуск центрирования (пикс)
+CENTERING_KP = 0.002   # Пропорциональный коэффициент для центрирования
+MAX_CENTER_SPEED = 0.1 # Максимальная скорость при центрировании (м/с)
+# ================================================
+
+
+class ArucoCentering:
+    """
+    Класс для точного центрирования над ArUco-маркером по камере.
+    Работает без ROS2 — чисто с изображениями OpenCV.
+    """
+
+    def __init__(self):
+        if not OPENCV_AVAILABLE:
+            raise RuntimeError("OpenCV не установлен!")
+
+        # Словарь ArUco-маркеров: DICT_6X6_50 (6×6 битовый паттерн, 50 маркеров)
+        try:
+            self.aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_50)
+        except AttributeError:
+            self.aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_50)
+
+        # КРИТИЧНО: old API (< 4.7) требует DetectorParameters_create()
+        try:
+            self.aruco_params = aruco.DetectorParameters_create()  # old API (< 4.7)
+        except AttributeError:
+            self.aruco_params = aruco.DetectorParameters()          # new API (>= 4.7)
+
+        # Для новых версий OpenCV (4.7+)
+        try:
+            self.detector = aruco.ArucoDetector(self.aruco_dict, self.aruco_params)
+            self.use_new_api = True
+        except AttributeError:
+            self.use_new_api = False
+
+    def detect_markers(self, image: np.ndarray):
+        """
+        Обнаруживает ArUco-маркеры на изображении.
+
+        Возвращает:
+            corners: список углов маркеров
+            ids: массив ID маркеров
+        """
+        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if len(image.shape) == 3 else image
+
+        if self.use_new_api:
+            corners, ids, rejected = self.detector.detectMarkers(gray)
+        else:
+            corners, ids, rejected = aruco.detectMarkers(
+                gray, self.aruco_dict, parameters=self.aruco_params)
+
+        return corners, ids
+
+    def get_marker_center(self, corners) -> tuple:
+        """
+        Вычисляет центр маркера по его углам.
+
+        Возвращает: (cx, cy) — координаты центра в пикселях.
+        """
+        c = corners[0]
+        cx = int(np.mean(c[:, 0]))
+        cy = int(np.mean(c[:, 1]))
+        return (cx, cy)
+
+    def get_centering_error(self, image: np.ndarray, target_id: int = -1):
+        """
+        Вычисляет ошибку центрирования для указанного маркера.
+
+        Args:
+            image: Изображение с камеры (BGR)
+            target_id: ID маркера (-1 = любой маркер)
+
+        Возвращает:
+            (error_x, error_y, marker_id, found)
+            error_x: смещение по X в пикселях (>0 = маркер справа)
+            error_y: смещение по Y в пикселях (>0 = маркер ниже)
+            marker_id: ID обнаруженного маркера
+            found: True если маркер найден
+        """
+        h, w = image.shape[:2]
+        image_cx = w // 2
+        image_cy = h // 2
+
+        corners, ids = self.detect_markers(image)
+
+        if ids is None or len(ids) == 0:
+            return (0, 0, -1, False)
+
+        # Ищем нужный маркер
+        for i, marker_id in enumerate(ids.flatten()):
+            if target_id == -1 or marker_id == target_id:
+                cx, cy = self.get_marker_center(corners[i])
+                error_x = cx - image_cx
+                error_y = cy - image_cy
+                return (error_x, error_y, int(marker_id), True)
+
+        return (0, 0, -1, False)
+
+    def get_marker_orientation(self, corners) -> float:
+        """
+        Оценивает ориентацию маркера по его углам.
+        Возвращает угол в радианах.
+        """
+        c = corners[0]
+        # Вектор от первого угла ко второму — это "верх" маркера
+        dx = c[1][0] - c[0][0]
+        dy = c[1][1] - c[0][1]
+        return math.atan2(dy, dx)
+
+    def compute_velocity(self, error_x: float, error_y: float):
+        """
+        Вычисляет команду скорости для центрирования.
+
+        Возвращает: (vx, vy, centered)
+            vx: линейная скорость по X
+            vy: линейная скорость по Y  (для omni-drive, для diff-drive = 0)
+            centered: True если робот отцентрирован
+        """
+        centered = (abs(error_x) < CENTER_TOLERANCE and
+                    abs(error_y) < CENTER_TOLERANCE)
+
+        if centered:
+            return (0.0, 0.0, True)
+
+        # Пропорциональное управление
+        # Примечание: ось Y камеры инвертирована (вниз = +)
+        vx = -CENTERING_KP * error_y  # error_y камеры → движение вперёд/назад
+        vy = 0.0  # РМК-2 не имеет бокового движения (diff-drive)
+
+        # Ограничиваем скорость
+        vx = max(-MAX_CENTER_SPEED, min(MAX_CENTER_SPEED, vx))
+
+        return (vx, vy, False)
+
+
+# ==================== ROS2-УЗЕЛ ====================
+
+if ROS_AVAILABLE and OPENCV_AVAILABLE:
+
+    class ArucoCenteringNode(Node):
+        """
+        ROS2-узел для точного центрирования над ArUco-маркером.
+        Подписывается на изображение камеры и публикует cmd_vel.
+        """
+
+        def __init__(self, target_marker_id: int = -1):
+            super().__init__('aruco_centering')
+
+            self.target_id = target_marker_id
+            self.centering = ArucoCentering()
+            self.bridge = CvBridge()
+            self.is_centered = False
+
+            self.image_sub = self.create_subscription(
+                Image, '/RMC2/camera_bottom/image_color',
+                self._image_callback, 10)
+
+            self.cmd_vel_pub = self.create_publisher(
+                Twist, '/RMC2/cmd_vel', 10)
+
+            self.get_logger().info(
+                f"Центрирование над маркером "
+                f"{'(любой)' if target_marker_id == -1 else target_marker_id}")
+
+        def _image_callback(self, msg: Image):
+            """Обработка изображения с камеры."""
+            try:
+                cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+            except Exception as e:
+                self.get_logger().error(f"Ошибка преобразования: {e}")
+                return
+
+            error_x, error_y, marker_id, found = \
+                self.centering.get_centering_error(cv_image, self.target_id)
+
+            cmd = Twist()
+
+            if found:
+                vx, vy, centered = self.centering.compute_velocity(error_x, error_y)
+
+                if centered:
+                    if not self.is_centered:
+                        self.get_logger().info(
+                            f"✅ Отцентрирован над маркером {marker_id}!")
+                        self.is_centered = True
+                else:
+                    cmd.linear.x = vx
+                    self.is_centered = False
+                    self.get_logger().info(
+                        f"Центрирование: err=({error_x}, {error_y}), "
+                        f"vx={vx:.3f}", throttle_duration_sec=0.5)
+            else:
+                self.get_logger().warn("Маркер не найден!", throttle_duration_sec=1.0)
+
+            self.cmd_vel_pub.publish(cmd)
+
+
+# ==================== ТЕСТ ====================
+
+if __name__ == '__main__':
+    if OPENCV_AVAILABLE:
+        print("OpenCV ArUco Centering — тест")
+        centering = ArucoCentering()
+
+        # Создаём тестовое изображение с маркером
+        marker_image = aruco.generateImageMarker(
+            centering.aruco_dict, 5, 200)
+
+        # Помещаем маркер на белый фон со смещением
+        test_img = np.ones((480, 640), dtype=np.uint8) * 255
+        # Маркер смещён вправо и вниз от центра
+        x_offset, y_offset = 350, 270
+        test_img[y_offset:y_offset+200, x_offset:x_offset+200] = marker_image
+
+        test_img_bgr = cv2.cvtColor(test_img, cv2.COLOR_GRAY2BGR)
+
+        error_x, error_y, mid, found = centering.get_centering_error(test_img_bgr, 5)
+        print(f"Маркер найден: {found}")
+        print(f"ID: {mid}")
+        print(f"Ошибка X: {error_x} пикс, Y: {error_y} пикс")
+
+        vx, vy, centered = centering.compute_velocity(error_x, error_y)
+        print(f"Скорость: vx={vx:.4f}, отцентрирован: {centered}")
+    else:
+        print("OpenCV не установлен! pip3 install opencv-contrib-python")

nav_test.py

@@ -0,0 +1,527 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+from nav_msgs.msg import Odometry
+
+import cv2
+from cv_bridge import CvBridge
+import numpy as np
+import math
+import threading
+import time
+from collections import deque
+
+# ── Настраиваемые коэффициенты ──────────────────────────────────────────────
+K_VISUAL       = 0.8    # П-коэффициент визуального cross-track (агрессивная коррекция)
+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   # Предполагаемая длина одной клетки поля (м)
+# ────────────────────────────────────────────────────────────────────────────
+
+
+class RMC2Navigator(Node):
+    FIELD_ROWS = 6
+    FIELD_COLS = 6
+    
+    def __init__(self):
+        super().__init__('rmc2_navigator')
+
+        self.cmd_pub = self.create_publisher(Twist, '/RMC2/cmd_vel', 10)
+        self.create_subscription(String, '/RMC2/aruco_id', self.aruco_id_cb, 10)
+        self.create_subscription(Image, '/RMC2/camera_bottom/image_color', self.camera_cb, 10)
+        self.create_subscription(Odometry, '/RMC2/odometry', self.odom_cb, 10)
+
+        self.bridge = CvBridge()
+
+        self.current_marker_id = None
+        self.target_marker_id  = None
+        self.current_yaw = 0.0
+        self.current_x   = 0.0
+        self.current_y   = 0.0
+        self.target_yaw  = 0.0
+
+        self.state = 'IDLE'
+
+        self.total_cells        = 0
+        self.drive_start_x      = 0.0
+        self.drive_start_y      = 0.0
+        self.start_marker_id    = None
+        
+        self.blocked_cells = set()
+        self.waypoints = []
+        self.waypoint_idx = 0
+
+        # ArUco
+        try:
+            self.aruco_params = cv2.aruco.DetectorParameters_create()  # old API (< 4.7)
+        except AttributeError:
+            self.aruco_params = cv2.aruco.DetectorParameters()          # new API (>= 4.7)
+
+        try:
+            self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_50)
+        except AttributeError:
+            self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_50)
+
+        self._centering_ok_count = 0
+        self.latest_image = None
+        self._debug_frame_idx = 0
+        self._yaw_reference = None  # Базовый yaw при старте движения
+        self._cross_track_ref_marker = None  # Маркер, от которого ведётся odom cross-track
+        self._visual_err_x_memory = 0.0  # Память визуальной коррекции
+
+        # Таймер главного цикла (20 Гц)
+        self.timer = self.create_timer(0.05, self.control_loop)
+
+        # Поток ввода пользователя
+        self.input_thread = threading.Thread(target=self.user_input_thread, daemon=True)
+        self.input_thread.start()
+
+        self.get_logger().info("Узел навигации RMC2 запущен. Ожидание стартового маркера...")
+
+    # ── Колбэки ─────────────────────────────────────────────────────────────
+
+    def aruco_id_cb(self, msg):
+        try:
+            clean_id = msg.data.replace('aruco_', '')
+            self.current_marker_id = int(clean_id)
+        except ValueError:
+            pass
+
+    def odom_cb(self, msg):
+        self.current_x = msg.pose.pose.position.x
+        self.current_y = msg.pose.pose.position.y
+
+        q = msg.pose.pose.orientation
+        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
+        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
+        odom_yaw = math.atan2(siny_cosp, cosy_cosp)
+
+        # Компас жестко привязан к глобальным осям (отключен _yaw_reference)
+        self.current_yaw = self.normalize_angle(odom_yaw)
+
+    def camera_cb(self, msg):
+        try:
+            img_array = np.frombuffer(msg.data, dtype=np.uint8)
+            if msg.encoding == 'bgra8':
+                img = img_array.reshape((msg.height, msg.width, 4))
+                self.latest_image = cv2.cvtColor(img, cv2.COLOR_BGRA2BGR)
+            elif msg.encoding == 'bgr8':
+                self.latest_image = img_array.reshape((msg.height, msg.width, 3)).copy()
+            elif msg.encoding == 'rgb8':
+                img = img_array.reshape((msg.height, msg.width, 3))
+                self.latest_image = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+            else:
+                cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+                self.latest_image = cv_image.copy()
+        except Exception as e:
+            self.get_logger().error(f"Ошибка чтения кадра: {e}")
+
+    # ── Ввод пользователя ───────────────────────────────────────────────────
+
+    def user_input_thread(self):
+        while rclpy.ok():
+            if self.state == 'IDLE':
+                if self.current_marker_id is None:
+                    time.sleep(1)
+                    continue
+                try:
+                    print(f"\n--- Робот над маркером: {self.current_marker_id} ---")
+                    print("Заблокированные клетки:", sorted(self.blocked_cells))
+                    target = input("Введите ID целевой метки (или 'b <id>' для блокировки): ").strip()
+                    if target.startswith('b '):
+                        blocked_id = int(target[2:])
+                        if blocked_id in self.blocked_cells:
+                            self.blocked_cells.remove(blocked_id)
+                            print(f"Клетка {blocked_id} разблокирована")
+                        else:
+                            self.blocked_cells.add(blocked_id)
+                            print(f"Клетка {blocked_id} заблокирована")
+                        continue
+                    self.target_marker_id = int(target)
+                    self.start_navigation()
+                except ValueError:
+                    print("Ошибка: введите целое число.")
+            else:
+                time.sleep(0.5)
+
+    # ── Вспомогательные функции ─────────────────────────────────────────────
+
+    def get_marker_coords(self, marker_id):
+        row = marker_id % 6
+        col = marker_id // 6
+        x = col * 1.0
+        y = 5 - row * 1.0
+        return x, y
+
+    def marker_to_grid(self, marker_id):
+        return (marker_id % 6, marker_id // 6)
+    
+    def grid_to_marker(self, row, col):
+        return col * 6 + row
+
+    def get_neighbors(self, row, col):
+        neighbors = []
+        for dr, dc in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
+            nr, nc = row + dr, col + dc
+            if 0 <= nr < self.FIELD_ROWS and 0 <= nc < self.FIELD_COLS:
+                mid = self.grid_to_marker(nr, nc)
+                if mid not in self.blocked_cells:
+                    neighbors.append((nr, nc))
+        return neighbors
+
+    def find_path_bfs(self, start_id, goal_id):
+        if goal_id in self.blocked_cells:
+            return None
+        start = self.marker_to_grid(start_id)
+        goal = self.marker_to_grid(goal_id)
+        queue = deque([(start, [start])])
+        visited = {start}
+        while queue:
+            (row, col), path = queue.popleft()
+            if (row, col) == goal:
+                return path
+            for nr, nc in self.get_neighbors(row, col):
+                if (nr, nc) not in visited:
+                    visited.add((nr, nc))
+                    queue.append(((nr, nc), path + [(nr, nc)]))
+        return None
+
+    def normalize_angle(self, angle):
+        return math.atan2(math.sin(angle), math.cos(angle))
+
+    def start_navigation(self):
+        path = self.find_path_bfs(self.current_marker_id, self.target_marker_id)
+        if path is None:
+            print(f"Маршрут до {self.target_marker_id} не найден (заблокирован или нет пути)")
+            return
+        
+        self.waypoints = [self.grid_to_marker(r, c) for r, c in path]
+        self.waypoint_idx = 1
+        
+        print(f"Маршрут: {' → '.join(map(str, self.waypoints))}")
+        
+        self._navigate_to_next_waypoint()
+
+    def _navigate_to_next_waypoint(self):
+        if self.waypoint_idx >= len(self.waypoints):
+            self.get_logger().info("Все точки маршрута пройдены!")
+            self.state = 'IDLE'
+            return
+        
+        next_marker = self.waypoints[self.waypoint_idx]
+        cx, cy = self.get_marker_coords(self.current_marker_id)
+        tx, ty = self.get_marker_coords(next_marker)
+        dx, dy = tx - cx, ty - cy
+        
+        dist = math.sqrt(dx*dx + dy*dy)
+        if dist < 0.01:
+            self.waypoint_idx += 1
+            self._navigate_to_next_waypoint()
+            return
+        
+        self.get_logger().info(f"DEBUG: {self.current_marker_id}({cx:.1f},{cy:.1f}) -> {next_marker}({tx:.1f},{ty:.1f}), d=({dx:.1f},{dy:.1f})")
+        self.get_logger().info(f"DEBUG: current_yaw={math.degrees(self.current_yaw):.1f}°")
+        raw_yaw = math.atan2(dy, dx)
+        # Угол строго 180° может быть +π или -π, выбираем ближе к текущему yaw
+        raw_yaw_normalized = math.atan2(dy, dx)  # в диапазоне [-π, π]
+        # Проверяем оба варианта: raw_yaw и raw_yaw ± 2π
+        candidates = [raw_yaw_normalized - 2*math.pi, raw_yaw_normalized, raw_yaw_normalized + 2*math.pi]
+        # Выбираем ближайший к current_yaw
+        best_yaw = min(candidates, key=lambda y: abs(y - self.current_yaw))
+        self.target_yaw = best_yaw
+        self.get_logger().info(f"DEBUG: target_yaw={math.degrees(self.target_yaw):.1f}°")
+        self.total_cells = 1
+        self.start_marker_id = self.current_marker_id
+        self._centering_ok_count = 0
+        
+        self._line_start_x, self._line_start_y = self.get_marker_coords(self.current_marker_id)
+        self._line_perp_yaw = self.target_yaw + math.pi / 2.0
+        self._cross_track_ref_marker = self.current_marker_id
+        self._visual_err_x_memory = 0.0
+        
+        self.get_logger().info(
+            f"Еду к точке {next_marker} (waypoint {self.waypoint_idx}/{len(self.waypoints)-1}), "
+            f"курс {math.degrees(self.target_yaw):.1f}°")
+        self.state = 'TURN_TO_TARGET'
+
+    def _cross_track_error(self, x, y):
+        """
+        Рассчитывает signed cross-track error — расстояние от текущей позиции
+        до идеальной линии движения (с указанием знака: + справа, − слева).
+
+        Линия: проходит через (_line_start_x, _line_start_y) под углом target_yaw.
+        """
+        dx = x - self._line_start_x
+        dy = y - self._line_start_y
+        # Проекция на нормаль к линии (перпендикуляр target_yaw)
+        cross_track = -dx * math.sin(self.target_yaw) + dy * math.cos(self.target_yaw)
+        return cross_track
+
+    # ── ArUco-детектор (общий, переиспользуемый) ────────────────────────────
+
+    def _detect_markers(self):
+        """Детектирует маркеры на последнем кадре камеры.
+
+        Возвращает (ids_list, corners_list, img_w, img_h)
+        или (None, None, 0, 0) при ошибке / нет кадра.
+        """
+        if self.latest_image is None:
+            return None, None, 0, 0
+        try:
+            frame = self.latest_image.copy()
+            gray  = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+            gray  = np.ascontiguousarray(gray, dtype=np.uint8)
+            h, w  = gray.shape
+
+            scale = min(1.0, 640.0 / w)
+            small = cv2.resize(gray, (0, 0), fx=scale, fy=scale,
+                               interpolation=cv2.INTER_AREA) if scale < 1.0 else gray
+
+            corners, ids, _ = cv2.aruco.detectMarkers(
+                small, self.aruco_dict, parameters=self.aruco_params)
+
+            if ids is not None and scale < 1.0:
+                corners = [c / scale for c in corners]
+
+            if ids is None:
+                return [], [], w, h
+
+            return ids.flatten().tolist(), corners, w, h
+        except Exception as e:
+            self.get_logger().error(f"Ошибка детекции: {e}")
+            return None, None, 0, 0
+
+    def _marker_offset(self, marker_id, ids_list, corners, img_w, img_h):
+        """Возвращает нормализованные (err_x, err_y) ∈ [-1, 1] для заданного маркера
+        или (None, None) если маркер не найден.
+
+        err_x > 0: маркер правее центра камеры (робот левее центра маркера).
+        err_y > 0: маркер ниже центра камеры (маркер «за» роботом).
+        """
+        if not ids_list or marker_id not in ids_list:
+            return None, None
+        idx   = ids_list.index(marker_id)
+        pts   = corners[idx][0]
+        mcx   = np.mean(pts[:, 0])
+        mcy   = np.mean(pts[:, 1])
+        err_x = (mcx - img_w / 2.0) / (img_w / 2.0)
+        err_y = (mcy - img_h / 2.0) / (img_h / 2.0)
+        return err_x, err_y
+
+    # ── Главный цикл управления (20 Гц) ─────────────────────────────────────
+
+    def control_loop(self):
+        if self.state == 'IDLE':
+            return
+
+        twist = Twist()
+
+        # ── Поворот на месте (к цели или финальное выравнивание) ────────────
+        if self.state in ('TURN_TO_TARGET', 'ALIGN_ORIENTATION'):
+            yaw_err = self.normalize_angle(self.target_yaw - self.current_yaw)
+
+            if abs(yaw_err) > 0.035:  # ~2°
+                # Замедляемся при подходе к цели: ниже 5° уменьшаем усиление
+                gain = K_TURN if abs(yaw_err) > 0.087 else K_TURN * 0.6
+                twist.angular.z = gain * yaw_err
+                twist.angular.z = max(-1.0, min(1.0, twist.angular.z))
+                if not hasattr(self, '_log_counter'):
+                    self._log_counter = 0
+                self._log_counter += 1
+                if self._log_counter % 10 == 1:
+                    self.get_logger().info(
+                        f"TURN: target={math.degrees(self.target_yaw):.1f}°, "
+                        f"current={math.degrees(self.current_yaw):.1f}°, "
+                        f"err={math.degrees(yaw_err):.1f}°, cmd={twist.angular.z:.2f}")
+            else:
+                if self.state == 'TURN_TO_TARGET':
+                    self.get_logger().info(
+                        f"Курс взят ({math.degrees(self.target_yaw):.1f}°). "
+                        f"Еду {self.total_cells} кл. прямо.")
+                    self.state = 'DRIVE_STRAIGHT'
+                    self.drive_start_x = self.current_x
+                    self.drive_start_y = self.current_y
+                else:
+                    self.get_logger().info("Ориентация выровнена!")
+                    self.current_marker_id = self.target_marker_id
+                    self.state = 'IDLE'
+
+        elif self.state == 'DRIVE_STRAIGHT':
+            if self.current_marker_id == self.target_marker_id:
+                self.get_logger().info("Финальный маркер подтверждён!")
+                self.waypoint_idx += 1
+                if self.waypoint_idx < len(self.waypoints):
+                    self._navigate_to_next_waypoint()
+                else:
+                    self.state = 'IDLE'
+                self.cmd_pub.publish(twist)
+                return
+
+            ids_list, corners, img_w, img_h = self._detect_markers()
+
+            if self.waypoint_idx < len(self.waypoints):
+                next_wp = self.waypoints[self.waypoint_idx]
+                if ids_list is not None and next_wp in ids_list:
+                    self.get_logger().info(f"Промежуточный waypoint {next_wp} достигнут")
+                    self.current_marker_id = next_wp
+                    self._line_start_x, self._line_start_y = self.get_marker_coords(next_wp)
+                    self.waypoint_idx += 1
+                    if self.waypoint_idx < len(self.waypoints):
+                        self._navigate_to_next_waypoint()
+                    else:
+                        self.state = 'IDLE'
+                    self.cmd_pub.publish(twist)
+                    return
+
+            if ids_list is not None and self.target_marker_id in ids_list:
+                ex, ey = self._marker_offset(self.target_marker_id, ids_list, corners, img_w, img_h)
+                if ey is not None and ey > -0.2:
+                    self.get_logger().info(
+                        f"Финальный маркер {self.target_marker_id} в кадре (err_y={ey:.2f}).")
+                    self.waypoint_idx += 1
+                    if self.waypoint_idx < len(self.waypoints):
+                        self._navigate_to_next_waypoint()
+                    else:
+                        self.state = 'IDLE'
+                    self.cmd_pub.publish(twist)
+                    return
+
+            visual_err_x = None
+            if ids_list:
+                best_dist = float('inf')
+                for mid in ids_list:
+                    ex, ey = self._marker_offset(mid, ids_list, corners, img_w, img_h)
+                    if ex is None:
+                        continue
+                    dist = abs(ex) + abs(ey)
+                    if dist < best_dist:
+                        best_dist = dist
+                        visual_err_x = ex
+            
+            if visual_err_x is not None:
+                self._visual_err_x_memory = visual_err_x
+            else:
+                self._visual_err_x_memory *= K_VISUAL_DECAY
+
+            base_yaw_err = self.normalize_angle(self.target_yaw - self.current_yaw)
+
+            use_visual = abs(self._visual_err_x_memory) > 0.01
+            if use_visual:
+                twist.linear.x = DRIVE_SPEED
+                twist.angular.z = K_HEADING * base_yaw_err - K_VISUAL * self._visual_err_x_memory
+                twist.angular.z = max(-MAX_ANGULAR, min(MAX_ANGULAR, twist.angular.z))
+            else:
+                twist.linear.x = DRIVE_SPEED
+                twist.angular.z = K_HEADING * base_yaw_err
+                twist.angular.z = max(-MAX_ANGULAR, min(MAX_ANGULAR, twist.angular.z))
+
+            if not hasattr(self, '_cross_log_counter'):
+                self._cross_log_counter = 0
+            self._cross_log_counter += 1
+            if self._cross_log_counter % 10 == 0:
+                self.get_logger().info(
+                    f"[VIS-MEM] err_x={self._visual_err_x_memory:+.3f}, "
+                    f"yaw={math.degrees(base_yaw_err):+.1f}°, tw.z={twist.angular.z:+.3f}")
+
+        # ── Визуальное центрирование над финальным маркером ─────────────────
+        elif self.state == 'CENTER_MARKER':
+            if self.latest_image is None:
+                self.get_logger().warn("CENTER_MARKER: нет кадра", throttle_duration_sec=1.0)
+                return
+
+            try:
+                frame = self.latest_image.copy()
+                gray  = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+                gray  = np.ascontiguousarray(gray, dtype=np.uint8)
+
+                self._debug_frame_idx += 1
+                if self._debug_frame_idx % 40 == 1:
+                    cv2.imwrite('/tmp/aruco_debug_gray.png', gray)
+                    self.get_logger().info(
+                        f"DEBUG кадр #{self._debug_frame_idx}: shape={gray.shape}, "
+                        f"mean={gray.mean():.1f} → /tmp/aruco_debug_gray.png")
+
+                h_orig, w_orig = gray.shape
+                scale = min(1.0, 640.0 / w_orig)
+                if scale < 1.0:
+                    small = cv2.resize(gray, (0, 0), fx=scale, fy=scale,
+                                       interpolation=cv2.INTER_AREA)
+                else:
+                    small = gray
+
+                corners, ids, _ = cv2.aruco.detectMarkers(
+                    small, self.aruco_dict, parameters=self.aruco_params)
+
+                if ids is not None and scale < 1.0:
+                    corners = [c / scale for c in corners]
+
+                target_corners = None
+                if ids is not None:
+                    found_ids = ids.flatten().tolist()
+                    for i in range(len(ids)):
+                        if ids[i][0] == self.target_marker_id:
+                            target_corners = corners[i][0]
+                            break
+                    if target_corners is None:
+                        self.get_logger().warn(
+                            f"CENTER_MARKER: вижу {found_ids}, цель={self.target_marker_id} не найдена",
+                            throttle_duration_sec=1.0)
+                else:
+                    self.get_logger().warn(
+                        "CENTER_MARKER: маркеры не найдены", throttle_duration_sec=1.0)
+                    self._centering_ok_count = 0
+
+                if target_corners is not None:
+                    mcx = np.mean(target_corners[:, 0])
+                    mcy = np.mean(target_corners[:, 1])
+                    h, w = gray.shape
+
+                    err_x = (mcx - w / 2.0) / (w / 2.0)   # нормализованная ошибка X
+                    err_y = (mcy - h / 2.0) / (h / 2.0)   # нормализованная ошибка Y
+
+                    twist.linear.x  = -0.12 * err_y
+                    twist.angular.z = -0.25 * err_x
+
+                    twist.linear.x  = max(-0.08, min(0.08, twist.linear.x))
+                    twist.angular.z = max(-0.20, min(0.20, twist.angular.z))
+
+                    if abs(err_x) < 0.05 and abs(err_y) < 0.05:
+                        self._centering_ok_count += 1
+                        if self._centering_ok_count >= 5:
+                            self.get_logger().info(
+                                f"Центрирование завершено (err={err_x:.2f},{err_y:.2f}). "
+                                f"Выравниваю ориентацию.")
+                            self._centering_ok_count = 0
+                            self.state = 'ALIGN_ORIENTATION'
+                    else:
+                        self._centering_ok_count = 0
+
+            except Exception as e:
+                self.get_logger().error(f"OpenCV ошибка: {e}")
+
+        self.cmd_pub.publish(twist)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    navigator = RMC2Navigator()
+    try:
+        rclpy.spin(navigator)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        navigator.cmd_pub.publish(Twist())
+        navigator.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()