first commit
This commit is contained in:
527
nav_test.py
Normal file
527
nav_test.py
Normal file
@@ -0,0 +1,527 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user