#!/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")