700 lines
30 KiB
Python
700 lines
30 KiB
Python
#!/usr/bin/env python3
|
||
|
||
import math
|
||
import threading
|
||
import time
|
||
from collections import deque
|
||
|
||
import cv2
|
||
import numpy as np
|
||
import rclpy
|
||
from cv_bridge import CvBridge
|
||
from geometry_msgs.msg import Twist
|
||
from nav_msgs.msg import Odometry
|
||
from rclpy.node import Node
|
||
from sensor_msgs.msg import Image, LaserScan
|
||
from std_msgs.msg import String
|
||
|
||
# ── Настраиваемые коэффициенты ──────────────────────────────────────────────
|
||
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 # Предполагаемая длина одной клетки поля (м)
|
||
|
||
# ── Параметры проверки препятствия лидаром ──────────────────────────────────
|
||
LIDAR_FORWARD_HALF_ANGLE_DEG = 15.0 # Проверяем узкий сектор строго перед роботом
|
||
LIDAR_MIN_VALID_RANGE_M = 0.12 # Игнорируем слишком близкие шумные значения
|
||
LIDAR_BLOCK_DISTANCE_M = 0.85 # Если впереди объект ближе этого порога — следующая клетка занята
|
||
LIDAR_CONFIRM_CYCLES = 3 # Сколько циклов подряд нужно подтвердить препятствие
|
||
# ────────────────────────────────────────────────────────────────────────────
|
||
|
||
|
||
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.create_subscription(LaserScan, '/RMC2/scan_front', self.scan_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.dynamic_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.latest_scan = 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 # Память визуальной коррекции
|
||
|
||
# Служебное состояние детектора динамических препятствий.
|
||
self._lidar_block_confirm_count = 0
|
||
self._lidar_checked_marker = None
|
||
self._last_front_range = None
|
||
|
||
# Таймер главного цикла (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 scan_cb(self, msg):
|
||
self.latest_scan = msg
|
||
|
||
# ── Ввод пользователя ───────────────────────────────────────────────────
|
||
|
||
def user_input_thread(self):
|
||
while rclpy.ok():
|
||
if self.state == 'IDLE':
|
||
if self.current_marker_id is None:
|
||
time.sleep(1)
|
||
continue
|
||
try:
|
||
all_blocked = sorted(self.get_all_blocked_cells())
|
||
print(f'\n--- Робот над маркером: {self.current_marker_id} ---')
|
||
print('Статические блокировки:', sorted(self.blocked_cells))
|
||
print('Динамические блокировки:', sorted(self.dynamic_blocked_cells))
|
||
print('Все занятые клетки:', all_blocked)
|
||
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_all_blocked_cells(self):
|
||
return self.blocked_cells | self.dynamic_blocked_cells
|
||
|
||
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 = []
|
||
blocked = self.get_all_blocked_cells()
|
||
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 blocked:
|
||
neighbors.append((nr, nc))
|
||
return neighbors
|
||
|
||
def find_path_bfs(self, start_id, goal_id):
|
||
blocked = self.get_all_blocked_cells()
|
||
if goal_id in blocked:
|
||
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 _reset_lidar_block_detector(self):
|
||
self._lidar_block_confirm_count = 0
|
||
self._lidar_checked_marker = None
|
||
self._last_front_range = None
|
||
|
||
def _get_forward_min_range(self):
|
||
if self.latest_scan is None:
|
||
return None
|
||
|
||
ranges = np.asarray(self.latest_scan.ranges, dtype=np.float32)
|
||
if ranges.size == 0:
|
||
return None
|
||
|
||
angles = self.latest_scan.angle_min + np.arange(ranges.size) * self.latest_scan.angle_increment
|
||
half_angle = math.radians(LIDAR_FORWARD_HALF_ANGLE_DEG)
|
||
|
||
valid_mask = np.isfinite(ranges)
|
||
valid_mask &= ranges >= max(LIDAR_MIN_VALID_RANGE_M, self.latest_scan.range_min)
|
||
if math.isfinite(self.latest_scan.range_max) and self.latest_scan.range_max > 0.0:
|
||
valid_mask &= ranges <= self.latest_scan.range_max
|
||
valid_mask &= np.abs(angles) <= half_angle
|
||
|
||
if not np.any(valid_mask):
|
||
return None
|
||
|
||
return float(np.min(ranges[valid_mask]))
|
||
|
||
def _check_next_waypoint_occupied(self):
|
||
if self.waypoint_idx >= len(self.waypoints):
|
||
self._reset_lidar_block_detector()
|
||
return None
|
||
|
||
next_marker = self.waypoints[self.waypoint_idx]
|
||
|
||
if next_marker in self.get_all_blocked_cells():
|
||
self._reset_lidar_block_detector()
|
||
return next_marker
|
||
|
||
front_range = self._get_forward_min_range()
|
||
self._last_front_range = front_range
|
||
|
||
if self._lidar_checked_marker != next_marker:
|
||
self._lidar_checked_marker = next_marker
|
||
self._lidar_block_confirm_count = 0
|
||
|
||
if front_range is None or front_range > LIDAR_BLOCK_DISTANCE_M:
|
||
self._lidar_block_confirm_count = 0
|
||
return None
|
||
|
||
self._lidar_block_confirm_count += 1
|
||
self.get_logger().info(
|
||
f'[LIDAR] obstacle candidate for marker {next_marker}: '
|
||
f'range={front_range:.2f} m, confirm={self._lidar_block_confirm_count}/{LIDAR_CONFIRM_CYCLES}',
|
||
throttle_duration_sec=0.5,
|
||
)
|
||
|
||
if self._lidar_block_confirm_count >= LIDAR_CONFIRM_CYCLES:
|
||
self._lidar_block_confirm_count = 0
|
||
return next_marker
|
||
|
||
return None
|
||
|
||
def _mark_dynamic_blocked(self, marker_id, distance_m=None):
|
||
if marker_id is None or marker_id == self.current_marker_id:
|
||
return
|
||
|
||
if marker_id not in self.dynamic_blocked_cells:
|
||
self.dynamic_blocked_cells.add(marker_id)
|
||
msg = f'Клетка {marker_id} помечена как занятая по лидару'
|
||
if distance_m is not None:
|
||
msg += f' (препятствие ~ {distance_m:.2f} м впереди)'
|
||
self.get_logger().warn(msg)
|
||
else:
|
||
self.get_logger().warn(
|
||
f'Лидар снова подтверждает занятость клетки {marker_id}',
|
||
throttle_duration_sec=1.0,
|
||
)
|
||
|
||
def _build_path_to_target(self, announce_prefix='Маршрут'):
|
||
if self.current_marker_id is None or self.target_marker_id is None:
|
||
self.get_logger().error('Невозможно построить маршрут: неизвестен текущий или целевой маркер')
|
||
self.state = 'IDLE'
|
||
return False
|
||
|
||
path = self.find_path_bfs(self.current_marker_id, self.target_marker_id)
|
||
if path is None:
|
||
blocked = sorted(self.get_all_blocked_cells())
|
||
self.get_logger().error(
|
||
f'Маршрут до {self.target_marker_id} не найден. Занятые клетки: {blocked}'
|
||
)
|
||
self.waypoints = []
|
||
self.waypoint_idx = 0
|
||
self.state = 'IDLE'
|
||
return False
|
||
|
||
self.waypoints = [self.grid_to_marker(r, c) for r, c in path]
|
||
self.waypoint_idx = 1
|
||
self._reset_lidar_block_detector()
|
||
|
||
print(f"{announce_prefix}: {' → '.join(map(str, self.waypoints))}")
|
||
return True
|
||
|
||
def start_navigation(self):
|
||
if self._build_path_to_target('Маршрут'):
|
||
self._navigate_to_next_waypoint()
|
||
|
||
def _replan_from_current_marker(self, reason='Перестроение маршрута'):
|
||
self.cmd_pub.publish(Twist())
|
||
self.state = 'REPLAN'
|
||
if self._build_path_to_target(reason):
|
||
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}) -> '
|
||
f'{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_normalized = math.atan2(dy, dx)
|
||
candidates = [
|
||
raw_yaw_normalized - 2 * math.pi,
|
||
raw_yaw_normalized,
|
||
raw_yaw_normalized + 2 * math.pi,
|
||
]
|
||
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._reset_lidar_block_detector()
|
||
|
||
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
|
||
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°
|
||
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':
|
||
blocked_marker = self._check_next_waypoint_occupied()
|
||
if blocked_marker is not None:
|
||
self._mark_dynamic_blocked(blocked_marker, self._last_front_range)
|
||
self._replan_from_current_marker(
|
||
f'Перестроение маршрута после обнаружения препятствия на {blocked_marker}'
|
||
)
|
||
self.cmd_pub.publish(Twist())
|
||
return
|
||
|
||
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':
|
||
blocked_marker = self._check_next_waypoint_occupied()
|
||
if blocked_marker is not None:
|
||
self._mark_dynamic_blocked(blocked_marker, self._last_front_range)
|
||
self._replan_from_current_marker(
|
||
f'Перестроение маршрута: следующий маркер {blocked_marker} занят'
|
||
)
|
||
self.cmd_pub.publish(Twist())
|
||
return
|
||
|
||
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
|
||
twist.linear.x = DRIVE_SPEED
|
||
if use_visual:
|
||
twist.angular.z = K_HEADING * base_yaw_err - K_VISUAL * self._visual_err_x_memory
|
||
else:
|
||
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:
|
||
lidar_msg = 'n/a' if self._last_front_range is None else f'{self._last_front_range:.2f} m'
|
||
self.get_logger().info(
|
||
f'[VIS-MEM] err_x={self._visual_err_x_memory:+.3f}, '
|
||
f'yaw={math.degrees(base_yaw_err):+.1f}°, '
|
||
f'tw.z={twist.angular.z:+.3f}, lidar={lidar_msg}'
|
||
)
|
||
|
||
# ── Визуальное центрирование над финальным маркером ─────────────────
|
||
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)
|
||
err_y = (mcy - h / 2.0) / (h / 2.0)
|
||
|
||
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}). '
|
||
'Выравниваю ориентацию.'
|
||
)
|
||
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()
|