528 lines
24 KiB
Python
528 lines
24 KiB
Python
#!/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 <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_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()
|