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

feat: add rth map

Browse Source
This commit is contained in:
russian_proger
2026-05-31 14:07:34 +03:00
parent 52181aab88
commit e98cd9a588
4 changed files with 235 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
visualization.py
View File

@@ -3,9 +3,10 @@
Модуль для управления общим окном визуализации
"""
from PIL import Image
from enum import Enum
from scipy.interpolate import make_interp_spline
from PIL import Image
from enum import Enum
from pathlib import Path
from scipy.interpolate import make_interp_spline
import cv2
import matplotlib
@@ -31,8 +32,9 @@ class VisualizationManager:
self.window_title = window_title
self.fig = None
self.ax_error_plot = None # График погрешности позиции
self.ax_global_map = None
self.ax_detection = None
self.ax_global_map = None
self.ax_route_map = None
self.ax_detection = None
self.ax_matches = None
self.ax_chunk_matches = None
self.ax_motion_vectors = None
@@ -48,7 +50,18 @@ class VisualizationManager:
# Данные для траектории БПЛА (его собственное видение)
self.drone_trajectory_x = []
self.drone_trajectory_y = []
self.drone_trajectory_y = []
# Данные для RTH-карты
self.map_image = None
self.map_extent = None
self.forward_route = None
self.turn_point = None
self.start_point = None
self.final_point = None
self.final_return_error = None
self.rth_trajectory_x = []
self.rth_trajectory_y = []
# Данные для графика погрешности
self.error_times = []
@@ -95,9 +108,15 @@ class VisualizationManager:
self.ax_matches.axis('off')
# Сопоставление точек (средний средний угол)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 1:3])
self.ax_chunk_matches.set_title('Chunk Matching')
self.ax_chunk_matches.axis('off')
self.ax_route_map = self.fig.add_subplot(gs[1, 1])
self.ax_route_map.set_title('RTH Map - маршрут на карте')
self.ax_route_map.set_xlabel('X координата')
self.ax_route_map.set_ylabel('Y координата')
self.ax_route_map.grid(True, alpha=0.3)
self.ax_chunk_matches = self.fig.add_subplot(gs[1, 2])
self.ax_chunk_matches.set_title('Chunk Matching')
self.ax_chunk_matches.axis('off')
# Визуализация движения ключевых точек (левый нижний угол)
# self.ax_motion_vectors = self.fig.add_subplot(gs[1, 1])
@@ -112,16 +131,104 @@ class VisualizationManager:
# Настройки окна
self.fig.canvas.manager.window.attributes('-topmost', False)
plt.tight_layout()
plt.show(block=False)
self.fig.subplots_adjust(left=0.06, right=0.98, bottom=0.06, top=0.94, hspace=0.3, wspace=0.3)
plt.show(block=False)
def set_target_points(self, target_pts):
""" Обновление списка координат целевых точек """
self.target_pts = target_pts
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 set_target_index(self, target_idx):
""" Обновление номера целевой точки """
self.target_idx = target_idx
def set_route_map(self, map_path, map_extent, forward_route, turn_point, start_point):
"""Настраивает отдельный график маршрута на фоне карты."""
self.map_image = np.array(Image.open(map_path))
self.map_extent = map_extent
self.forward_route = np.array(forward_route)
self.turn_point = np.array([turn_point.x, turn_point.y]) if hasattr(turn_point, 'x') else np.array(turn_point)
self.start_point = np.array(start_point)
self._draw_route_map()
def update_rth_trajectory(self, x: float, y: float):
"""Добавляет точку обратного полета на карту RTH."""
self.rth_trajectory_x.append(x)
self.rth_trajectory_y.append(y)
self._draw_route_map()
def set_final_point(self, x: float, y: float, return_error: float | None = None):
"""Фиксирует последнюю точку и итоговую ошибку возврата."""
self.final_point = np.array([x, y])
self.final_return_error = return_error
self._draw_route_map()
def _draw_route_map(self):
if self.ax_route_map is None:
return
self.ax_route_map.clear()
self.ax_route_map.set_title('RTH Map - маршрут на карте')
self.ax_route_map.set_xlabel('X координата')
self.ax_route_map.set_ylabel('Y координата')
self.ax_route_map.grid(True, alpha=0.3)
if self.map_image is not None and self.map_extent is not None:
self.ax_route_map.imshow(self.map_image, extent=self.map_extent, origin='upper', alpha=0.85)
all_points = []
if self.forward_route is not None and len(self.forward_route) > 0:
self.ax_route_map.plot(
self.forward_route[:, 0],
self.forward_route[:, 1],
color='red',
linewidth=2,
label='Маршрут туда',
)
all_points.append(self.forward_route)
if len(self.rth_trajectory_x) > 0:
rth_points = np.column_stack([self.rth_trajectory_x, self.rth_trajectory_y])
self.ax_route_map.plot(
self.rth_trajectory_x,
self.rth_trajectory_y,
color='gold',
linewidth=2,
label='Маршрут обратно',
)
all_points.append(rth_points)
if self.start_point is not None:
self.ax_route_map.plot(self.start_point[0], self.start_point[1], 'go', markersize=8, label='Старт')
all_points.append(self.start_point.reshape(1, 2))
if self.turn_point is not None:
self.ax_route_map.plot(self.turn_point[0], self.turn_point[1], 'rx', markersize=10, markeredgewidth=2.5, label='RTH')
all_points.append(self.turn_point.reshape(1, 2))
if self.final_point is not None:
self.ax_route_map.plot(self.final_point[0], self.final_point[1], 'bo', markersize=8, label='Финиш')
all_points.append(self.final_point.reshape(1, 2))
if self.final_return_error is not None:
self.ax_route_map.text(
0.02,
0.98,
f"Ошибка возврата: {self.final_return_error:.2f}",
transform=self.ax_route_map.transAxes,
va='top',
fontsize=8,
bbox=dict(boxstyle="round,pad=0.3", facecolor="white", alpha=0.8),
)
if all_points:
points = np.vstack(all_points)
margin = 50
self.ax_route_map.set_xlim(points[:, 0].min() - margin, points[:, 0].max() + margin)
self.ax_route_map.set_ylim(points[:, 1].min() - margin, points[:, 1].max() + margin)
self.ax_route_map.legend(loc='best')
def update_global_map(self, x: float, y: float):
"""Обновляет глобальную карту"""
@@ -573,11 +680,33 @@ class VisualizationManager:
"""Закрывает окно"""
plt.close(self.fig)
def show_final(self):
"""Показывает финальное состояние окна"""
plt.ioff()
print("Симуляция завершена. Окно визуализации остается открытым для анализа.")
plt.pause(100000)
def pause(self, duration: float):
plt.pause(duration)
def show_final(self):
"""Показывает финальное состояние окна"""
plt.ioff()
print("Симуляция завершена. Окно визуализации остается открытым для анализа.")
plt.pause(100000)
def pause(self, duration: float):
plt.pause(duration)
def save_plots(self, output_dir: Path | str):
"""Сохраняет итоговое окно и отдельные графики в папку тестового запуска."""
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
self.fig.canvas.draw()
self.fig.savefig(output_dir / 'visualization.png', dpi=150, bbox_inches='tight')
axes = {
'error_plot.png': self.ax_error_plot,
'global_map.png': self.ax_global_map,
'rth_map.png': self.ax_route_map,
'keypoint_detection.png': self.ax_motion_gomography,
'feature_matching.png': self.ax_matches,
'chunk_matching.png': self.ax_chunk_matches,
}
renderer = self.fig.canvas.get_renderer()
for filename, axes_item in axes.items():
if axes_item is None:
continue
bbox = axes_item.get_tightbbox(renderer).expanded(1.08, 1.15)
self.fig.savefig(output_dir / filename, dpi=150, bbox_inches=bbox)