zenloger/autopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ref: test version

Browse Source
This commit is contained in:
russian_proger
2025-10-04 14:04:32 +03:00
parent 7610fc6f50
commit dc8c869bcf
7 changed files with 225 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
a.ipynb
View File

173
autopilot.py
View File

@@ -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):

49
main.py
View File

@@ -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()

BIN
map.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.8 MiB

79
test_autopilot.ipynb
View File

@@ -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"
]
}
],

13
todo.md Normal file
View File

@@ -0,0 +1,13 @@
[-] График межкадрового смещения
[-] Отображение bounding box на графике сопоставления точек кадра с ориентиром
[-] Проверка корректности выявления ориентира на кадре
[-] Исправление коррекции координат на основе сопоставления с ориентиром
[-] FPS счетчик
[-] Устранение большой погрешности при повороте
[-] Оформление статистики при тестовых запусках
[-] Проведение тестовых запусков
[-] Оформление отчета
[?] Изменение масштаба во время полёта, обработка этой трансформации
[?] Поворот ориентиров
[?] Ограничение выбора точек, чтобы ориентиры полностью попадали в кадр

65
visualization.py
View File

@@ -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: