263 lines
10 KiB
Python
263 lines
10 KiB
Python
#!/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")
|