You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
482 lines
17 KiB
Python
482 lines
17 KiB
Python
#!/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() |