|
|
#!/usr/bin/env python3
|
|
|
"""
|
|
|
飛行任務規劃模組
|
|
|
支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
|
|
|
"""
|
|
|
import math
|
|
|
from typing import List, Tuple, Optional, Dict, Any
|
|
|
from enum import Enum
|
|
|
|
|
|
|
|
|
class MissionType(Enum):
|
|
|
"""任務類型"""
|
|
|
M_FORMATION = "m_formation"
|
|
|
CIRCLE_FORMATION = "circle_formation"
|
|
|
LEADER_FOLLOWER = "leader_follower"
|
|
|
GRID_SWEEP = "grid_sweep"
|
|
|
|
|
|
|
|
|
class CoordinateConverter:
|
|
|
"""GPS 座標與 Local 座標的轉換器"""
|
|
|
|
|
|
def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
|
|
|
self.origin_lat = origin_lat
|
|
|
self.origin_lon = origin_lon
|
|
|
self.origin_alt = origin_alt
|
|
|
self.R = 6371000.0
|
|
|
|
|
|
def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
|
|
|
lat_rad = math.radians(lat)
|
|
|
lon_rad = math.radians(lon)
|
|
|
origin_lat_rad = math.radians(self.origin_lat)
|
|
|
origin_lon_rad = math.radians(self.origin_lon)
|
|
|
|
|
|
dlat = lat_rad - origin_lat_rad
|
|
|
dlon = lon_rad - origin_lon_rad
|
|
|
|
|
|
x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
|
|
|
y = self.R * dlat
|
|
|
z = alt - self.origin_alt
|
|
|
|
|
|
return x, y, z
|
|
|
|
|
|
def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
|
|
|
origin_lat_rad = math.radians(self.origin_lat)
|
|
|
origin_lon_rad = math.radians(self.origin_lon)
|
|
|
|
|
|
lat_rad = origin_lat_rad + (y / self.R)
|
|
|
lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))
|
|
|
|
|
|
lat = math.degrees(lat_rad)
|
|
|
lon = math.degrees(lon_rad)
|
|
|
alt = self.origin_alt + z
|
|
|
|
|
|
return lat, lon, alt
|
|
|
|
|
|
|
|
|
class FormationPlanner:
|
|
|
"""隊形規劃器"""
|
|
|
|
|
|
def __init__(self, spacing: float = 5.0,
|
|
|
base_altitude: float = 10.0,
|
|
|
altitude_diff: float = 2.0):
|
|
|
self.spacing = spacing
|
|
|
self.base_altitude = base_altitude
|
|
|
self.altitude_diff = altitude_diff
|
|
|
self.current_origin = None
|
|
|
self.converter = None
|
|
|
|
|
|
def plan_formation_mission(self,
|
|
|
drone_gps_positions: List[Tuple[float, float, float]],
|
|
|
target_gps: Tuple[float, float, float],
|
|
|
mission_type: MissionType = MissionType.M_FORMATION,
|
|
|
params: Optional[Dict[str, Any]] = None
|
|
|
) -> Tuple[List[List[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("無人機位置列表不能為空")
|
|
|
|
|
|
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)
|
|
|
|
|
|
drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
|
|
|
target_local = self.converter.gps_to_local(*target_gps)
|
|
|
|
|
|
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 {}
|
|
|
route_wps_gps = params.get('route_waypoints')
|
|
|
if route_wps_gps is None or len(route_wps_gps) < 2:
|
|
|
raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
|
|
|
route_wps_local = [
|
|
|
self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
|
|
|
for wp in route_wps_gps
|
|
|
]
|
|
|
waypoints_local, rendezvous_indices = self._calculate_leader_follower(
|
|
|
drone_local, route_wps_local, params
|
|
|
)
|
|
|
|
|
|
elif mission_type == MissionType.GRID_SWEEP:
|
|
|
params = params or {}
|
|
|
rect_corners_gps = params.get('rect_corners')
|
|
|
if rect_corners_gps is None or len(rect_corners_gps) != 4:
|
|
|
raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
|
|
|
rect_corners_local = [
|
|
|
self.converter.gps_to_local(c[0], c[1], 0)[:2]
|
|
|
for c in rect_corners_gps
|
|
|
]
|
|
|
waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
|
|
|
drone_local, rect_corners_local, params
|
|
|
)
|
|
|
else:
|
|
|
raise ValueError(f"不支援的任務類型: {mission_type}")
|
|
|
|
|
|
waypoints_gps = []
|
|
|
for drone_wps in waypoints_local:
|
|
|
gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
|
|
|
waypoints_gps.append(gps_wps)
|
|
|
|
|
|
return waypoints_gps, self.current_origin, rendezvous_indices
|
|
|
|
|
|
# ------------------------------------------------------------------ M-Formation
|
|
|
|
|
|
def _calculate_m_formation(self, drone_positions, target_point, params):
|
|
|
params = params or {}
|
|
|
N = len(drone_positions)
|
|
|
spacing = params.get('spacing', self.spacing)
|
|
|
base_altitude = params.get('base_altitude', self.base_altitude)
|
|
|
altitude_diff = params.get('altitude_diff', self.altitude_diff)
|
|
|
|
|
|
C_x = sum(pos[0] for pos in drone_positions) / N
|
|
|
C_y = sum(pos[1] for pos in drone_positions) / N
|
|
|
|
|
|
T_x, T_y, T_z = target_point
|
|
|
V_x, V_y = T_x - C_x, T_y - C_y
|
|
|
P_x, P_y = -V_y, V_x
|
|
|
|
|
|
length = math.sqrt(P_x ** 2 + P_y ** 2)
|
|
|
P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)
|
|
|
|
|
|
projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
|
|
|
for i, pos in enumerate(drone_positions)]
|
|
|
projections.sort()
|
|
|
|
|
|
stage1_positions = [None] * N
|
|
|
stage2_positions = [None] * N
|
|
|
|
|
|
for rank, (_, original_idx) in enumerate(projections):
|
|
|
offset = (rank - (N - 1) / 2) * spacing
|
|
|
altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
|
|
|
stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
|
|
|
stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)
|
|
|
|
|
|
return stage1_positions, stage2_positions
|
|
|
|
|
|
# ------------------------------------------------------------------ Circle
|
|
|
|
|
|
def _calculate_circle_formation(self, drone_positions, target_point, params):
|
|
|
params = params or {}
|
|
|
N = len(drone_positions)
|
|
|
radius = params.get('radius', 10.0)
|
|
|
altitude = params.get('altitude', 10.0)
|
|
|
start_angle = params.get('start_angle', 0.0)
|
|
|
|
|
|
center_x, center_y, center_z = target_point
|
|
|
stage1_positions = []
|
|
|
stage2_positions = []
|
|
|
angle_step = 360.0 / N
|
|
|
|
|
|
for i in range(N):
|
|
|
angle_rad = math.radians(start_angle + angle_step * i)
|
|
|
final_x = center_x + radius * math.cos(angle_rad)
|
|
|
final_y = center_y + radius * math.sin(angle_rad)
|
|
|
final_z = altitude
|
|
|
|
|
|
current_x, current_y, current_z = drone_positions[i]
|
|
|
stage1_positions.append((
|
|
|
current_x + (final_x - current_x) * 0.5,
|
|
|
current_y + (final_y - current_y) * 0.5,
|
|
|
current_z + (final_z - current_z) * 0.5
|
|
|
))
|
|
|
stage2_positions.append((final_x, final_y, final_z))
|
|
|
|
|
|
return stage1_positions, stage2_positions
|
|
|
|
|
|
# ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)
|
|
|
|
|
|
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)
|
|
|
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)
|
|
|
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,
|
|
|
)
|
|
|
|
|
|
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 做插值。
|
|
|
"""
|
|
|
if n_pts == 1:
|
|
|
pt = center_path[0]
|
|
|
return pt['x'], pt['y'], pt['dir']
|
|
|
|
|
|
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
|
|
|
|
|
|
# 預先算好路徑終點的切線（給收尾點用）
|
|
|
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 i in range(n_pts):
|
|
|
s_follower = s_list[i] - lon
|
|
|
x, y, (dx, dy) = lookup(s_follower)
|
|
|
perp_x, perp_y = -dy, dx
|
|
|
waypoints.append((
|
|
|
x + lat * perp_x,
|
|
|
y + lat * perp_y,
|
|
|
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):
|
|
|
"""
|
|
|
建立以「圓弧」連接直線段的中心路徑。
|
|
|
|
|
|
每個中間 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'
|
|
|
|
|
|
Returns:
|
|
|
(path, barrier_indices)
|
|
|
"""
|
|
|
num_wps = len(waypoints)
|
|
|
|
|
|
if num_wps < 2:
|
|
|
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.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)
|
|
|
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
|
|
|
|
|
|
# 平滑弧
|
|
|
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])
|
|
|
|
|
|
# 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,
|
|
|
})
|
|
|
|
|
|
# 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)
|
|
|
|
|
|
# 終點
|
|
|
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]
|
|
|
cur = path[i]
|
|
|
cur['s'] = prev['s'] + math.hypot(
|
|
|
cur['x'] - prev['x'], cur['y'] - prev['y']
|
|
|
)
|
|
|
|
|
|
return path, barrier_indices
|
|
|
|
|
|
# ------------------------------------------------------------------ 柵狀掃描
|
|
|
|
|
|
def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
|
|
|
"""柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
|
|
|
N = len(drone_positions)
|
|
|
line_spacing = params.get('line_spacing', 5.0)
|
|
|
altitude = params.get('altitude', self.base_altitude)
|
|
|
|
|
|
c0, c1, c2, c3 = rect_corners_local
|
|
|
|
|
|
edge_a = (c1[0] - c0[0], c1[1] - c0[1])
|
|
|
len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
|
|
|
edge_b = (c3[0] - c0[0], c3[1] - c0[1])
|
|
|
len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)
|
|
|
|
|
|
if len_a >= len_b:
|
|
|
sweep_vec = edge_a
|
|
|
sweep_len = len_a
|
|
|
cut_vec = edge_b
|
|
|
cut_len = len_b
|
|
|
sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
|
|
|
sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
|
|
|
cut_start_corner = c0
|
|
|
else:
|
|
|
sweep_vec = edge_b
|
|
|
sweep_len = len_b
|
|
|
cut_vec = edge_a
|
|
|
cut_len = len_a
|
|
|
sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
|
|
|
sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
|
|
|
cut_start_corner = c0
|
|
|
|
|
|
sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
|
|
|
cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)
|
|
|
|
|
|
C_x = sum(p[0] for p in drone_positions) / N
|
|
|
C_y = sum(p[1] for p in drone_positions) / N
|
|
|
|
|
|
dist_to_start = math.sqrt(
|
|
|
(C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
|
|
|
)
|
|
|
dist_to_end = math.sqrt(
|
|
|
(C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
|
|
|
)
|
|
|
|
|
|
if dist_to_start <= dist_to_end:
|
|
|
near_corner_a = cut_start_corner
|
|
|
else:
|
|
|
sweep_dir = (-sweep_dir[0], -sweep_dir[1])
|
|
|
near_corner_a = (cut_start_corner[0] + sweep_vec[0],
|
|
|
cut_start_corner[1] + sweep_vec[1])
|
|
|
|
|
|
projections = [
|
|
|
((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
|
|
|
for i, pos in enumerate(drone_positions)
|
|
|
]
|
|
|
projections.sort()
|
|
|
|
|
|
strip_width = cut_len / N
|
|
|
drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
|
|
|
near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
|
|
|
gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2
|
|
|
|
|
|
all_waypoints = [None] * N
|
|
|
|
|
|
for rank, (_, original_idx) in enumerate(projections):
|
|
|
strip_center_offset = (rank + 0.5) * strip_width
|
|
|
base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
|
|
|
base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset
|
|
|
|
|
|
waypoints_list = []
|
|
|
|
|
|
gather_offset = gather_sweep_proj - near_sweep_proj
|
|
|
gx = base_x + sweep_dir[0] * gather_offset
|
|
|
gy = base_y + sweep_dir[1] * gather_offset
|
|
|
waypoints_list.append((gx, gy, altitude))
|
|
|
|
|
|
num_lines = max(1, int(strip_width / line_spacing))
|
|
|
actual_spacing = strip_width / num_lines
|
|
|
first_line_offset = (rank * strip_width) + actual_spacing / 2
|
|
|
|
|
|
entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
|
|
|
entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
|
|
|
waypoints_list.append((entry_x, entry_y, altitude))
|
|
|
|
|
|
current_cut_offset = first_line_offset
|
|
|
|
|
|
for line_idx in range(num_lines):
|
|
|
line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
|
|
|
line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
|
|
|
line_far_x = line_near_x + sweep_dir[0] * sweep_len
|
|
|
line_far_y = line_near_y + sweep_dir[1] * sweep_len
|
|
|
|
|
|
if line_idx % 2 == 0:
|
|
|
waypoints_list.append((line_far_x, line_far_y, altitude))
|
|
|
else:
|
|
|
waypoints_list.append((line_near_x, line_near_y, altitude))
|
|
|
|
|
|
if line_idx < num_lines - 1:
|
|
|
next_cut_offset = current_cut_offset + actual_spacing
|
|
|
next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
|
|
|
next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
|
|
|
next_far_x = next_near_x + sweep_dir[0] * sweep_len
|
|
|
next_far_y = next_near_y + sweep_dir[1] * sweep_len
|
|
|
|
|
|
if line_idx % 2 == 0:
|
|
|
waypoints_list.append((next_far_x, next_far_y, altitude))
|
|
|
else:
|
|
|
waypoints_list.append((next_near_x, next_near_y, altitude))
|
|
|
current_cut_offset = next_cut_offset
|
|
|
|
|
|
all_waypoints[original_idx] = waypoints_list
|
|
|
|
|
|
# 每一條掃描線的「起點」都做一次同步：
|
|
|
# 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
|
|
|
|
|
|
|
|
|
# ================================================================================
|
|
|
# 測試
|
|
|
# ================================================================================
|
|
|
if __name__ == "__main__":
|
|
|
planner = FormationPlanner()
|
|
|
|
|
|
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),
|
|
|
]
|
|
|
target = (24.12400, 120.56795, 10.0)
|
|
|
|
|
|
# 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 (虛擬領隊 / 弧長參數化)
|
|
|
route = [
|
|
|
(24.12345, 120.56780),
|
|
|
(24.12370, 120.56800),
|
|
|
(24.12390, 120.56820),
|
|
|
(24.12400, 120.56800),
|
|
|
(24.12420, 120.56790),
|
|
|
]
|
|
|
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,
|
|
|
'min_inner_radius': 3.0,
|
|
|
'arc_resolution': 8,
|
|
|
'altitude': 10.0
|
|
|
}
|
|
|
)
|
|
|
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):
|
|
|
print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")
|
|
|
|
|
|
# Grid Sweep
|
|
|
rect = [
|
|
|
(24.1237, 120.5679),
|
|
|
(24.1237, 120.5683),
|
|
|
(24.1240, 120.5683),
|
|
|
(24.1240, 120.5679)
|
|
|
]
|
|
|
wps2, origin2, rv2 = planner.plan_formation_mission(
|
|
|
drones, target, MissionType.GRID_SWEEP,
|
|
|
params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
|
|
|
)
|
|
|
print(f"\nGrid Sweep:")
|
|
|
for i, wp_list in enumerate(wps2):
|
|
|
print(f" Drone {i}: {len(wp_list)} waypoints") |