Merge GUI 1.0.1 from ken
This commit is contained in:
@@ -0,0 +1,299 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作
|
||||
|
||||
使用方式:
|
||||
1. 啟動 SITL
|
||||
2. 修改下方 CONFIG 區塊
|
||||
3. python3 test_mission.py
|
||||
"""
|
||||
import sys, os
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
|
||||
|
||||
import time
|
||||
from pymavlink import mavutil
|
||||
from PyQt6.QtWidgets import QApplication
|
||||
from PyQt6.QtCore import QTimer
|
||||
|
||||
from mission_planner import FormationPlanner, MissionType
|
||||
from command_sender import MavlinkSender
|
||||
from mission_executor import MissionExecutor, MissionState
|
||||
|
||||
|
||||
# ================================================================================
|
||||
# CONFIG
|
||||
# ================================================================================
|
||||
|
||||
# 接收用連線 (讀取無人機狀態)
|
||||
RECV_CONNECTION = "udp:127.0.0.1:14550"
|
||||
|
||||
# 發送用連線 (發送 setpoint 指令)
|
||||
SEND_CONNECTION = "udpout:127.0.0.1:14550"
|
||||
|
||||
# 要控制的無人機 sysid 列表
|
||||
DRONE_SYSIDS = [1]
|
||||
|
||||
# 起飛高度 (公尺)
|
||||
TAKEOFF_ALT = 10.0
|
||||
|
||||
# 任務規劃參數
|
||||
FORMATION_SPACING = 5.0
|
||||
BASE_ALTITUDE = 10.0
|
||||
ALTITUDE_DIFF = 2.0
|
||||
ARRIVAL_RADIUS = 2.0
|
||||
|
||||
# 測試模式: "formation" 或 "grid_sweep"
|
||||
TEST_MODE = "formation"
|
||||
|
||||
# Grid Sweep 專用設定
|
||||
GRID_LINE_SPACING = 5.0
|
||||
|
||||
# ================================================================================
|
||||
|
||||
|
||||
class SITLDroneManager:
|
||||
"""管理 SITL 無人機的連線、起飛前置作業"""
|
||||
|
||||
def __init__(self, connection_string, sysids):
|
||||
self.connection_string = connection_string
|
||||
self.sysids = sysids
|
||||
self.mav = None
|
||||
self.drone_gps = {}
|
||||
|
||||
def connect(self):
|
||||
"""建立 MAVLink 連線並等待心跳"""
|
||||
print(f"連線到 {self.connection_string} ...")
|
||||
self.mav = mavutil.mavlink_connection(self.connection_string)
|
||||
self.mav.wait_heartbeat()
|
||||
print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}")
|
||||
|
||||
def set_guided_and_arm(self, sysid):
|
||||
"""切換到 GUIDED 模式並解鎖"""
|
||||
print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---")
|
||||
|
||||
# 切換 GUIDED 模式
|
||||
self.mav.mav.command_long_send(
|
||||
sysid, 1,
|
||||
mavutil.mavlink.MAV_CMD_DO_SET_MODE,
|
||||
0,
|
||||
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
|
||||
4, # GUIDED = 4
|
||||
0, 0, 0, 0, 0
|
||||
)
|
||||
time.sleep(1)
|
||||
|
||||
# 解鎖
|
||||
self.mav.mav.command_long_send(
|
||||
sysid, 1,
|
||||
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
0, 1, 0, 0, 0, 0, 0, 0
|
||||
)
|
||||
time.sleep(1)
|
||||
print(f" sysid={sysid}: GUIDED + ARMED")
|
||||
|
||||
def takeoff(self, sysid, altitude):
|
||||
"""起飛到指定高度"""
|
||||
print(f" sysid={sysid}: 起飛到 {altitude}m ...")
|
||||
self.mav.mav.command_long_send(
|
||||
sysid, 1,
|
||||
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
|
||||
0, 0, 0, 0, 0, 0, 0, altitude
|
||||
)
|
||||
|
||||
def wait_for_altitude(self, sysid, target_alt, timeout=30):
|
||||
"""等待無人機到達指定高度"""
|
||||
print(f" sysid={sysid}: 等待到達 {target_alt}m ...")
|
||||
start = time.time()
|
||||
while time.time() - start < timeout:
|
||||
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
|
||||
if msg and msg.get_srcSystem() == sysid:
|
||||
alt = msg.relative_alt / 1000.0
|
||||
if alt >= target_alt * 0.9:
|
||||
print(f" sysid={sysid}: 已到達 {alt:.1f}m")
|
||||
return True
|
||||
print(f" sysid={sysid}: 等待超時!")
|
||||
return False
|
||||
|
||||
def update_gps_once(self):
|
||||
"""讀取一輪 GPS 資料更新 drone_gps"""
|
||||
deadline = time.time() + 3
|
||||
received = set()
|
||||
while time.time() < deadline and len(received) < len(self.sysids):
|
||||
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
|
||||
if msg is None:
|
||||
continue
|
||||
sid = msg.get_srcSystem()
|
||||
if sid in self.sysids:
|
||||
drone_id = f"s0_{sid}"
|
||||
self.drone_gps[drone_id] = {
|
||||
'lat': msg.lat / 1e7,
|
||||
'lon': msg.lon / 1e7,
|
||||
'alt': msg.relative_alt / 1000.0
|
||||
}
|
||||
received.add(sid)
|
||||
|
||||
for sid in self.sysids:
|
||||
drone_id = f"s0_{sid}"
|
||||
if drone_id in self.drone_gps:
|
||||
gps = self.drone_gps[drone_id]
|
||||
print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)")
|
||||
else:
|
||||
print(f" {drone_id}: 尚未收到 GPS")
|
||||
|
||||
def start_gps_polling(self, interval_ms=200):
|
||||
"""啟動定時 GPS 輪詢 (用 QTimer)"""
|
||||
self._gps_timer = QTimer()
|
||||
self._gps_timer.timeout.connect(self._poll_gps)
|
||||
self._gps_timer.start(interval_ms)
|
||||
|
||||
def _poll_gps(self):
|
||||
"""非阻塞方式讀取最新 GPS"""
|
||||
while True:
|
||||
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False)
|
||||
if msg is None:
|
||||
break
|
||||
sid = msg.get_srcSystem()
|
||||
if sid in self.sysids:
|
||||
drone_id = f"s0_{sid}"
|
||||
self.drone_gps[drone_id] = {
|
||||
'lat': msg.lat / 1e7,
|
||||
'lon': msg.lon / 1e7,
|
||||
'alt': msg.relative_alt / 1000.0
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
# 建立 Qt 應用 (MissionExecutor 需要 QTimer)
|
||||
app = QApplication(sys.argv)
|
||||
|
||||
# 連線 + 起飛前置作業
|
||||
manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS)
|
||||
manager.connect()
|
||||
|
||||
for sysid in DRONE_SYSIDS:
|
||||
manager.set_guided_and_arm(sysid)
|
||||
manager.takeoff(sysid, TAKEOFF_ALT)
|
||||
|
||||
# 等待所有無人機到達起飛高度
|
||||
for sysid in DRONE_SYSIDS:
|
||||
manager.wait_for_altitude(sysid, TAKEOFF_ALT)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
# 讀取當前 GPS 位置
|
||||
print("\n讀取當前 GPS 位置 ...")
|
||||
manager.update_gps_once()
|
||||
|
||||
drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS]
|
||||
drone_gps_positions = []
|
||||
for drone_id in drone_ids:
|
||||
gps = manager.drone_gps.get(drone_id)
|
||||
if gps is None:
|
||||
print(f"錯誤: 讀不到 {drone_id} 的 GPS")
|
||||
return
|
||||
drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt']))
|
||||
|
||||
# 規劃任務
|
||||
print(f"\n規劃任務 (模式: {TEST_MODE}) ...")
|
||||
planner = FormationPlanner(
|
||||
spacing=FORMATION_SPACING,
|
||||
base_altitude=BASE_ALTITUDE,
|
||||
altitude_diff=ALTITUDE_DIFF
|
||||
)
|
||||
|
||||
center_lat = drone_gps_positions[0][0]
|
||||
center_lon = drone_gps_positions[0][1]
|
||||
|
||||
if TEST_MODE == "formation":
|
||||
target_lat = center_lat + 30.0 / 111000.0
|
||||
target_lon = center_lon
|
||||
target_gps = (target_lat, target_lon, BASE_ALTITUDE)
|
||||
print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})")
|
||||
|
||||
waypoints_per_drone, origin = planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.M_FORMATION
|
||||
)
|
||||
|
||||
elif TEST_MODE == "grid_sweep":
|
||||
# 在無人機前方 30m 處建立 40m x 30m 的矩形
|
||||
offset_lat = 30.0 / 111000.0
|
||||
half_w = 20.0 / (111000.0 * 0.9)
|
||||
half_h = 15.0 / 111000.0
|
||||
|
||||
rect_center_lat = center_lat + offset_lat
|
||||
rect_center_lon = center_lon
|
||||
|
||||
rect_corners = [
|
||||
(rect_center_lat - half_h, rect_center_lon - half_w),
|
||||
(rect_center_lat - half_h, rect_center_lon + half_w),
|
||||
(rect_center_lat + half_h, rect_center_lon + half_w),
|
||||
(rect_center_lat + half_h, rect_center_lon - half_w),
|
||||
]
|
||||
target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE)
|
||||
print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})")
|
||||
|
||||
waypoints_per_drone, origin = planner.plan_formation_mission(
|
||||
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
|
||||
params={
|
||||
'rect_corners': rect_corners,
|
||||
'line_spacing': GRID_LINE_SPACING,
|
||||
'altitude': BASE_ALTITUDE
|
||||
}
|
||||
)
|
||||
else:
|
||||
print(f"未知測試模式: {TEST_MODE}")
|
||||
return
|
||||
|
||||
planned_waypoints = {
|
||||
'drone_ids': drone_ids,
|
||||
'waypoints': waypoints_per_drone
|
||||
}
|
||||
|
||||
# 印出規劃結果
|
||||
for i, did in enumerate(drone_ids):
|
||||
wps = waypoints_per_drone[i]
|
||||
print(f" {did}: {len(wps)} 個航點")
|
||||
for j, wp in enumerate(wps):
|
||||
print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
|
||||
|
||||
# 啟動任務
|
||||
print("\n啟動任務 ...")
|
||||
manager.start_gps_polling(interval_ms=200)
|
||||
|
||||
sender = MavlinkSender(SEND_CONNECTION)
|
||||
executor = MissionExecutor(
|
||||
sender=sender,
|
||||
drone_gps=manager.drone_gps,
|
||||
arrival_radius=ARRIVAL_RADIUS,
|
||||
send_rate_hz=2.0
|
||||
)
|
||||
|
||||
executor.drone_waypoint_reached.connect(
|
||||
lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}")
|
||||
)
|
||||
executor.mission_completed.connect(
|
||||
lambda: (print("\n===== 任務全部完成 ====="), app.quit())
|
||||
)
|
||||
|
||||
# 設定超時自動退出
|
||||
timeout_timer = QTimer()
|
||||
timeout_timer.setSingleShot(True)
|
||||
timeout_timer.timeout.connect(lambda: (
|
||||
print("\n⚠ 測試超時，強制退出"),
|
||||
executor.stop(),
|
||||
app.quit()
|
||||
))
|
||||
timeout_timer.start(180_000) # 180 秒超時
|
||||
|
||||
executor.start(planned_waypoints)
|
||||
|
||||
print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n")
|
||||
app.exec()
|
||||
|
||||
executor.stop()
|
||||
sender.close()
|
||||
print("測試結束")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,482 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
任務規劃視覺化驗證工具（含動畫模擬）
|
||||
|
||||
使用方式:
|
||||
1. 由 GUI 自動觸發: python3 verify_waypoints.py --file /tmp/mission_plan.json
|
||||
2. 獨立手動執行: python3 verify_waypoints.py
|
||||
|
||||
操作:
|
||||
- 啟動後先顯示靜態航點圖
|
||||
- 點擊圖下方的按鈕控制動畫
|
||||
"""
|
||||
import sys, os
|
||||
sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..'))
|
||||
|
||||
import json
|
||||
import argparse
|
||||
import math
|
||||
import matplotlib
|
||||
matplotlib.use('TkAgg')
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.animation as animation
|
||||
from matplotlib.widgets import Button
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
import numpy as np
|
||||
from mission_planner import FormationPlanner, MissionType, CoordinateConverter
|
||||
|
||||
|
||||
# ================================================================================
|
||||
# 色彩定義
|
||||
# ================================================================================
|
||||
COLORS = ['#378ADD', '#1D9E75', '#BA7517', '#D85A30', '#7F77DD', '#D4537E',
|
||||
'#E24B4A', '#639922', '#00BFFF', '#FF69B4']
|
||||
|
||||
# 動畫參數
|
||||
FRAMES_PER_SEGMENT = 40 # 每段航點之間的畫面數
|
||||
TRAIL_LENGTH = 60 # 拖尾長度（畫面數）
|
||||
INTERVAL_MS = 50 # 每幀間隔 (ms)
|
||||
|
||||
|
||||
# ================================================================================
|
||||
# 靜態繪圖
|
||||
# ================================================================================
|
||||
|
||||
def plot_grid_sweep(ax, data, conv):
|
||||
"""畫 Grid Sweep 俯視圖"""
|
||||
if 'rect_corners' in data:
|
||||
rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
|
||||
xs = [p[0] for p in rect_local] + [rect_local[0][0]]
|
||||
ys = [p[1] for p in rect_local] + [rect_local[0][1]]
|
||||
ax.plot(xs, ys, 'k--', linewidth=1.5, label='Task area')
|
||||
ax.fill(xs, ys, alpha=0.05, color='gray')
|
||||
|
||||
_draw_waypoint_paths(ax, data, conv, show_sweep_labels=True)
|
||||
|
||||
total_wps = sum(len(wps) for wps in data['waypoints'])
|
||||
ax.set_title(f'Grid Sweep - {len(data["drone_ids"])} drones, {total_wps} waypoints')
|
||||
ax.set_xlabel('East (m)')
|
||||
ax.set_ylabel('North (m)')
|
||||
ax.set_aspect('equal')
|
||||
ax.grid(True, alpha=0.3)
|
||||
|
||||
|
||||
def plot_formation(ax, data, conv):
|
||||
"""畫 M-Formation / Circle / Leader-Follower 俯視圖"""
|
||||
_draw_waypoint_paths(ax, data, conv, show_sweep_labels=False)
|
||||
|
||||
if 'target' in data:
|
||||
t = data['target']
|
||||
tx, ty, _ = conv.gps_to_local(t[0], t[1], t[2] if len(t) > 2 else 0)
|
||||
ax.plot(tx, ty, 'r*', markersize=18, zorder=5)
|
||||
ax.annotate('Target', (tx, ty + 1), fontsize=9, color='red', ha='center', va='bottom')
|
||||
|
||||
if 'route_waypoints' in data:
|
||||
route_local = [conv.gps_to_local(wp[0], wp[1], 0)[:2] for wp in data['route_waypoints']]
|
||||
rxs = [p[0] for p in route_local]
|
||||
rys = [p[1] for p in route_local]
|
||||
ax.plot(rxs, rys, 'r--', linewidth=1.5, alpha=0.5, label='Route center')
|
||||
for i, (rx, ry) in enumerate(route_local):
|
||||
ax.plot(rx, ry, 'ro', markersize=6, alpha=0.5)
|
||||
ax.annotate(f'R{i+1}', (rx, ry + 0.8), fontsize=7, color='red',
|
||||
ha='center', va='bottom', alpha=0.7)
|
||||
|
||||
mission_type = data.get('mission_type', 'formation')
|
||||
total_wps = sum(len(wps) for wps in data['waypoints'])
|
||||
ax.set_title(f'{mission_type} - {len(data["drone_ids"])} drones, {total_wps} waypoints')
|
||||
ax.set_xlabel('East (m)')
|
||||
ax.set_ylabel('North (m)')
|
||||
ax.set_aspect('equal')
|
||||
ax.grid(True, alpha=0.3)
|
||||
|
||||
|
||||
def _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False):
|
||||
"""共用的航點路徑繪圖"""
|
||||
drone_ids = data['drone_ids']
|
||||
waypoints = data['waypoints']
|
||||
drones_gps = data.get('drones_gps', [])
|
||||
|
||||
for i, pos in enumerate(drones_gps):
|
||||
x, y, _ = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
|
||||
c = COLORS[i % len(COLORS)]
|
||||
ax.plot(x, y, 'o', color=c, markersize=10, zorder=5)
|
||||
ax.annotate(f'{drone_ids[i]}', (x, y + 1), fontsize=8, fontweight='bold',
|
||||
ha='center', va='bottom', color=c)
|
||||
|
||||
for i, wps in enumerate(waypoints):
|
||||
c = COLORS[i % len(COLORS)]
|
||||
local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
|
||||
xs = [p[0] for p in local_wps]
|
||||
ys = [p[1] for p in local_wps]
|
||||
ax.plot(xs, ys, '-', color=c, linewidth=1.5, alpha=0.7)
|
||||
|
||||
for j, (x, y, z) in enumerate(local_wps):
|
||||
if show_sweep_labels:
|
||||
if j == 0:
|
||||
ax.plot(x, y, 's', color=c, markersize=8, zorder=4)
|
||||
ax.annotate('gather', (x, y), fontsize=6, ha='right', va='top')
|
||||
elif j == 1:
|
||||
ax.plot(x, y, '^', color=c, markersize=8, zorder=4)
|
||||
ax.annotate('entry', (x, y), fontsize=6, ha='right', va='top')
|
||||
else:
|
||||
ax.plot(x, y, '.', color=c, markersize=4)
|
||||
else:
|
||||
marker = 's' if j == 0 else '*'
|
||||
ax.plot(x, y, marker, color=c, markersize=10, zorder=4)
|
||||
ax.annotate(f'WP{j}\n({z:.0f}m)', (x, y), fontsize=6, ha='center', va='bottom')
|
||||
|
||||
if local_wps:
|
||||
lx, ly, _ = local_wps[-1]
|
||||
ax.plot(lx, ly, 'X', color=c, markersize=10, markeredgewidth=2, zorder=4)
|
||||
|
||||
|
||||
# ================================================================================
|
||||
# 動畫模擬
|
||||
# ================================================================================
|
||||
|
||||
class MissionAnimator:
|
||||
"""任務動畫控制器"""
|
||||
|
||||
def __init__(self, fig, ax, data, conv):
|
||||
self.fig = fig
|
||||
self.ax = ax
|
||||
self.data = data
|
||||
self.conv = conv
|
||||
self.is_playing = False
|
||||
self.anim = None
|
||||
self.current_frame = 0
|
||||
|
||||
drone_ids = data['drone_ids']
|
||||
waypoints = data['waypoints']
|
||||
drones_gps = data.get('drones_gps', [])
|
||||
self.num_drones = len(drone_ids)
|
||||
|
||||
# 建立完整航點序列：初始位置 → WP0 → WP1 → ...
|
||||
self.all_local_wps = []
|
||||
for i, wps in enumerate(waypoints):
|
||||
local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
|
||||
|
||||
# 把初始位置插入最前面
|
||||
if i < len(drones_gps):
|
||||
pos = drones_gps[i]
|
||||
start = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0)
|
||||
local_wps.insert(0, start)
|
||||
|
||||
self.all_local_wps.append(local_wps)
|
||||
|
||||
# 計算最大航點段數
|
||||
self.max_segments = max(len(wps) - 1 for wps in self.all_local_wps) if self.all_local_wps else 0
|
||||
self.total_frames = self.max_segments * FRAMES_PER_SEGMENT
|
||||
|
||||
# 預計算位置
|
||||
self.positions = self._precompute_positions()
|
||||
|
||||
# 動畫元素
|
||||
self.drone_dots = []
|
||||
self.trail_lines = []
|
||||
self.trail_data = [[] for _ in range(self.num_drones)]
|
||||
self.status_text = None
|
||||
|
||||
def _precompute_positions(self):
|
||||
"""預計算所有幀的位置 — 等時間步進"""
|
||||
positions = []
|
||||
|
||||
for frame in range(self.total_frames + 1):
|
||||
seg_idx = frame // FRAMES_PER_SEGMENT
|
||||
seg_progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
|
||||
|
||||
frame_positions = []
|
||||
for drone_idx in range(self.num_drones):
|
||||
wps = self.all_local_wps[drone_idx]
|
||||
num_segs = len(wps) - 1
|
||||
|
||||
if seg_idx >= num_segs:
|
||||
frame_positions.append((wps[-1][0], wps[-1][1]))
|
||||
else:
|
||||
x0, y0, _ = wps[seg_idx]
|
||||
x1, y1, _ = wps[seg_idx + 1]
|
||||
x = x0 + (x1 - x0) * seg_progress
|
||||
y = y0 + (y1 - y0) * seg_progress
|
||||
frame_positions.append((x, y))
|
||||
|
||||
positions.append(frame_positions)
|
||||
|
||||
return positions
|
||||
|
||||
def setup(self):
|
||||
"""建立動畫元素和按鈕"""
|
||||
for i in range(self.num_drones):
|
||||
c = COLORS[i % len(COLORS)]
|
||||
dot, = self.ax.plot([], [], 'o', color=c, markersize=12,
|
||||
markeredgecolor='white', markeredgewidth=1.5, zorder=10)
|
||||
self.drone_dots.append(dot)
|
||||
trail, = self.ax.plot([], [], '-', color=c, linewidth=2.5, alpha=0.4, zorder=9)
|
||||
self.trail_lines.append(trail)
|
||||
|
||||
self.status_text = self.ax.text(
|
||||
0.02, 0.98, 'Ready',
|
||||
transform=self.ax.transAxes, fontsize=10,
|
||||
verticalalignment='top',
|
||||
bbox=dict(boxstyle='round,pad=0.3', facecolor='white', alpha=0.8)
|
||||
)
|
||||
|
||||
# 按鈕放在右上角圖內，避免擋到軸標籤
|
||||
ax_play = self.fig.add_axes([0.72, 0.91, 0.08, 0.035])
|
||||
self.btn_play = Button(ax_play, 'Play', color='#4CAF50', hovercolor='#66BB6A')
|
||||
self.btn_play.label.set_color('white')
|
||||
self.btn_play.label.set_fontweight('bold')
|
||||
self.btn_play.on_clicked(self._on_play)
|
||||
|
||||
ax_pause = self.fig.add_axes([0.81, 0.91, 0.08, 0.035])
|
||||
self.btn_pause = Button(ax_pause, 'Pause', color='#FF9800', hovercolor='#FFB74D')
|
||||
self.btn_pause.label.set_color('white')
|
||||
self.btn_pause.label.set_fontweight('bold')
|
||||
self.btn_pause.on_clicked(self._on_pause)
|
||||
|
||||
ax_reset = self.fig.add_axes([0.90, 0.91, 0.08, 0.035])
|
||||
self.btn_reset = Button(ax_reset, 'Reset', color='#F44336', hovercolor='#EF5350')
|
||||
self.btn_reset.label.set_color('white')
|
||||
self.btn_reset.label.set_fontweight('bold')
|
||||
self.btn_reset.on_clicked(self._on_reset)
|
||||
|
||||
def _on_play(self, event):
|
||||
if self.is_playing:
|
||||
return
|
||||
if self.anim is None:
|
||||
self.anim = animation.FuncAnimation(
|
||||
self.fig, self._update_frame,
|
||||
frames=range(self.current_frame, self.total_frames + 1),
|
||||
interval=INTERVAL_MS,
|
||||
blit=False,
|
||||
repeat=False
|
||||
)
|
||||
else:
|
||||
self.anim.resume()
|
||||
self.is_playing = True
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def _on_pause(self, event):
|
||||
if not self.is_playing or self.anim is None:
|
||||
return
|
||||
self.anim.pause()
|
||||
self.is_playing = False
|
||||
self.status_text.set_text(f'Paused (frame {self.current_frame}/{self.total_frames})')
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def _on_reset(self, event):
|
||||
if self.anim is not None:
|
||||
self.anim.event_source.stop()
|
||||
self.anim = None
|
||||
self.is_playing = False
|
||||
self.current_frame = 0
|
||||
self.trail_data = [[] for _ in range(self.num_drones)]
|
||||
for dot in self.drone_dots:
|
||||
dot.set_data([], [])
|
||||
for trail in self.trail_lines:
|
||||
trail.set_data([], [])
|
||||
self.status_text.set_text('Ready')
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def _update_frame(self, frame):
|
||||
self.current_frame = frame
|
||||
|
||||
if frame >= len(self.positions):
|
||||
self.is_playing = False
|
||||
self.status_text.set_text('Done')
|
||||
return self.drone_dots + self.trail_lines + [self.status_text]
|
||||
|
||||
# seg_idx - 1 是因為第 0 段是 start→WP0
|
||||
seg_idx = frame // FRAMES_PER_SEGMENT
|
||||
progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT
|
||||
|
||||
# 顯示時把第 0 段標為 "Start -> WP0"
|
||||
if seg_idx == 0:
|
||||
label = f'Start -> WP0 Progress {progress:.0%}'
|
||||
else:
|
||||
label = f'WP{seg_idx-1} -> WP{seg_idx} Progress {progress:.0%}'
|
||||
self.status_text.set_text(
|
||||
f'{label} Frame {frame}/{self.total_frames}'
|
||||
)
|
||||
|
||||
for i in range(self.num_drones):
|
||||
x, y = self.positions[frame][i]
|
||||
self.drone_dots[i].set_data([x], [y])
|
||||
|
||||
self.trail_data[i].append((x, y))
|
||||
if len(self.trail_data[i]) > TRAIL_LENGTH:
|
||||
self.trail_data[i] = self.trail_data[i][-TRAIL_LENGTH:]
|
||||
trail_x = [p[0] for p in self.trail_data[i]]
|
||||
trail_y = [p[1] for p in self.trail_data[i]]
|
||||
self.trail_lines[i].set_data(trail_x, trail_y)
|
||||
|
||||
return self.drone_dots + self.trail_lines + [self.status_text]
|
||||
|
||||
|
||||
# ================================================================================
|
||||
# 主流程
|
||||
# ================================================================================
|
||||
|
||||
def visualize_from_file(filepath):
|
||||
"""從 JSON 檔案讀取並視覺化"""
|
||||
with open(filepath, 'r') as f:
|
||||
data = json.load(f)
|
||||
|
||||
origin = data['origin']
|
||||
conv = CoordinateConverter(origin[0], origin[1], 0)
|
||||
mission_type = data.get('mission_type', 'formation')
|
||||
is_sweep = mission_type == 'grid_sweep'
|
||||
|
||||
fig, ax = plt.subplots(1, 1, figsize=(10, 8))
|
||||
fig.suptitle(f'Mission Verification - {mission_type}', fontsize=13, fontweight='bold')
|
||||
|
||||
if is_sweep:
|
||||
plot_grid_sweep(ax, data, conv)
|
||||
else:
|
||||
plot_formation(ax, data, conv)
|
||||
|
||||
_print_summary(data)
|
||||
|
||||
animator = MissionAnimator(fig, ax, data, conv)
|
||||
animator.setup()
|
||||
|
||||
plt.tight_layout(rect=[0, 0, 1, 0.95])
|
||||
plt.show()
|
||||
|
||||
|
||||
def visualize_standalone():
|
||||
"""獨立執行：使用內建測試資料"""
|
||||
drones = [
|
||||
(24.123450, 120.567800, 0.0),
|
||||
(24.123470, 120.567820, 0.0),
|
||||
(24.123440, 120.567810, 0.0),
|
||||
(24.123460, 120.567830, 0.0),
|
||||
]
|
||||
rect_corners = [
|
||||
(24.12380, 120.56775),
|
||||
(24.12380, 120.56810),
|
||||
(24.12420, 120.56810),
|
||||
(24.12420, 120.56775),
|
||||
]
|
||||
target = (24.12400, 120.56795, 10.0)
|
||||
|
||||
planner = FormationPlanner(spacing=5.0, base_altitude=10.0, altitude_diff=2.0)
|
||||
|
||||
fig = plt.figure(figsize=(16, 10))
|
||||
fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold')
|
||||
|
||||
# M-Formation
|
||||
wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
|
||||
conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0)
|
||||
data_m = {
|
||||
'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
|
||||
'waypoints': wps_m,
|
||||
'drones_gps': drones,
|
||||
'target': target,
|
||||
'mission_type': 'M_FORMATION'
|
||||
}
|
||||
ax1 = fig.add_subplot(2, 2, 1)
|
||||
plot_formation(ax1, data_m, conv_m)
|
||||
|
||||
# Grid Sweep 5m
|
||||
target_gs = (sum(c[0] for c in rect_corners) / 4,
|
||||
sum(c[1] for c in rect_corners) / 4, 10.0)
|
||||
wps_g, origin_g = planner.plan_formation_mission(
|
||||
drones, target_gs, MissionType.GRID_SWEEP,
|
||||
params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0}
|
||||
)
|
||||
conv_g = CoordinateConverter(origin_g[0], origin_g[1], 0)
|
||||
data_g = {
|
||||
'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
|
||||
'waypoints': wps_g,
|
||||
'drones_gps': drones,
|
||||
'rect_corners': rect_corners,
|
||||
'mission_type': 'grid_sweep'
|
||||
}
|
||||
ax2 = fig.add_subplot(2, 2, 2)
|
||||
plot_grid_sweep(ax2, data_g, conv_g)
|
||||
|
||||
# Leader-Follower
|
||||
route = [
|
||||
(24.12360, 120.56780),
|
||||
(24.12380, 120.56800),
|
||||
(24.12400, 120.56820),
|
||||
(24.12410, 120.56800),
|
||||
(24.12420, 120.56790),
|
||||
]
|
||||
wps_lf, origin_lf = planner.plan_formation_mission(
|
||||
drones, target, MissionType.LEADER_FOLLOWER,
|
||||
params={'route_waypoints': route, 'lateral_offset': 3.0,
|
||||
'longitudinal_spacing': 5.0, 'altitude': 10.0}
|
||||
)
|
||||
conv_lf = CoordinateConverter(origin_lf[0], origin_lf[1], 0)
|
||||
data_lf = {
|
||||
'drone_ids': [f's0_{i + 1}' for i in range(len(drones))],
|
||||
'waypoints': wps_lf,
|
||||
'drones_gps': drones,
|
||||
'route_waypoints': route,
|
||||
'target': target,
|
||||
'mission_type': 'LEADER_FOLLOWER'
|
||||
}
|
||||
ax3 = fig.add_subplot(2, 2, 3)
|
||||
plot_formation(ax3, data_lf, conv_lf)
|
||||
|
||||
# 3D
|
||||
ax4 = fig.add_subplot(2, 2, 4, projection='3d')
|
||||
_plot_3d(ax4, data_g, conv_g)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
|
||||
def _plot_3d(ax, data, conv):
|
||||
"""3D 視角"""
|
||||
if 'rect_corners' in data:
|
||||
rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']]
|
||||
xs = [p[0] for p in rect_local] + [rect_local[0][0]]
|
||||
ys = [p[1] for p in rect_local] + [rect_local[0][1]]
|
||||
ax.plot(xs, ys, [0] * len(xs), 'k--', linewidth=1, alpha=0.4)
|
||||
|
||||
for i, wps in enumerate(data['waypoints']):
|
||||
c = COLORS[i % len(COLORS)]
|
||||
local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps]
|
||||
xs = [p[0] for p in local_wps]
|
||||
ys = [p[1] for p in local_wps]
|
||||
zs = [p[2] for p in local_wps]
|
||||
ax.plot(xs, ys, zs, '-', color=c, linewidth=1.5)
|
||||
if local_wps:
|
||||
ax.scatter(xs[0], ys[0], zs[0], color=c, s=50, marker='s')
|
||||
ax.scatter(xs[-1], ys[-1], zs[-1], color=c, s=50, marker='X')
|
||||
|
||||
ax.set_title('3D view')
|
||||
ax.set_xlabel('East (m)')
|
||||
ax.set_ylabel('North (m)')
|
||||
ax.set_zlabel('Alt (m)')
|
||||
|
||||
|
||||
def _print_summary(data):
|
||||
"""終端印出摘要"""
|
||||
drone_ids = data['drone_ids']
|
||||
waypoints = data['waypoints']
|
||||
mission_type = data.get('mission_type', 'unknown')
|
||||
print(f"\n{'=' * 50}")
|
||||
print(f"Mission: {mission_type}")
|
||||
print(f"Drones: {len(drone_ids)}")
|
||||
print(f"{'=' * 50}")
|
||||
for i, did in enumerate(drone_ids):
|
||||
wps = waypoints[i]
|
||||
print(f" {did}: {len(wps)} waypoints")
|
||||
for j, wp in enumerate(wps):
|
||||
print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)")
|
||||
print(f"{'=' * 50}\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description='Mission waypoint visualizer')
|
||||
parser.add_argument('--file', '-f', type=str, default=None,
|
||||
help='JSON file from GUI (auto-generated)')
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.file:
|
||||
visualize_from_file(args.file)
|
||||
else:
|
||||
visualize_standalone()
|
||||
Reference in New Issue
Block a user