diff --git a/a.ipynb b/a.ipynb deleted file mode 100644 index e69de29..0000000 diff --git a/autopilot.py b/autopilot.py index 8f727b1..46f63aa 100644 --- a/autopilot.py +++ b/autopilot.py @@ -58,20 +58,28 @@ class AutoPilot(Pilot): self.image_center = (0, 0) # Будет обновлено при получении первого изображения self.vis_manager = viz_manager # Менеджер визуализации + # Пороговые значения качества сопоставления/гомографии + self.min_inliers: int = 12 + self.min_inlier_ratio: float = 0.5 + self.max_reproj_rmse: float = 3.0 + self.min_scale: float = 0.7 + self.max_scale: float = 1.4 + # Инициализация ORB детектора self.orb_detector = cv2.ORB_create( - nfeatures=1000, + nfeatures=3000, scaleFactor=1.2, nlevels=8, - edgeThreshold=31, + edgeThreshold=15, firstLevel=0, WTA_K=2, patchSize=31, - fastThreshold=20 + fastThreshold=8, + scoreType=cv2.ORB_HARRIS_SCORE ) - # Инициализация матчера для сопоставления ключевых точек - self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) + # Инициализация матчера для сопоставления ключевых точек (kNN + Lowe ratio) + self.bf_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) self.points = points self.keypoints = keypoints @@ -88,31 +96,44 @@ class AutoPilot(Pilot): """ Обнаруживает ключевые точки ORB и сопоставляет их между двумя изображениями """ - # Обнаружение ключевых точек и дескрипторов - kp1, des1 = self.orb_detector.detectAndCompute(img1, None) - kp2, des2 = self.orb_detector.detectAndCompute(img2, None) + # Предобработка (повышение контраста и устойчивости) + def preprocess(gray_img: np.ndarray) -> np.ndarray: + clahe = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)) + return clahe.apply(gray_img) - if des1 is None or des2 is None: + # В градации серого + g1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) if len(img1.shape) == 3 else img1 + g2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) if len(img2.shape) == 3 else img2 + g1 = preprocess(g1) + g2 = preprocess(g2) + + # Обнаружение ключевых точек и дескрипторов + kp1, des1 = self.orb_detector.detectAndCompute(g1, None) + kp2, des2 = self.orb_detector.detectAndCompute(g2, None) + + if des1 is None or des2 is None or len(kp1) < 4 or len(kp2) < 4: return None, None, None, None, None - # Сопоставление ключевых точек - matches = self.bf_matcher.match(des1, des2) + # kNN сопоставление и тест Лоу + matches_knn = self.bf_matcher.knnMatch(des1, des2, k=2) + good_matches: list[cv2.DMatch] = [] + for m_n in matches_knn: + if len(m_n) < 2: + continue + m, n = m_n + if m.distance < 0.75 * n.distance: + good_matches.append(m) - # Сортировка совпадений по расстоянию - matches = sorted(matches, key=lambda x: x.distance) - - # Фильтрация хороших совпадений (расстояние меньше порога) - good_matches = [] - for match in matches: - if match.distance < 50: # Порог расстояния - good_matches.append(match) + # Дополнительная фильтрация по расстоянию (мягкий порог) + good_matches = sorted(good_matches, key=lambda x: x.distance) + good_matches = [m for m in good_matches if m.distance < 64] if len(good_matches) < 4: return None, None, None, None, None # Получаем центр изображения - height1, width1 = img1.shape[:2] - height2, width2 = img2.shape[:2] + height1, width1 = g1.shape[:2] + height2, width2 = g2.shape[:2] center_x1, center_y1 = width1 // 2, height1 // 2 center_x2, center_y2 = width2 // 2, height2 // 2 @@ -147,6 +168,24 @@ class AutoPilot(Pilot): if H is None: return None + # Маска инлайеров + inliers = int(mask.sum()) if mask is not None else 0 + total = int(mask.size) if mask is not None else 0 + inlier_ratio = (inliers / total) if total > 0 else 0.0 + + # Ошибка репроекции по инлайерам (RMSE) + rmse = None + try: + # Проецируем src через H и сравниваем с dst + proj = cv2.perspectiveTransform(src_pts, H) + errs = np.linalg.norm((proj - dst_pts).reshape(-1, 2), axis=1) + if mask is not None and mask.shape[0] == errs.shape[0]: + errs = errs[mask.ravel().astype(bool)] + if errs.size > 0: + rmse = float(np.sqrt(np.mean(errs ** 2))) + except Exception: + rmse = None + # Извлекаем параметры трансформации из матрицы гомографии # H = [a11 a12 tx] # [a21 a22 ty] @@ -174,7 +213,10 @@ class AutoPilot(Pilot): 'rotation': angle, 'scale': scale, 'homography': H, - 'mask': mask + 'mask': mask, + 'inliers': inliers, + 'inliers_ratio': inlier_ratio, + 'rmse': rmse } def update_drone_position(self, transformation_info: dict): @@ -189,6 +231,9 @@ class AutoPilot(Pilot): # Координаты уже отцентрированы, поэтому используем их напрямую dx_meters = ty dy_meters = tx + + # Обновляем угол БПЛА + self.angle += rotation # Применяем поворот к смещению (учитываем текущий угол БПЛА) cos_angle = math.cos(self.angle) @@ -203,9 +248,6 @@ class AutoPilot(Pilot): self.x += dx_global self.y += dy_global - # Обновляем угол БПЛА - self.angle += rotation - # Нормализуем угол в диапазоне [-π, π] self.angle = math.atan2(math.sin(self.angle), math.cos(self.angle)) @@ -221,41 +263,6 @@ class AutoPilot(Pilot): 'frame_count': self.frame_count } - def visualize_matches(self, img1: np.ndarray, img2: np.ndarray, - kp1, kp2, matches, transformation_info): - """ - Визуализирует сопоставленные ключевые точки и трансформацию - """ - # Рисуем сопоставления - img_matches = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, - flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS) - - # Добавляем информацию о трансформации - if transformation_info: - tx, ty = transformation_info['translation'] - angle = transformation_info['rotation'] - scale = transformation_info['scale'] - - info_text = f"Translation: ({tx:.2f}, {ty:.2f})" - info_text2 = f"Rotation: {angle:.2f} rad ({np.degrees(angle):.1f}°)" - info_text3 = f"Scale: {scale:.2f}" - info_text4 = f"Image Center: {self.image_center}" - - cv2.putText(img_matches, info_text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - cv2.putText(img_matches, info_text2, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - cv2.putText(img_matches, info_text3, (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - cv2.putText(img_matches, info_text4, (10, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) - - # Добавляем информацию о позиции БПЛА - drone_state = self.get_drone_state() - pos_text = f"Drone Pos: ({drone_state['x']:.2f}, {drone_state['y']:.2f})" - angle_text = f"Drone Angle: {drone_state['angle_degrees']:.1f}°" - - cv2.putText(img_matches, pos_text, (10, 150), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2) - cv2.putText(img_matches, angle_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2) - - return img_matches - def handle(self, image: Image) -> PilotCommand: self.frame_count += 1 @@ -294,13 +301,11 @@ class AutoPilot(Pilot): transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts) if transformation_info: - # Обновляем позицию и угол БПЛА + # Обновляем позицию и угол БПЛА (одометрия по кадрам) self.update_drone_position(transformation_info) # Выводим текущее состояние БПЛА drone_state = self.get_drone_state() - if self.vis_manager: - self.vis_manager.update_drone_trajectory(drone_state['x'], drone_state['y']) print(f" [Pilot] Drone Position: ({drone_state['x']:.2f}, {drone_state['y']:.2f})") print(f" [Pilot] Angle: {drone_state['angle_degrees']:.1f}°") print(f" [Pilot] Target Index: {self.target_idx}") @@ -322,13 +327,43 @@ class AutoPilot(Pilot): # Пытаемся найти ориентир на картинке: landmark_image = cv2.imread(Path('chunks') / f'chunk_{self.target_idx}.png', cv2.IMREAD_COLOR_RGB) src_pts, dst_pts, matches, kp1, kp2 = self.detect_and_match_keypoints(current_image, landmark_image) - # if src_pts is not None and dst_pts is not None: - # # Оцениваем матрицу трансформации - # transformation_info = self.estimate_transformation_matrix(src_pts, dst_pts) - # if self.vis_manager: - # self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches, transformation_info) - if distance_to_target < 50: + if src_pts is not None and dst_pts is not None and self.vis_manager: + self.vis_manager.update_chunk_matches(current_image, landmark_image, kp1, kp2, matches) + + if src_pts is not None and dst_pts is not None: + # Оцениваем матрицу трансформации + landmark_transform = self.estimate_transformation_matrix(src_pts, dst_pts) + # Если ориентир достоверно найден — скорректируем глобальные координаты и угол + if landmark_transform is not None: + ok_scale = (self.min_scale <= landmark_transform['scale'] <= self.max_scale) + ok_inliers = (landmark_transform.get('inliers', 0) >= self.min_inliers) + ratio = landmark_transform.get('inliers_ratio', 0.0) + ok_ratio = (ratio >= self.min_inlier_ratio) + rmse = landmark_transform.get('rmse', None) + ok_rmse = (rmse is not None and rmse <= self.max_reproj_rmse) + if ok_scale and ok_inliers and ok_ratio and ok_rmse: + print("[PILOT]", rmse, ratio, ok_rmse) + # if False: + # Считаем абсолютную позу относительно координат ориентира + landmark_world_x, landmark_world_y = self.points[self.target_idx] + tx, ty = landmark_transform['translation'] + # Смещение в системе БПЛА: (dx_meters, dy_meters) + dx_meters = ty + dy_meters = tx + # Используем оценённый абсолютный угол из гомографии относительно карты (север-вверх) + absolute_angle = landmark_transform['rotation'] + cos_a = math.cos(absolute_angle) + sin_a = math.sin(absolute_angle) + # Переход в глобальные координаты карты + dx_global = dx_meters * cos_a - dy_meters * sin_a + dy_global = dx_meters * sin_a + dy_meters * cos_a + self.x = landmark_world_x + dx_global + self.y = landmark_world_y + dy_global + self.angle = math.atan2(math.sin(absolute_angle), math.cos(absolute_angle)) + print(f" [Pilot] Landmark correction applied (inliers={landmark_transform['inliers']}, ratio={ratio:.2f}, rmse={rmse:.2f})") + + if distance_to_target < 75: self.target_idx += 1 if self.target_idx == len(self.points): diff --git a/main.py b/main.py index c3142a6..0c03ad5 100644 --- a/main.py +++ b/main.py @@ -29,27 +29,28 @@ def main(): # make_global_photo('map.jpg') # Получаем траекторию от пользователя - points = get_trajectory_points('map.jpg') - print(points) + # points = get_trajectory_points('map.jpg') + # print(points) # points = [np.float64(0.5443937502226799), np.float64(0.4030424838774785)], [np.float64(0.18517133490120316), np.float64(0.4586935604608052)], [np.float64(0.1641887171838272), np.float64(0.5586383510594329)], [np.float64(0.587198290366127), # np.float64(0.5699957136274587)] + points = [[np.float64(0.481445897070552), np.float64(0.49958006570569835)], [np.float64(0.5485902737661552), np.float64(0.4325716265543457)], [np.float64(0.5376793125531197), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.6052035375883389)], [np.float64(0.34044270600978543), np.float64(0.3689703961734008)], [np.float64(0.6333600493443542), np.float64(0.3224052096444948)], [np.float64(0.6686108471095458), np.float64(0.5518239335186174)]] # Для каждой точки сделаем приближенный снимок yandexMap = YandexMap() keypoints: list[(any, any)] = [] - plt.ion() - for i in range(len(points)): - point = points[i] - yandexMap.scroll(point[0], point[1], 5, True) - sleep(1) - img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2) - Path('chunks').mkdir(exist_ok=True) - cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img) - plt.subplot(1, len(points), i+1) - plt.imshow(img) - plt.pause(0.25) - yandexMap.scroll(point[0], point[1], 5, False) + # plt.ion() + # for i in range(len(points)): + # point = points[i] + # yandexMap.scroll(point[0], point[1], 5, True) + # sleep(1) + # img = yandexMap.make_screenshot(point[0], point[1], 0.2, 0.2) + # Path('chunks').mkdir(exist_ok=True) + # cv2.imwrite(Path('.') / 'chunks' / f'chunk_{i}.png', img) + # plt.subplot(1, len(points), i+1) + # plt.imshow(img) + # plt.pause(0.25) + # yandexMap.scroll(point[0], point[1], 5, False) plt.tight_layout() @@ -81,9 +82,9 @@ def main(): # Начнём симуляцию полёта с первой точки yandexMap.scroll(points[0][0], points[0][1], 5, True) - sleep(1) + sleep(0.2) yandexMap.make_as_center(*points[0]) - sleep(5) + sleep(1) vis_manager = VisualizationManager() width, height = yandexMap.get_size() @@ -97,11 +98,11 @@ def main(): photo = simulator.get_photo() command = pilot.handle(photo) - print("DEBUG SIZE:", yandexMap.get_size()) - print("DEBUG SIZE:", photo.size) vis_manager.update_display() - vis_manager.pause(5) + vis_manager.pause(1) + + vis_manager.set_target_points(points_coords) for i in range(10000000000): photo = simulator.get_photo() @@ -116,11 +117,17 @@ def main(): if command.stop: break + vis_manager.set_target_index(pilot.target_idx) + vis_manager.update_drone_trajectory(pilot.x, pilot.y) + vis_manager.update_global_map(simulator.current_x, simulator.current_y) + vis_manager.update_error_plot(i, pilot.x, pilot.y, simulator.current_x, simulator.current_y) + simulator.handle(command.dangle, command.velocity) + vis_manager.update_display() vis_manager.pause(0.2) - vis_manager.pause(50) - sleep(30) + + vis_manager.show_final() if __name__ == "__main__": main() \ No newline at end of file diff --git a/map.jpg b/map.jpg new file mode 100644 index 0000000..3a18e86 Binary files /dev/null and b/map.jpg differ diff --git a/test_autopilot.ipynb b/test_autopilot.ipynb index 8cb53bc..8a9dfe6 100644 --- a/test_autopilot.ipynb +++ b/test_autopilot.ipynb @@ -10,68 +10,77 @@ "output_type": "stream", "text": [ "\n", - " [Pilot] translate: -1.1016611435961554 -11.000654444967525\n", - " [Pilot] Drone Position: (11.00, 1.10)\n", - " [Pilot] Angle: 5.7°\n", + " [Pilot] translate: 0.05017116751607286 9.687502397756333\n", + " [Pilot] Drone Position: (9.63, 1.05)\n", + " [Pilot] Angle: 5.9°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", " [Pilot] Distance: 1802.7756377319947\n", + " [Pilot] translate: 34.49815603085627 29.137861884347547\n", "\n", - " [Pilot] translate: -1.3733859493278928 -62.101186076671574\n", - " [Pilot] Drone Position: (72.66, 8.66)\n", - " [Pilot] Angle: 7.0°\n", + " [Pilot] translate: -3.880257616756106 51.463125720098006\n", + " [Pilot] Drone Position: (60.82, 7.61)\n", + " [Pilot] Angle: 11.6°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1793.018946479205\n", + " [Pilot] Distance: 1794.1861357457751\n", + " [Pilot] translate: -81.10123958005454 -137.34212482489963\n", "\n", - " [Pilot] translate: -0.286107956133153 -61.700187952027996\n", - " [Pilot] Drone Position: (133.86, 16.44)\n", - " [Pilot] Angle: 7.3°\n", + " [Pilot] translate: -4.965415715097187 51.25399608516978\n", + " [Pilot] Drone Position: (111.23, 18.11)\n", + " [Pilot] Angle: 17.3°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1737.8358192949638\n", + " [Pilot] Distance: 1748.165148948221\n", + " [Pilot] translate: 45.12386596220679 -80.30038654495203\n", "\n", - " [Pilot] translate: -1.217279449147543 -61.59798129394066\n", - " [Pilot] Drone Position: (194.81, 25.43)\n", - " [Pilot] Angle: 8.4°\n", + " [Pilot] translate: -4.306576877574656 51.03219412293733\n", + " [Pilot] Drone Position: (159.88, 34.13)\n", + " [Pilot] Angle: 23.1°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1683.363020511812\n", + " [Pilot] Distance: 1700.8189617850037\n", + " [Pilot] translate: 21.716104890355993 -140.57795924978092\n", "\n", - " [Pilot] translate: -0.5202028232620485 -61.25377666064388\n", - " [Pilot] Drone Position: (255.34, 34.87)\n", - " [Pilot] Angle: 8.9°\n", + " [Pilot] translate: -4.698241337005581 50.82696807577958\n", + " [Pilot] Drone Position: (206.68, 54.50)\n", + " [Pilot] Angle: 28.8°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1628.8954458349883\n", + " [Pilot] Distance: 1651.9184793917282\n", + " [Pilot] translate: -105.83895859495036 -240.1764870881905\n", "\n", - " [Pilot] translate: 0.030834225184026082 -62.02734738952944\n", - " [Pilot] Drone Position: (316.63, 44.41)\n", - " [Pilot] Angle: 8.8°\n", + " [Pilot] translate: -4.958766997526737 51.41923183115156\n", + " [Pilot] Drone Position: (251.86, 79.55)\n", + " [Pilot] Angle: 34.5°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1575.0091545970413\n", + " [Pilot] Distance: 1602.0752180361662\n", + " [Pilot] translate: -148.22727851217576 277.70124157634604\n", "\n", - " [Pilot] translate: -0.4160221189622985 -62.497522722978935\n", - " [Pilot] Drone Position: (378.32, 54.42)\n", - " [Pilot] Angle: 9.2°\n", + " [Pilot] translate: -3.092076978507596 51.453303771043075\n", + " [Pilot] Drone Position: (294.20, 108.95)\n", + " [Pilot] Angle: 38.2°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1521.0283267047403\n", + " [Pilot] Distance: 1550.834822541291\n", + " [Pilot] translate: 84.73919078536481 6.965084966855142\n", "\n", - " [Pilot] translate: 0.5176443048484498 -60.8909129531913\n", - " [Pilot] Drone Position: (438.51, 63.67)\n", - " [Pilot] Angle: 8.7°\n", + " [Pilot] translate: 0.6990361118758528 51.266860568260554\n", + " [Pilot] Drone Position: (333.90, 141.39)\n", + " [Pilot] Angle: 38.5°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1467.065793978559\n", + " [Pilot] Distance: 1499.310186566892\n", + " [Pilot] translate: -169.29950811743842 109.96288683148794\n", "\n", - " [Pilot] translate: -0.22161188590699135 -62.49031949249053\n", - " [Pilot] Drone Position: (500.24, 73.38)\n", - " [Pilot] Angle: 8.9°\n", + " [Pilot] translate: 0.06894026606203722 51.71176082802062\n", + " [Pilot] Drone Position: (374.40, 173.54)\n", + " [Pilot] Angle: 38.4°\n", " [Pilot] Target Index: 0\n", " [Pilot] Target Position: (1500, 1000)\n", - " [Pilot] Distance: 1415.4431252709192\n" + " [Pilot] Distance: 1448.1010213418751\n", + " [Pilot] translate: -175.2651402920743 -178.56804299442655\n" ] } ], diff --git a/todo.md b/todo.md new file mode 100644 index 0000000..f19db0f --- /dev/null +++ b/todo.md @@ -0,0 +1,13 @@ +[-] График межкадрового смещения +[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром +[-] Проверка корректности выявления ориентира на кадре +[-] Исправление коррекции координат на основе сопоставления с ориентиром +[-] FPS счетчик +[-] Устранение большой погрешности при повороте +[-] Оформление статистики при тестовых запусках +[-] Проведение тестовых запусков +[-] Оформление отчета + +[?] Изменение масштаба во время полёта, обработка этой трансформации +[?] Поворот ориентиров +[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр diff --git a/visualization.py b/visualization.py index 9e04e7d..f4a5757 100644 --- a/visualization.py +++ b/visualization.py @@ -36,10 +36,12 @@ class VisualizationManager: # Данные для глобальной карты self.trajectory_x = [] self.trajectory_y = [] - self.trajectory_modes = [] self.current_x = 0.0 self.current_y = 0.0 + self.target_idx = 0 + self.target_pts = [] + # Данные для траектории БПЛА (его собственное видение) self.drone_trajectory_x = [] self.drone_trajectory_y = [] @@ -65,7 +67,7 @@ class VisualizationManager: self.fig.canvas.manager.window.state('zoomed') # Создаем сетку 2x2 с разными размерами колонок - gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[.5, 1, 1]) + gs = self.fig.add_gridspec(2, 3, hspace=0.3, wspace=0.3, width_ratios=[1, 0.7, 1]) # График погрешности позиции (левый верхний угол) self.ax_error_plot = self.fig.add_subplot(gs[0, 0]) @@ -89,7 +91,7 @@ class VisualizationManager: self.ax_detection.axis('off') # Сопоставление точек (правый нижний угол) - self.ax_matches = self.fig.add_subplot(gs[1, 1]) + self.ax_matches = self.fig.add_subplot(gs[1, 2]) self.ax_matches.set_title('Feature Matching') self.ax_matches.axis('off') @@ -103,14 +105,21 @@ class VisualizationManager: plt.tight_layout() plt.show(block=False) + + def set_target_points(self, target_pts): + """ Обновление списка координат целевых точек """ + self.target_pts = target_pts + + def set_target_index(self, target_idx): + """ Обновление номера целевой точки """ + self.target_idx = target_idx - def update_global_map(self, x: float, y: float, mode: SimMode): + def update_global_map(self, x: float, y: float): """Обновляет глобальную карту""" self.current_x = x self.current_y = y self.trajectory_x.append(x) self.trajectory_y.append(y) - self.trajectory_modes.append(mode) self.ax_global_map.clear() self.ax_global_map.set_title('Global Map - Траектория полета беспилотника') @@ -121,43 +130,41 @@ class VisualizationManager: self.ax_global_map.axvline(x=0, color='k', linestyle='-', alpha=0.3) if len(self.trajectory_x) > 1: - # Разделяем траекторию по режимам - operator_indices = [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.OPERATOR] - autonome_indices = operator_indices[-1:] + [i for i, m in enumerate(self.trajectory_modes) if m == SimMode.AUTONOME] - # Рисуем траекторию оператора (синий цвет) - if len(operator_indices) > 1: - operator_x = [self.trajectory_x[i] for i in operator_indices] - operator_y = [self.trajectory_y[i] for i in operator_indices] - self.ax_global_map.plot(operator_x, operator_y, 'b-', linewidth=2, label='Режим оператора') - - # Рисуем траекторию автономного режима (красный цвет) - if len(autonome_indices) > 1: - autonome_x = [self.trajectory_x[i] for i in autonome_indices] - autonome_y = [self.trajectory_y[i] for i in autonome_indices] - self.ax_global_map.plot(autonome_x, autonome_y, 'r-', linewidth=2, label='Автономный режим') - + self.ax_global_map.plot(self.trajectory_x, self.trajectory_y, 'b-', linewidth=2, label='Режим оператора') + # Рисуем траекторию БПЛА (пунктирная линия, тонкая) if len(self.drone_trajectory_x) > 1: self.ax_global_map.plot(self.drone_trajectory_x, self.drone_trajectory_y, 'g--', linewidth=1, alpha=0.7, label='Данные по одометрии') - - # Рисуем начальную точку (зеленая) - self.ax_global_map.plot(self.trajectory_x[0], self.trajectory_y[0], 'go', markersize=8, label='Начальная точка') - + # Рисуем текущую позицию (черная) self.ax_global_map.plot(self.current_x, self.current_y, 'ko', markersize=6, label='Текущая позиция') - - # Рисуем целевую точку (0, 0) - желтая - self.ax_global_map.plot(0, 0, 'yo', markersize=8, label='Цель (0, 0)') - + + # Рисуем ориентиры + for i in range(len(self.target_pts)): + if i != self.target_idx: + pt = self.target_pts[i] + self.ax_global_map.plot(pt[0], pt[1], 'go', markersize=8) + + # Рисуем текущую целевую точку + if self.target_idx < len(self.target_pts): + pt = self.target_pts[self.target_idx] + self.ax_global_map.plot(pt[0], pt[1], 'yo', markersize=8, label='Цель (0, 0)') + self.ax_global_map.legend() - + # Автоматически масштабируем оси if len(self.trajectory_x) > 0: margin = 50 x_min, x_max = min(self.trajectory_x), max(self.trajectory_x) y_min, y_max = min(self.trajectory_y), max(self.trajectory_y) + + for pt in self.target_pts: + x_min = min(x_min, pt[0]) + x_max = max(x_max, pt[0]) + y_min = min(y_min, pt[1]) + y_max = max(y_max, pt[1]) # Учитываем также траекторию БПЛА при масштабировании if len(self.drone_trajectory_x) > 0: