From 2c202af51f0d32882ecf69668d49265c3f9bc8c8 Mon Sep 17 00:00:00 2001 From: russian_proger Date: Mon, 30 Jun 2025 02:36:45 +0300 Subject: [PATCH] fix: return to center --- autopilot.py | 28 ++++++++++++++-------------- simulator.py | 15 ++++++++------- visualization.py | 41 ++++++++++++++++++++++++++++++++++------- 3 files changed, 56 insertions(+), 28 deletions(-) diff --git a/autopilot.py b/autopilot.py index 83b7e6c..9395ef2 100644 --- a/autopilot.py +++ b/autopilot.py @@ -9,7 +9,7 @@ random.seed(1) class Pilot: def __init__(self): pass def handle(self, image: Image): pass - def act(self) -> float | None: pass + def act(self) -> tuple[float, float] | None: pass class AutoPilot(Pilot): prev_image: np.ndarray | None @@ -89,11 +89,11 @@ class AutoPilot(Pilot): for match in good_matches: # Координаты точки в первом изображении pt1 = kp1[match.queryIdx].pt - src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1]) + src_pts.append([center_x1 - pt1[0], pt1[1] - center_y1]) # Координаты точки во втором изображении pt2 = kp2[match.trainIdx].pt - dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2]) + dst_pts.append([center_x2 - pt2[0], pt2[1] - center_y2]) # Конвертируем в numpy массивы src_pts = np.float32(src_pts).reshape(-1, 1, 2) @@ -125,7 +125,7 @@ class AutoPilot(Pilot): tx, ty = H[0, 2], H[1, 2] # Вычисляем угол поворота - angle = np.arctan2(a21, a11) + angle = -np.arctan2(a21, a11) # Вычисляем масштаб scale_x = np.sqrt(a11**2 + a21**2) @@ -150,8 +150,8 @@ class AutoPilot(Pilot): scale = transformation_info['scale'] # Координаты уже отцентрированы, поэтому используем их напрямую - dx_meters = tx - dy_meters = ty + dx_meters = ty + dy_meters = tx # Применяем поворот к смещению (учитываем текущий угол БПЛА) cos_angle = math.cos(self.angle) @@ -159,8 +159,8 @@ class AutoPilot(Pilot): # Поворачиваем смещение в глобальные координаты # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз - dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle - dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle + dx_global = dx_meters * cos_angle - dy_meters * sin_angle + dy_global = dx_meters * sin_angle + dy_meters * cos_angle # Обновляем координаты БПЛА self.x += dx_global @@ -276,7 +276,7 @@ class AutoPilot(Pilot): # Обновляем предыдущее изображение self.prev_image = current_image - def act(self) -> float | None: + def act(self) -> tuple[float, float] | None: """ Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0). Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None. @@ -284,8 +284,8 @@ class AutoPilot(Pilot): # Расстояние до цели (0, 0) distance_to_target = math.sqrt(self.x**2 + self.y**2) - # Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся - if distance_to_target < 1.0: + # Если дрон находится рядом с целью, останавливаемся + if distance_to_target < 10.0: return None # Вычисляем угол к цели @@ -302,7 +302,7 @@ class AutoPilot(Pilot): angle_diff -= 2 * math.pi # Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо) - return max(min(0.05, angle_diff / 2), -0.05) + return max(min(0.1, angle_diff), -0.1), min(10., distance_to_target / 2) def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0): """Сбрасывает позицию и угол БПЛА""" @@ -318,12 +318,12 @@ class RandomPilot(Pilot): def __init__(self, velocity: float = 1): self.counter = 0 - def act(self) -> float | None: + def act(self) -> tuple[float, float] | None: self.counter += 1 if self.counter > 40: return None - return 1 / (self.counter + 20) + return 1 / (self.counter + 20), 10.0 # def _test(): # randomPilot = RandomPilot() diff --git a/simulator.py b/simulator.py index 5d1e9d1..7612c64 100644 --- a/simulator.py +++ b/simulator.py @@ -107,26 +107,27 @@ class Simulator: html = self.driver.find_element(By.TAG_NAME, 'html') action = ActionChains(self.driver) - velocity = 10 # Добавляем начальную точку в траекторию self.update_trajectory(0, 0) self.viz_manager.update_global_map(self.current_x, self.current_y, self.mode) for i in range(1000): - dangle = None + signal = None if self.mode == SimMode.OPERATOR: - dangle = self.operatorPilot.act() - if dangle is None: + signal = self.operatorPilot.act() + if signal is None: self.mode = SimMode.AUTONOME print("Режим возвращения домой!") if self.mode == SimMode.AUTONOME: - dangle = self.autonomePilot.act() + signal = self.autonomePilot.act() - if dangle is None: + if signal is None: break + dangle, velocity = signal + # Сдвиг камеры action = ActionChains(self.driver) action.move_to_element_with_offset(html, 200, 200) @@ -148,7 +149,7 @@ class Simulator: im = im.crop([0, 80, im.width-80, im.height-60]) # Применяем поворот как будто съемка с дрона - rotated_im = self.rotate_image_like_drone(im, self.angle + math.pi / 2) + rotated_im = self.rotate_image_like_drone(im, math.pi / 2 - self.angle) # Передаем изображение в AutoPilot для анализа self.autonomePilot.handle(rotated_im) diff --git a/visualization.py b/visualization.py index f5f44e0..90e86d9 100644 --- a/visualization.py +++ b/visualization.py @@ -27,6 +27,7 @@ class VisualizationManager: def __init__(self, window_title="Drone Autopilot Visualization"): self.window_title = window_title self.fig = None + self.ax_drone_view = None self.ax_global_map = None self.ax_detection = None self.ax_matches = None @@ -43,6 +44,9 @@ class VisualizationManager: self.keypoints = [] self.matches = [] + # Данные для вида БПЛА + self.drone_view_image = None + self._setup_window() def _setup_window(self): @@ -54,11 +58,16 @@ class VisualizationManager: # Открываем окно на полный экран self.fig.canvas.manager.window.state('zoomed') - # Создаем сетку 2x2 - gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3) + # Создаем сетку 2x2 с разными размерами колонок + gs = self.fig.add_gridspec(2, 2, hspace=0.3, wspace=0.3, width_ratios=[1, 1]) - # Глобальная карта (левый верхний угол, занимает 2x1) - self.ax_global_map = self.fig.add_subplot(gs[0, :]) + # Вид БПЛА (левый верхний угол) + self.ax_drone_view = self.fig.add_subplot(gs[0, 0]) + self.ax_drone_view.set_title('Вид БПЛА') + self.ax_drone_view.axis('off') + + # Глобальная карта (левый нижний угол) + self.ax_global_map = self.fig.add_subplot(gs[1, 0]) self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') self.ax_global_map.set_xlabel('X координата') self.ax_global_map.set_ylabel('Y координата') @@ -66,8 +75,8 @@ class VisualizationManager: self.ax_global_map.axhline(y=0, color='k', linestyle='-', alpha=0.3) self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) - # Детекция ключевых точек (правый нижний угол) - self.ax_detection = self.fig.add_subplot(gs[1, 0]) + # Детекция ключевых точек (правый верхний угол) + self.ax_detection = self.fig.add_subplot(gs[0, 1]) self.ax_detection.set_title('Keypoint Detection') self.ax_detection.axis('off') @@ -100,7 +109,7 @@ class VisualizationManager: if len(self.trajectory_x) > 1: # Разделяем траекторию по режимам operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR] - autonome_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME] + autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME] # Рисуем траекторию оператора (синий цвет) if len(operator_indices) > 1: @@ -139,6 +148,24 @@ class VisualizationManager: self.ax_global_map.set_xlim(x_min - margin, x_max + margin) self.ax_global_map.set_ylim(y_min - margin, y_max + margin) + def update_drone_view(self, image: np.ndarray): + """Обновляет вид БПЛА""" + self.drone_view_image = image.copy() + + self.ax_drone_view.clear() + self.ax_drone_view.set_title('Вид БПЛА') + + if image is not None: + # Конвертируем BGR в RGB для matplotlib + if len(image.shape) == 3 and image.shape[2] == 3: + image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + else: + image_rgb = image + + self.ax_drone_view.imshow(image_rgb) + + self.ax_drone_view.axis('off') + def update_detection(self, image: np.ndarray, keypoints): """Обновляет визуализацию детекции ключевых точек""" self.current_frame = image.copy()