222 lines
8.6 KiB
Python
222 lines
8.6 KiB
Python
"""
|
||
Примеры использования системы отслеживания БПЛА
|
||
"""
|
||
|
||
import numpy as np
|
||
from PIL import Image
|
||
import cv2
|
||
from autopilot import AutoPilot
|
||
import matplotlib.pyplot as plt
|
||
|
||
def example_basic_usage():
|
||
"""Базовый пример использования автопилота"""
|
||
print("=== Базовый пример использования ===")
|
||
|
||
# Создаем автопилот с начальными параметрами
|
||
autopilot = AutoPilot(
|
||
initial_x=0.0, # Начальная X координата
|
||
initial_y=0.0, # Начальная Y координата
|
||
initial_altitude=15.0 # Начальная высота
|
||
)
|
||
|
||
# Создаем тестовые изображения
|
||
img1 = create_simple_test_image()
|
||
img2 = create_simple_test_image(offset_x=20, offset_y=10)
|
||
|
||
# Обрабатываем изображения
|
||
autopilot.handle(img1)
|
||
autopilot.handle(img2)
|
||
|
||
# Получаем состояние
|
||
state = autopilot.get_drone_state()
|
||
print(f"Позиция БПЛА: ({state['x']:.2f}, {state['y']:.2f})")
|
||
print(f"Угол БПЛА: {state['angle_degrees']:.1f}°")
|
||
print(f"Высота: {state['altitude']:.1f}м")
|
||
print(f"Обработано кадров: {state['frame_count']}")
|
||
|
||
def example_altitude_change():
|
||
"""Пример изменения высоты и пересчета масштаба"""
|
||
print("\n=== Пример изменения высоты ===")
|
||
|
||
autopilot = AutoPilot(initial_altitude=10.0)
|
||
|
||
# Создаем изображения
|
||
img1 = create_simple_test_image()
|
||
img2 = create_simple_test_image(offset_x=10, offset_y=5)
|
||
|
||
# Обрабатываем при высоте 10м
|
||
autopilot.handle(img1)
|
||
autopilot.handle(img2)
|
||
|
||
state1 = autopilot.get_drone_state()
|
||
print(f"При высоте 10м: ({state1['x']:.2f}, {state1['y']:.2f})")
|
||
|
||
# Изменяем высоту на 20м
|
||
autopilot.set_altitude(20.0)
|
||
|
||
# Обрабатываем те же изображения при новой высоте
|
||
autopilot.reset_position()
|
||
autopilot.handle(img1)
|
||
autopilot.handle(img2)
|
||
|
||
state2 = autopilot.get_drone_state()
|
||
print(f"При высоте 20м: ({state2['x']:.2f}, {state2['y']:.2f})")
|
||
|
||
# Коэффициент должен измениться в 2 раза
|
||
ratio = state2['x'] / state1['x'] if state1['x'] != 0 else 0
|
||
print(f"Коэффициент изменения: {ratio:.2f} (ожидается ~2.0)")
|
||
|
||
def example_circular_movement():
|
||
"""Пример отслеживания кругового движения"""
|
||
print("\n=== Пример кругового движения ===")
|
||
|
||
autopilot = AutoPilot()
|
||
|
||
# Создаем базовое изображение
|
||
base_img = create_simple_test_image()
|
||
|
||
# Параметры кругового движения
|
||
radius = 30
|
||
center_x, center_y = 320, 240
|
||
num_steps = 8
|
||
|
||
x_history = []
|
||
y_history = []
|
||
angle_history = []
|
||
|
||
# Симулируем круговое движение
|
||
for i in range(num_steps):
|
||
angle = 2 * np.pi * i / num_steps
|
||
|
||
# Вычисляем смещение
|
||
dx = radius * np.cos(angle)
|
||
dy = radius * np.sin(angle)
|
||
|
||
# Создаем смещенное изображение
|
||
current_img = create_simple_test_image(
|
||
offset_x=int(dx),
|
||
offset_y=int(dy),
|
||
rotation=angle * 0.1
|
||
)
|
||
|
||
# Обрабатываем
|
||
autopilot.handle(current_img)
|
||
|
||
# Сохраняем историю
|
||
state = autopilot.get_drone_state()
|
||
x_history.append(state['x'])
|
||
y_history.append(state['y'])
|
||
angle_history.append(state['angle_degrees'])
|
||
|
||
print(f"Шаг {i+1}: ({state['x']:.2f}, {state['y']:.2f}), угол: {state['angle_degrees']:.1f}°")
|
||
|
||
# Визуализируем траекторию
|
||
plot_circular_trajectory(x_history, y_history, angle_history)
|
||
|
||
def example_position_reset():
|
||
"""Пример сброса позиции"""
|
||
print("\n=== Пример сброса позиции ===")
|
||
|
||
autopilot = AutoPilot()
|
||
|
||
# Создаем и обрабатываем изображения
|
||
img1 = create_simple_test_image()
|
||
img2 = create_simple_test_image(offset_x=15, offset_y=10)
|
||
|
||
autopilot.handle(img1)
|
||
autopilot.handle(img2)
|
||
|
||
state_before = autopilot.get_drone_state()
|
||
print(f"До сброса: ({state_before['x']:.2f}, {state_before['y']:.2f})")
|
||
|
||
# Сбрасываем позицию
|
||
autopilot.reset_position(x=100.0, y=200.0, angle=np.pi/4)
|
||
|
||
state_after = autopilot.get_drone_state()
|
||
print(f"После сброса: ({state_after['x']:.2f}, {state_after['y']:.2f}), угол: {state_after['angle_degrees']:.1f}°")
|
||
|
||
def create_simple_test_image(offset_x=0, offset_y=0, rotation=0):
|
||
"""Создает простое тестовое изображение с заданным смещением"""
|
||
width, height = 640, 480
|
||
|
||
# Создаем базовое изображение с геометрическими фигурами
|
||
img = np.zeros((height, width, 3), dtype=np.uint8)
|
||
|
||
# Добавляем фигуры
|
||
cv2.rectangle(img, (100, 100), (200, 200), (255, 0, 0), -1)
|
||
cv2.circle(img, (400, 300), 50, (0, 255, 0), -1)
|
||
cv2.line(img, (50, 400), (550, 400), (0, 0, 255), 5)
|
||
cv2.rectangle(img, (300, 150), (350, 200), (255, 255, 0), -1)
|
||
|
||
# Применяем трансформацию
|
||
if offset_x != 0 or offset_y != 0 or rotation != 0:
|
||
cos_rot = np.cos(rotation)
|
||
sin_rot = np.sin(rotation)
|
||
|
||
M = np.float32([
|
||
[cos_rot, -sin_rot, offset_x],
|
||
[sin_rot, cos_rot, offset_y]
|
||
])
|
||
|
||
img = cv2.warpAffine(img, M, (width, height),
|
||
borderMode=cv2.BORDER_REPLICATE)
|
||
|
||
return Image.fromarray(img)
|
||
|
||
def plot_circular_trajectory(x_history, y_history, angle_history):
|
||
"""Визуализирует круговую траекторию"""
|
||
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6))
|
||
|
||
# График траектории
|
||
ax1.plot(x_history, y_history, 'b-o', linewidth=2, markersize=8)
|
||
ax1.plot(x_history[0], y_history[0], 'go', markersize=12, label='Начало')
|
||
ax1.plot(x_history[-1], y_history[-1], 'ro', markersize=12, label='Конец')
|
||
ax1.set_xlabel('X координата (м)')
|
||
ax1.set_ylabel('Y координата (м)')
|
||
ax1.set_title('Круговая траектория БПЛА')
|
||
ax1.grid(True)
|
||
ax1.legend()
|
||
ax1.axis('equal')
|
||
|
||
# График угла
|
||
ax2.plot(range(len(angle_history)), angle_history, 'r-o', linewidth=2, markersize=8)
|
||
ax2.set_xlabel('Номер шага')
|
||
ax2.set_ylabel('Угол поворота (градусы)')
|
||
ax2.set_title('Изменение угла поворота')
|
||
ax2.grid(True)
|
||
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
def example_error_handling():
|
||
"""Пример обработки ошибок и граничных случаев"""
|
||
print("\n=== Пример обработки ошибок ===")
|
||
|
||
autopilot = AutoPilot()
|
||
|
||
# Создаем изображение без текстуры (однородное)
|
||
blank_img = Image.fromarray(np.zeros((480, 640, 3), dtype=np.uint8))
|
||
|
||
# Обрабатываем - должно работать без ошибок
|
||
try:
|
||
autopilot.handle(blank_img)
|
||
autopilot.handle(blank_img)
|
||
print("Обработка однородного изображения прошла успешно")
|
||
except Exception as e:
|
||
print(f"Ошибка при обработке: {e}")
|
||
|
||
# Проверяем состояние
|
||
state = autopilot.get_drone_state()
|
||
print(f"Состояние после обработки: ({state['x']:.2f}, {state['y']:.2f})")
|
||
|
||
if __name__ == "__main__":
|
||
print("Примеры использования системы отслеживания БПЛА\n")
|
||
|
||
# Запускаем все примеры
|
||
example_basic_usage()
|
||
example_altitude_change()
|
||
example_circular_movement()
|
||
example_position_reset()
|
||
example_error_handling()
|
||
|
||
print("\nВсе примеры выполнены успешно!") |