chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(mission): virtual-leader path following + 固定領隊 + barrier

Browse Source 
- mission_planner: Leader-Follower 改用 arc-length 參數化（virtual
  leader），_build_center_path 以同心圓弧連接直線段，銳角退化成 hover
  單點並在該點放 rendezvous barrier，避免暴衝與編隊交叉
- mission_planner: rank 改以輸入順序為準，不再對投影排序，避免浮點噪音
  導致 run-to-run leader 漂移
- mission_group / gui: 新增 leader_drone_id 與「領隊」下拉選單，handle_
  route_confirmed 把指定領隊重排到 rank 0，整個縱隊順序固定如軍隊行進
- mission_executor: rendezvous barrier 釋放邏輯、fallback LOITER、send
  retry；加 barrier_timeout 保護
- command_sender / communication / navigation: 配合 setpoint 送達與
  Ros2 bridge 更新

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
wenchun
2026-04-15 16:13:31 +08:00
co-authored by Claude Opus 4.6
parent 5c0d21fc1d
commit 0edc477df8
9 changed files with 882 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/mission_planner.py
+285 -120
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
"""
飛行任務規劃模組
支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep
支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
"""
import math
from typing import List, Tuple, Optional, Dict, Any
@@ -72,7 +72,12 @@ class FormationPlanner:
mission_type: MissionType = MissionType.M_FORMATION,
params: Optional[Dict[str, Any]] = None
) -> Tuple[List[List[Tuple[float, float, float]]],
Tuple[float, float, float]]:
Tuple[float, float, float],
List[int]]:
"""
回傳 (waypoints_gps, origin, rendezvous_indices)。
rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
"""
if len(drone_gps_positions) == 0:
raise ValueError("無人機位置列表不能為空")
@@ -89,10 +94,14 @@ class FormationPlanner:
if mission_type == MissionType.M_FORMATION:
s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
# 起點集合後一起出發到 stage2
rendezvous_indices = [0]
elif mission_type == MissionType.CIRCLE_FORMATION:
s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
# 半程集合後一起進最終圓環位置
rendezvous_indices = [0]
elif mission_type == MissionType.LEADER_FOLLOWER:
params = params or {}
@@ -103,7 +112,9 @@ class FormationPlanner:
self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
for wp in route_wps_gps
]
waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params)
waypoints_local, rendezvous_indices = self._calculate_leader_follower(
drone_local, route_wps_local, params
)
elif mission_type == MissionType.GRID_SWEEP:
params = params or {}
@@ -114,7 +125,9 @@ class FormationPlanner:
self.converter.gps_to_local(c[0], c[1], 0)[:2]
for c in rect_corners_gps
]
waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params)
waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
drone_local, rect_corners_local, params
)
else:
raise ValueError(f"不支援的任務類型: {mission_type}")
@@ -123,7 +136,7 @@ class FormationPlanner:
gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
waypoints_gps.append(gps_wps)
return waypoints_gps, self.current_origin
return waypoints_gps, self.current_origin, rendezvous_indices
# ------------------------------------------------------------------ M-Formation
@@ -189,177 +202,321 @@ class FormationPlanner:
return stage1_positions, stage2_positions
# ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎)
# ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
"""
路徑跟隨編隊 — Bezier 曲線轉彎版
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
步驟:
1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
核心想法：
- center_path 建好後計算每個 sample 的累積弧長 s
- 每架 drone 給 (lat_k, lon_k) 的隊形偏移
- 把 leader 想成沿 s 參數化的點，follower k 的目標 s = s_leader - lon_k
- follower k 第 i 個 waypoint = lookup(s_list[i] - lon_k) + lat_k × perp(tangent)
- 超過 path 起點/終點以 clip 處理，避免倒退或衝出
隊形 (俯視):
| (前進方向) |
| D1 | ← 左偏移, 後 0m
| D2 | ← 右偏移, 後 5m
| D3 | ← 左偏移, 後 10m
| D4 | ← 右偏移, 後 15m
解決前版「空間偏移」的三個 bug：
1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
Rank 定序規則：
**以 drone_positions 的輸入順序為準**（= GUI 選取順序）。
drone_positions[0] = rank 0 = leader (lon=0)，
drone_positions[1] = rank 1 (lon=5)，依此類推。
刻意「不」用位置投影自動排序，避免浮點噪音造成 run-to-run
leader 漂移，以確保整個 mission 像軍隊縱隊那樣順序固定。
隊形 (俯視, 以路徑行進方向為前):
D0 (lat=-L, lon=0) ← front-right (leader)
D1 (lat=+L, lon=5)
D2 (lat=-L, lon=10)
D3 (lat=+L, lon=15)
"""
N = len(drone_positions)
lateral_offset = params.get('lateral_offset', 3.0)
longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
altitude = params.get('altitude', self.base_altitude)
turn_margin = params.get('turn_margin', 0.35) # 轉彎切入距離佔段長比例
curve_resolution = params.get('curve_resolution', 8) # 每個彎道的插值點數
min_inner_radius = params.get('min_inner_radius', 3.0)
arc_resolution = params.get('arc_resolution', 12)
sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0) # cos(90°)=0
# Step 1: 建立帶 Bezier 轉彎的中心路徑
center_path = self._build_center_path(
route_wps_local, turn_margin, curve_resolution
# Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
center_path, barrier_indices = self._build_center_path(
route_wps_local,
formation_max_lateral=lateral_offset,
min_inner_radius=min_inner_radius,
arc_resolution=arc_resolution,
sharp_turn_cos_threshold=sharp_turn_cos,
)
# Step 2: 固定排序
first_dir = (center_path[0][2], center_path[0][3])
first_perp = (-first_dir[1], first_dir[0])
C_x = sum(p[0] for p in drone_positions) / N
C_y = sum(p[1] for p in drone_positions) / N
s_list = [pt['s'] for pt in center_path]
s_max = s_list[-1]
n_pts = len(center_path)
projections = [
((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
for i, pos in enumerate(drone_positions)
]
projections.sort()
def lookup(s_target):
"""
在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
s<0 → clip 到 start；s>s_max → clip 到 end。
tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
"""
if n_pts == 1:
pt = center_path[0]
return pt['x'], pt['y'], pt['dir']
# Step 3: 偏移
if s_target <= 0.0:
a = center_path[0]
b = center_path[1]
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return a['x'], a['y'], (sdx / slen, sdy / slen)
return a['x'], a['y'], a['dir']
if s_target >= s_max:
a = center_path[-2]
b = center_path[-1]
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return b['x'], b['y'], (sdx / slen, sdy / slen)
return b['x'], b['y'], b['dir']
# Binary search：s_list[lo] <= s_target < s_list[hi]
lo, hi = 0, n_pts - 1
while lo + 1 < hi:
mid = (lo + hi) // 2
if s_list[mid] <= s_target:
lo = mid
else:
hi = mid
a = center_path[lo]
b = center_path[hi]
ds = s_list[hi] - s_list[lo]
if ds < 1e-9:
return a['x'], a['y'], a['dir']
t = (s_target - s_list[lo]) / ds
x = a['x'] + t * (b['x'] - a['x'])
y = a['y'] + t * (b['y'] - a['y'])
sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
return x, y, (sdx / slen, sdy / slen)
return x, y, a['dir']
# Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
# 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
all_waypoints = [None] * N
for rank, (_, original_idx) in enumerate(projections):
# 預先算好路徑終點的切線（給收尾點用）
end = center_path[-1]
end_dx, end_dy = end['dir']
if n_pts >= 2:
a = center_path[-2]
sdx, sdy = end['x'] - a['x'], end['y'] - a['y']
slen = math.hypot(sdx, sdy)
if slen > 1e-6:
end_dx, end_dy = sdx / slen, sdy / slen
for rank in range(N):
lat_sign = -1 if rank % 2 == 0 else 1
lat = lat_sign * lateral_offset
lon = rank * longitudinal_spacing
waypoints = []
for (cx, cy, dx, dy) in center_path:
for i in range(n_pts):
s_follower = s_list[i] - lon
x, y, (dx, dy) = lookup(s_follower)
perp_x, perp_y = -dy, dx
ox = cx + lat * perp_x - lon * dx
oy = cy + lat * perp_y - lon * dy
waypoints.append((ox, oy, altitude))
waypoints.append((
x + lat * perp_x,
y + lat * perp_y,
altitude,
))
all_waypoints[original_idx] = waypoints
# 收尾：全員到 path 終點的 rank 位置（lon 歸零）
waypoints.append((
end['x'] + lat * (-end_dy),
end['y'] + lat * end_dx,
altitude,
))
return all_waypoints
all_waypoints[rank] = waypoints
def _build_center_path(self, waypoints, turn_margin, curve_resolution):
# barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
# 不觸及末尾收尾點，直接沿用即可
return all_waypoints, barrier_indices
def _build_center_path(self, waypoints,
formation_max_lateral,
min_inner_radius,
arc_resolution,
sharp_turn_cos_threshold):
"""
建立帶 Bezier 曲線轉彎的中心路徑
建立以「圓弧」連接直線段的中心路徑。
在每個轉折 WP 處:
1. 計算 pre_turn = WP - d_in × T
2. 計算 post_turn = WP + d_out × T
3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
每個中間 waypoint 的處理：
1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
barrier 放在該點，讓隊伍整體停下再繼續
3. 否則 → 平滑弧：
R_base = formation_max_lateral + min_inner_radius
d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
R_actual = d / tan(β/2)
tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
arc 由 theta_in 到 theta_out 以 arc_resolution 等分
path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
barrier 放在 T_out (轉完彎後的集合點)
Args:
waypoints: 路徑航點 [(x, y), ...]
turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
curve_resolution: 每個彎道的 Bezier 插值點數
path 是一個 list[dict]，每個 dict 至少含:
{'seg': 'straight' | 'arc',
'x': float, 'y': float,
'dir': (dx, dy)}
'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
Returns:
[(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向
(path, barrier_indices)
"""
num_wps = len(waypoints)
if num_wps < 2:
return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)]
return [{
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': (0.0, 1.0),
}], []
# 計算每段方向和長度
segments = []
for i in range(num_wps - 1):
dx = waypoints[i + 1][0] - waypoints[i][0]
dy = waypoints[i + 1][1] - waypoints[i][1]
length = math.sqrt(dx ** 2 + dy ** 2)
length = math.hypot(dx, dy)
if length < 0.01:
segments.append((0.0, 1.0, length))
segments.append(((0.0, 1.0), length))
else:
segments.append((dx / length, dy / length, length))
segments.append(((dx / length, dy / length), length))
R_base = formation_max_lateral + min_inner_radius
path = []
barrier_indices = []
# 第一個航點
path.append((waypoints[0][0], waypoints[0][1],
segments[0][0], segments[0][1]))
# 起點
path.append({
'seg': 'straight',
'x': waypoints[0][0], 'y': waypoints[0][1],
'dir': segments[0][0],
})
# 中間航點 (轉彎處)
for i in range(1, num_wps - 1):
d_in_x, d_in_y, len_in = segments[i - 1]
d_out_x, d_out_y, len_out = segments[i]
u_in, len_in = segments[i - 1]
u_out, len_out = segments[i]
wp = waypoints[i]
# 切入距離 T: 取相鄰兩段中較短的 × turn_margin
T = min(len_in, len_out) * turn_margin
dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
dot = max(-1.0, min(1.0, dot))
if T < 0.5:
# 段太短，不插彎，直接加一個平均方向點
avg_dx = d_in_x + d_out_x
avg_dy = d_in_y + d_out_y
avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
# 銳角：hover + 原地轉向
if dot < sharp_turn_cos_threshold:
avg_dx = u_in[0] + u_out[0]
avg_dy = u_in[1] + u_out[1]
avg_len = math.hypot(avg_dx, avg_dy)
if avg_len > 0.01:
avg_dx /= avg_len
avg_dy /= avg_len
avg_dir = (avg_dx / avg_len, avg_dy / avg_len)
else:
avg_dx, avg_dy = d_in_x, d_in_y
path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
avg_dir = u_in
path.append({
'seg': 'straight',
'x': wp[0], 'y': wp[1],
'dir': avg_dir,
})
barrier_indices.append(len(path) - 1)
continue
# P0 = pre_turn (弧線起始)
p0_x = waypoints[i][0] - d_in_x * T
p0_y = waypoints[i][1] - d_in_y * T
# 平滑弧
beta = math.acos(dot)
if beta < 1e-4:
# 幾乎直線，不插弧
path.append({
'seg': 'straight',
'x': wp[0], 'y': wp[1],
'dir': u_in,
})
continue
# P1 = WP 本身 (控制點)
p1_x = waypoints[i][0]
p1_y = waypoints[i][1]
half_tan = math.tan(beta / 2.0)
d_ideal = R_base * half_tan
d_max = 0.45 * min(len_in, len_out)
d = min(d_ideal, d_max)
R_actual = d / half_tan
# P2 = post_turn (弧線結束)
p2_x = waypoints[i][0] + d_out_x * T
p2_y = waypoints[i][1] + d_out_y * T
T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
# 加入 pre_turn 點 (方向 = incoming)
path.append((p0_x, p0_y, d_in_x, d_in_y))
# +1 CCW (左轉) / -1 CW (右轉)
cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
sign = 1 if cross > 0 else -1
# 加入 Bezier 插值點
for j in range(1, curve_resolution):
t = j / curve_resolution
# 入向左法線
n_left = (-u_in[1], u_in[0])
center = (
T_in[0] + R_actual * sign * n_left[0],
T_in[1] + R_actual * sign * n_left[1],
)
# B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
one_minus_t = 1.0 - t
bx = one_minus_t * one_minus_t * p0_x + \
2 * t * one_minus_t * p1_x + \
t * t * p2_x
by = one_minus_t * one_minus_t * p0_y + \
2 * t * one_minus_t * p1_y + \
t * t * p2_y
theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
# B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向
tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
# T_in (straight, 方向 u_in)
path.append({
'seg': 'straight',
'x': T_in[0], 'y': T_in[1],
'dir': u_in,
})
# 正規化
t_len = math.sqrt(tdx ** 2 + tdy ** 2)
if t_len > 0.01:
tdx /= t_len
tdy /= t_len
else:
tdx, tdy = d_in_x, d_in_y
# 弧段採樣 k=1..arc_resolution-1（不含兩端）
for k in range(1, arc_resolution):
t = k / arc_resolution
theta = theta_in + sign * beta * t
px = center[0] + R_actual * math.cos(theta)
py = center[1] + R_actual * math.sin(theta)
# 切線: sign × (-sin θ, cos θ)
tx = sign * (-math.sin(theta))
ty = sign * math.cos(theta)
path.append({
'seg': 'arc',
'x': px, 'y': py,
'dir': (tx, ty),
'arc_center': center,
'arc_R': R_actual,
'arc_sign': sign,
'theta': theta,
})
path.append((bx, by, tdx, tdy))
# T_out (straight, 方向 u_out)，barrier 放這
path.append({
'seg': 'straight',
'x': T_out[0], 'y': T_out[1],
'dir': u_out,
})
barrier_indices.append(len(path) - 1)
# 加入 post_turn 點 (方向 = outgoing)
path.append((p2_x, p2_y, d_out_x, d_out_y))
# 終點
path.append({
'seg': 'straight',
'x': waypoints[-1][0], 'y': waypoints[-1][1],
'dir': segments[-1][0],
})
# 最後一個航點
path.append((waypoints[-1][0], waypoints[-1][1],
segments[-1][0], segments[-1][1]))
# 後處理：為每個 sample 加上累積 polyline 弧長 s
path[0]['s'] = 0.0
for i in range(1, len(path)):
prev = path[i - 1]
cur = path[i]
cur['s'] = prev['s'] + math.hypot(
cur['x'] - prev['x'], cur['y'] - prev['y']
)
return path
return path, barrier_indices
# ------------------------------------------------------------------ 柵狀掃描
@@ -474,7 +631,15 @@ class FormationPlanner:
all_waypoints[original_idx] = waypoints_list
return all_waypoints
# 每一條掃描線的「起點」都做一次同步：
# wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
# wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
# 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
# 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
# 所以用同一份 rendezvous_indices
rendezvous_indices = [2 * i + 1 for i in range(num_lines)]
return all_waypoints, rendezvous_indices
# ================================================================================
@@ -492,12 +657,12 @@ if __name__ == "__main__":
target = (24.12400, 120.56795, 10.0)
# M-Formation
wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
print("M-Formation:")
for i, wp_list in enumerate(wps):
print(f" Drone {i}: {len(wp_list)} waypoints")
# Leader-Follower (Bezier 轉彎)
# Leader-Follower (虛擬領隊 / 弧長參數化)
route = [
(24.12345, 120.56780),
(24.12370, 120.56800),
@@ -505,18 +670,18 @@ if __name__ == "__main__":
(24.12400, 120.56800),
(24.12420, 120.56790),
]
wps_lf, origin_lf = planner.plan_formation_mission(
wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
drones, target, MissionType.LEADER_FOLLOWER,
params={
'route_waypoints': route,
'lateral_offset': 3.0,
'longitudinal_spacing': 5.0,
'turn_margin': 0.35,
'curve_resolution': 8,
'min_inner_radius': 3.0,
'arc_resolution': 8,
'altitude': 10.0
}
)
print(f"\nLeader-Follower (Bezier turns):")
print(f"\nLeader-Follower (arc-length virtual leader):")
for i, wp_list in enumerate(wps_lf):
print(f" Drone {i}: {len(wp_list)} waypoints")
for j, wp in enumerate(wp_list):
@@ -529,7 +694,7 @@ if __name__ == "__main__":
(24.1240, 120.5683),
(24.1240, 120.5679)
]
wps2, origin2 = planner.plan_formation_mission(
wps2, origin2, rv2 = planner.plan_formation_mission(
drones, target, MissionType.GRID_SWEEP,
params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
)