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")
|