|
|
|
|
@ -83,10 +83,11 @@ class FormationPlanner:
|
|
|
|
|
|
|
|
|
|
center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
|
|
|
|
|
center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)
|
|
|
|
|
center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions)
|
|
|
|
|
|
|
|
|
|
self.current_origin = (center_lat, center_lon, center_alt)
|
|
|
|
|
self.converter = CoordinateConverter(center_lat, center_lon, center_alt)
|
|
|
|
|
# origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
|
|
|
|
|
# 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
|
|
|
|
|
self.current_origin = (center_lat, center_lon, 0.0)
|
|
|
|
|
self.converter = CoordinateConverter(center_lat, center_lon, 0.0)
|
|
|
|
|
|
|
|
|
|
drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
|
|
|
|
|
target_local = self.converter.gps_to_local(*target_gps)
|
|
|
|
|
@ -206,29 +207,17 @@ class FormationPlanner:
|
|
|
|
|
|
|
|
|
|
def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
|
|
|
|
|
"""
|
|
|
|
|
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
|
|
|
|
|
|
|
|
|
|
核心想法：
|
|
|
|
|
- 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 處理，避免倒退或衝出
|
|
|
|
|
|
|
|
|
|
解決前版「空間偏移」的三個 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)
|
|
|
|
|
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化
|
|
|
|
|
|
|
|
|
|
center_path = 使用者畫的路徑點（直線段連接，不插弧），
|
|
|
|
|
每架 drone 沿弧長 s 參數化做隊形偏移：
|
|
|
|
|
s_follower = s_leader - lon_k
|
|
|
|
|
position = lookup(s_follower) + lat_k × perp(tangent)
|
|
|
|
|
s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。
|
|
|
|
|
|
|
|
|
|
Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
|
|
|
|
|
隊形 (俯視):
|
|
|
|
|
D0 (lat=-L, lon=0) ← leader
|
|
|
|
|
D1 (lat=+L, lon=5)
|
|
|
|
|
D2 (lat=-L, lon=10)
|
|
|
|
|
D3 (lat=+L, lon=15)
|
|
|
|
|
@ -237,29 +226,16 @@ class FormationPlanner:
|
|
|
|
|
lateral_offset = params.get('lateral_offset', 3.0)
|
|
|
|
|
longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
|
|
|
|
|
altitude = params.get('altitude', self.base_altitude)
|
|
|
|
|
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: 建立中心路徑（含圓弧、銳角單點；每點帶累積 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 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
|
|
|
|
|
center_path, barrier_indices = self._build_center_path(route_wps_local)
|
|
|
|
|
|
|
|
|
|
s_list = [pt['s'] for pt in center_path]
|
|
|
|
|
s_max = s_list[-1]
|
|
|
|
|
n_pts = len(center_path)
|
|
|
|
|
|
|
|
|
|
def lookup(s_target):
|
|
|
|
|
"""
|
|
|
|
|
在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
|
|
|
|
|
s<0 → clip 到 start；s>s_max → clip 到 end。
|
|
|
|
|
tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
|
|
|
|
|
"""
|
|
|
|
|
"""在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。"""
|
|
|
|
|
if n_pts == 1:
|
|
|
|
|
pt = center_path[0]
|
|
|
|
|
return pt['x'], pt['y'], pt['dir']
|
|
|
|
|
@ -270,7 +246,8 @@ class FormationPlanner:
|
|
|
|
|
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)
|
|
|
|
|
dx, dy = sdx / slen, sdy / slen
|
|
|
|
|
return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
|
|
|
|
|
return a['x'], a['y'], a['dir']
|
|
|
|
|
|
|
|
|
|
if s_target >= s_max:
|
|
|
|
|
@ -279,7 +256,9 @@ class FormationPlanner:
|
|
|
|
|
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)
|
|
|
|
|
dx, dy = sdx / slen, sdy / slen
|
|
|
|
|
overshoot = s_target - s_max
|
|
|
|
|
return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
|
|
|
|
|
return b['x'], b['y'], b['dir']
|
|
|
|
|
|
|
|
|
|
# Binary search：s_list[lo] <= s_target < s_list[hi]
|
|
|
|
|
@ -308,16 +287,6 @@ class FormationPlanner:
|
|
|
|
|
# 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
|
|
|
|
|
all_waypoints = [None] * N
|
|
|
|
|
|
|
|
|
|
# 預先算好路徑終點的切線（給收尾點用）
|
|
|
|
|
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
|
|
|
|
|
@ -334,46 +303,16 @@ class FormationPlanner:
|
|
|
|
|
altitude,
|
|
|
|
|
))
|
|
|
|
|
|
|
|
|
|
# 收尾：全員到 path 終點的 rank 位置（lon 歸零）
|
|
|
|
|
waypoints.append((
|
|
|
|
|
end['x'] + lat * (-end_dy),
|
|
|
|
|
end['y'] + lat * end_dx,
|
|
|
|
|
altitude,
|
|
|
|
|
))
|
|
|
|
|
|
|
|
|
|
all_waypoints[rank] = waypoints
|
|
|
|
|
|
|
|
|
|
# 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):
|
|
|
|
|
def _build_center_path(self, waypoints):
|
|
|
|
|
"""
|
|
|
|
|
建立以「圓弧」連接直線段的中心路徑。
|
|
|
|
|
|
|
|
|
|
每個中間 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 (轉完彎後的集合點)
|
|
|
|
|
|
|
|
|
|
path 是一個 list[dict]，每個 dict 至少含:
|
|
|
|
|
{'seg': 'straight' | 'arc',
|
|
|
|
|
'x': float, 'y': float,
|
|
|
|
|
'dir': (dx, dy)}
|
|
|
|
|
'arc' 類型額外含: 'arc_center', 'arc_R', 'arc_sign', 'theta'
|
|
|
|
|
建立中心路徑 — 直接用路徑點連接，不插弧。
|
|
|
|
|
|
|
|
|
|
每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
|
|
|
|
|
飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。
|
|
|
|
|
|
|
|
|
|
Returns:
|
|
|
|
|
(path, barrier_indices)
|
|
|
|
|
@ -385,129 +324,35 @@ class FormationPlanner:
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': waypoints[0][0], 'y': waypoints[0][1],
|
|
|
|
|
'dir': (0.0, 1.0),
|
|
|
|
|
's': 0.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.hypot(dx, dy)
|
|
|
|
|
if length < 0.01:
|
|
|
|
|
segments.append(((0.0, 1.0), length))
|
|
|
|
|
else:
|
|
|
|
|
segments.append(((dx / length, dy / length), length))
|
|
|
|
|
|
|
|
|
|
R_base = formation_max_lateral + min_inner_radius
|
|
|
|
|
path = []
|
|
|
|
|
barrier_indices = []
|
|
|
|
|
|
|
|
|
|
# 起點
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': waypoints[0][0], 'y': waypoints[0][1],
|
|
|
|
|
'dir': segments[0][0],
|
|
|
|
|
})
|
|
|
|
|
|
|
|
|
|
for i in range(1, num_wps - 1):
|
|
|
|
|
u_in, len_in = segments[i - 1]
|
|
|
|
|
u_out, len_out = segments[i]
|
|
|
|
|
wp = waypoints[i]
|
|
|
|
|
|
|
|
|
|
dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
|
|
|
|
|
dot = max(-1.0, min(1.0, dot))
|
|
|
|
|
|
|
|
|
|
# 銳角：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_dir = (avg_dx / avg_len, avg_dy / avg_len)
|
|
|
|
|
for i in range(num_wps):
|
|
|
|
|
if i < num_wps - 1:
|
|
|
|
|
dx = waypoints[i + 1][0] - waypoints[i][0]
|
|
|
|
|
dy = waypoints[i + 1][1] - waypoints[i][1]
|
|
|
|
|
else:
|
|
|
|
|
avg_dir = u_in
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': wp[0], 'y': wp[1],
|
|
|
|
|
'dir': avg_dir,
|
|
|
|
|
})
|
|
|
|
|
barrier_indices.append(len(path) - 1)
|
|
|
|
|
continue
|
|
|
|
|
dx = waypoints[i][0] - waypoints[i - 1][0]
|
|
|
|
|
dy = waypoints[i][1] - waypoints[i - 1][1]
|
|
|
|
|
|
|
|
|
|
# 平滑弧
|
|
|
|
|
beta = math.acos(dot)
|
|
|
|
|
if beta < 1e-4:
|
|
|
|
|
# 幾乎直線，不插弧
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': wp[0], 'y': wp[1],
|
|
|
|
|
'dir': u_in,
|
|
|
|
|
})
|
|
|
|
|
continue
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
# +1 CCW (左轉) / -1 CW (右轉)
|
|
|
|
|
cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
|
|
|
|
|
sign = 1 if cross > 0 else -1
|
|
|
|
|
|
|
|
|
|
# 入向左法線
|
|
|
|
|
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],
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
|
|
|
|
|
length = math.hypot(dx, dy)
|
|
|
|
|
if length < 0.01:
|
|
|
|
|
direction = (0.0, 1.0)
|
|
|
|
|
else:
|
|
|
|
|
direction = (dx / length, dy / length)
|
|
|
|
|
|
|
|
|
|
# T_in (straight, 方向 u_in)
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': T_in[0], 'y': T_in[1],
|
|
|
|
|
'dir': u_in,
|
|
|
|
|
})
|
|
|
|
|
|
|
|
|
|
# 弧段採樣 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,
|
|
|
|
|
'x': waypoints[i][0], 'y': waypoints[i][1],
|
|
|
|
|
'dir': direction,
|
|
|
|
|
})
|
|
|
|
|
|
|
|
|
|
# T_out (straight, 方向 u_out)，barrier 放這
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': T_out[0], 'y': T_out[1],
|
|
|
|
|
'dir': u_out,
|
|
|
|
|
})
|
|
|
|
|
if 0 < i < num_wps - 1:
|
|
|
|
|
barrier_indices.append(len(path) - 1)
|
|
|
|
|
|
|
|
|
|
# 終點
|
|
|
|
|
path.append({
|
|
|
|
|
'seg': 'straight',
|
|
|
|
|
'x': waypoints[-1][0], 'y': waypoints[-1][1],
|
|
|
|
|
'dir': segments[-1][0],
|
|
|
|
|
})
|
|
|
|
|
|
|
|
|
|
# 後處理：為每個 sample 加上累積 polyline 弧長 s
|
|
|
|
|
path[0]['s'] = 0.0
|
|
|
|
|
for i in range(1, len(path)):
|
|
|
|
|
prev = path[i - 1]
|
|
|
|
|
|
