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

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: