From 10b08c715e3d171f5a6e3d8d31d333cfe971e019 Mon Sep 17 00:00:00 2001 From: Vyacheslav K Date: Wed, 1 Apr 2026 06:42:19 +0300 Subject: [PATCH] first commit --- README.md | 229 ++++++++++++++++++++ aruco_centering.py | 262 ++++++++++++++++++++++ nav_test.py | 527 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 1018 insertions(+) create mode 100644 README.md create mode 100644 aruco_centering.py create mode 100644 nav_test.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..196719a --- /dev/null +++ b/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 ` с автоматическим перестроением маршрута. + +### 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 ` (например, `b 15`) +4. Для разблокировки: повторная команда `b ` + +### Узел центрирования (отдельно) + +```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 diff --git a/aruco_centering.py b/aruco_centering.py new file mode 100644 index 0000000..24eab71 --- /dev/null +++ b/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") diff --git a/nav_test.py b/nav_test.py new file mode 100644 index 0000000..b6da564 --- /dev/null +++ b/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 ' для блокировки): ").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()