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>
lunu
wenchun 3 weeks ago
parent 5c0d21fc1d
commit 0edc477df8
9 changed files with 902 additions and 223 deletions
Show Stats Download Patch File Download Diff File

103
src/GUI/command_sender.py
Unescape Escape View File

@ -3,10 +3,16 @@
指令發送模組
負責將目標位置轉成具體的通訊指令發送到飛控
現階段: MavlinkSender (直接 pymavlink 發送)
未來: 替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
目前使用 Ros2CommandSender，經 fc_network_adapter 的
/fc_network/vehicle/pos_global_int service 發送。
MavlinkSender 保留作為直連 SITL 的 fallback 工具，但已不是預設路徑。
"""
import asyncio
import traceback
from abc import ABC, abstractmethod
from PyQt6.QtCore import QObject, pyqtSignal
from pymavlink import mavutil
@ -14,11 +20,12 @@ class CommandSender(ABC):
"""指令發送抽象介面"""
@abstractmethod
def send_position_global(self, sysid, lat, lon, alt):
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""
發送全球座標位置指令
Args:
drone_id: GUI 用的 drone 識別字串 (如 's0_11')
sysid: 目標無人機的 MAVLink system ID
lat: 緯度 (度)
lon: 經度 (度)
@ -58,16 +65,8 @@ class MavlinkSender(CommandSender):
self.mav = mavutil.mavlink_connection(connection_string)
print(f"MavlinkSender 已建立連線: {connection_string}")
def send_position_global(self, sysid, lat, lon, alt):
"""
發送 SET_POSITION_TARGET_GLOBAL_INT
注意:
- coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
高度是相對起飛點的高度 (公尺)
- 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
需要在外部先減去起飛點高度再傳入
"""
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
self.mav.mav.set_position_target_global_int_send(
0, # time_boot_ms (不使用)
sysid, # target_system
@ -87,4 +86,80 @@ class MavlinkSender(CommandSender):
if self.mav:
self.mav.close()
self.mav = None
print("MavlinkSender 已關閉")
print("MavlinkSender 已關閉")
class Ros2CommandSender(QObject):
"""
透過 fc_network_apps.PositionTargetGlobalIntClient 送 goto 指令。
設計重點:
- send_position_global 是同步介面，立刻回傳。結果透過 send_result signal 回報
- 每台 drone 維護自己的 asyncio.Task pipeline，新的目標會 cancel 掉舊的 in-flight
- client 由 DroneMonitor.get_or_create_position_client 管理，per-drone 獨立節點
"""
# (drone_id, sysid, success, message)
send_result = pyqtSignal(str, int, bool, str)
DEFAULT_TIMEOUT_SEC = 2.0
def __init__(self, monitor, timeout_sec: float = DEFAULT_TIMEOUT_SEC):
super().__init__()
self._monitor = monitor
self._timeout_sec = timeout_sec
self._pending = {} # drone_id -> asyncio.Task
def send_position_global(self, drone_id, sysid, lat, lon, alt):
"""同步介面。立刻回，結果透過 send_result signal 回報。"""
# 取消該機上一個未完成的 task，避免舊指令覆蓋新指令
old = self._pending.get(drone_id)
if old is not None and not old.done():
old.cancel()
try:
loop = asyncio.get_event_loop()
except RuntimeError:
loop = asyncio.new_event_loop()
asyncio.set_event_loop(loop)
task = loop.create_task(
self._do_send(drone_id, int(sysid), float(lat), float(lon), float(alt))
)
self._pending[drone_id] = task
async def _do_send(self, drone_id, sysid, lat, lon, alt):
try:
client = self._monitor.get_or_create_position_client(drone_id)
if client is None:
self.send_result.emit(
drone_id, sysid, False,
"PositionTargetGlobalIntClient 無法建立"
)
return
result = await client.goto_position_only_async(
target_sysid=sysid,
target_compid=0,
latitude_deg=lat,
longitude_deg=lon,
alt=alt,
timeout_sec=self._timeout_sec,
)
self.send_result.emit(
drone_id, sysid, bool(result.success), str(result.message or "")
)
except asyncio.CancelledError:
# 被新的目標取代，正常狀況，不用回報
pass
except Exception as e:
traceback.print_exc()
self.send_result.emit(drone_id, sysid, False, f"exception: {e}")
def close(self):
"""取消所有未完成的 task。PositionTargetGlobalIntClient 由 DroneMonitor 負責 destroy。"""
for task in self._pending.values():
if not task.done():
task.cancel()
self._pending.clear()
print("Ros2CommandSender 已關閉")

39
src/GUI/communication.py
Unescape Escape View File

@ -36,6 +36,16 @@ except ImportError as e:
traceback.print_exc()
CommandLongClient = None
# 導入 fc_network_apps 的 navigation（PositionTargetGlobalInt / Offboard goto）
try:
from fc_network_apps.navigation import PositionTargetGlobalIntClient
except ImportError as e:
import traceback
print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient")
print(f" 错误: {e}")
traceback.print_exc()
PositionTargetGlobalIntClient = None
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
@ -475,6 +485,13 @@ class DroneMonitor(Node):
self.client_counter = 0 # 用於生成唯一的 client 節點名稱
self.executor = None # 將在 gui.py 中設置，用於添加新的 clients
# ================================================================================
# ================================================================================
# PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
# ================================================================================
self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient}
self.pos_client_counter = 0
# ================================================================================
# 主题检测定时器
self.create_timer(1.0, self.scan_topics)
@ -531,7 +548,27 @@ class DroneMonitor(Node):
print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}")
return None
return self.command_long_clients[drone_id]
def get_or_create_position_client(self, drone_id):
"""為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。"""
if PositionTargetGlobalIntClient is None:
return None
with self.client_lock:
if drone_id not in self.position_target_clients:
try:
self.pos_client_counter += 1
unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}"
client = PositionTargetGlobalIntClient(node_name=unique_name)
self.position_target_clients[drone_id] = client
print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})")
if self.executor:
self.executor.add_node(client)
print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器")
except Exception as e:
print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}")
return None
return self.position_target_clients[drone_id]
def scan_topics(self):
topics = self.get_topic_names_and_types()
drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')

55
src/GUI/gui.py
Unescape Escape View File

@ -25,7 +25,7 @@ from overview_table import OverviewTable
# 導入任務規劃器、執行器、發送器、群組
# ================================================================================
from mission_planner import FormationPlanner, MissionType
from command_sender import MavlinkSender
from command_sender import MavlinkSender, Ros2CommandSender
from mission_executor import MissionExecutor, MissionState
from mission_group import (
MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS
@ -122,7 +122,8 @@ class ControlStationUI(QMainWindow):
# ================================================================================
# 初始化指令發送器（所有群組共用）
# ================================================================================
self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死
# 走 ROS2 /fc_network/vehicle/pos_global_int service；per-drone client + asyncio polling
self.command_sender = Ros2CommandSender(monitor=self.monitor)
# ================================================================================
# ================================================================================
@ -671,10 +672,12 @@ class ControlStationUI(QMainWindow):
executor = MissionExecutor(
sender=self.command_sender,
drone_gps=self.monitor.drone_gps,
monitor=self.monitor,
arrival_radius=2.0,
send_rate_hz=2.0
tick_rate_hz=2.0,
)
executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached)
executor.task_status_changed.connect(self._on_task_status_changed)
executor.mission_completed.connect(
lambda gid=group.group_id: self._on_group_mission_completed(gid))
group.executor = executor
@ -1229,11 +1232,15 @@ class ControlStationUI(QMainWindow):
if drone_gps_positions is None:
return
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, mission_type, params=params
)
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
group.center_origin = center_origin
self.show_planned_waypoints(group)
@ -1300,12 +1307,16 @@ class ControlStationUI(QMainWindow):
target_gps = (target_lat, target_lon, base_alt)
params['rect_corners'] = rect_corners
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
params=params
)
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
group.center_origin = center_origin
self.show_planned_waypoints(group)
@ -1349,6 +1360,11 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000)
return
# LEADER_FOLLOWER：把指定的領隊放到 rank 0
leader = getattr(group, 'leader_drone_id', None)
if leader and leader in selected_drones:
selected_drones = [leader] + [d for d in selected_drones if d != leader]
print(f"路徑確認 → Group {group.group_id}: {points_json}")
try:
route_points = json.loads(points_json)
@ -1377,12 +1393,16 @@ class ControlStationUI(QMainWindow):
target_gps = (target_lat, target_lon, base_alt)
params['route_waypoints'] = route_waypoints
waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission(
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
params=params
)
group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone}
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
group.center_origin = center_origin
self.show_planned_waypoints(group)
@ -1421,6 +1441,17 @@ class ControlStationUI(QMainWindow):
else:
self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000)
def _on_task_status_changed(self, drone_id, status, message):
"""MissionExecutor.task_status_changed slot：把 goto 失敗/重試/fallback/barrier 丟到 status bar"""
if status == "retrying":
self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000)
elif status == "fallback_loiter":
self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000)
elif status == "waiting_at_barrier":
self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000)
elif status == "normal":
self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000)
# ================================================================================
# 輔助方法
# ================================================================================
@ -1747,6 +1778,12 @@ class ControlStationUI(QMainWindow):
client.destroy_node()
except:
pass
# Clean up all PositionTargetGlobalIntClient instances
for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items():
try:
client.destroy_node()
except:
pass
self.monitor.destroy_node()
self.executor.shutdown()
except Exception as e:

275
src/GUI/mission_executor.py
Unescape Escape View File

@ -6,11 +6,15 @@
設計:
- 每架無人機持有一個航點序列，逐點推進
- 各自到達就各自切換到下一個航點
- 用 QTimer 驅動，在 Qt 主線程執行
- 暫停 = 停止發送 setpoint，飛控自動懸停
- 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
- 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
- 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
- Rendezvous barrier：在指定 wp index 等所有活著的機到齊才一起推進，有 timeout 保護
- 用 QTimer 驅動到達判定，在 Qt 主線程執行
- 暫停 = 停止到達判定 + 停止新指令
"""
import asyncio
import math
import time
from enum import Enum
from PyQt6.QtCore import QObject, QTimer, pyqtSignal
@ -22,22 +26,31 @@ class MissionState(Enum):
PAUSED = "paused"
class TaskStatus(Enum):
"""單機任務狀態"""
NORMAL = "normal"
RETRYING = "retrying"
WAITING_AT_BARRIER = "waiting_at_barrier"
FALLBACK_LOITER = "fallback_loiter"
class DroneTask:
"""單架無人機的任務資料"""
__slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done')
__slots__ = (
'drone_id', 'sysid', 'waypoints', 'wp_index', 'done',
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
)
def __init__(self, drone_id, sysid, waypoints):
"""
Args:
drone_id: GUI 用的 ID (如 's0_11')
sysid: MAVLink system ID (如 11)
waypoints: 航點序列 [(lat, lon, alt), ...]
"""
self.drone_id = drone_id
self.sysid = sysid
self.waypoints = waypoints
self.wp_index = 0
self.done = len(waypoints) == 0
self.sent_current_wp = False
self.fail_count = 0
self.status = TaskStatus.NORMAL
self.waiting_since = 0.0 # monotonic time 進入 WAITING_AT_BARRIER 的瞬間
@property
def current_target(self):
@ -52,7 +65,7 @@ class DroneTask:
class MissionExecutor(QObject):
"""
任務執行器
任務執行器 (事件驅動版 + rendezvous barrier)
planned_waypoints 格式:
{
@ -60,31 +73,41 @@ class MissionExecutor(QObject):
'waypoints': [
[(lat,lon,alt), ...], # drone 0
[(lat,lon,alt), ...], # drone 1
]
],
'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑）
}
"""
# 信號
drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
MAX_RETRY = 3
drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total)
task_status_changed = pyqtSignal(str, str, str) # (drone_id, status, message)
mission_completed = pyqtSignal()
def __init__(self, sender, drone_gps,
arrival_radius=2.0, send_rate_hz=2.0):
def __init__(self, sender, drone_gps, monitor=None,
arrival_radius=2.0, tick_rate_hz=2.0,
barrier_timeout_sec=20.0):
super().__init__()
self.sender = sender
self.drone_gps = drone_gps
self.monitor = monitor # 用於失敗 fallback 到 LOITER
self.arrival_radius = arrival_radius
self.barrier_timeout_sec = barrier_timeout_sec
self.state = MissionState.IDLE
self.tasks = {}
self.rendezvous_indices = set()
self._interval_ms = int(1000 / send_rate_hz)
self._interval_ms = int(1000 / tick_rate_hz)
self._timer = QTimer()
self._timer.timeout.connect(self._tick)
# 只有 Ros2CommandSender 有 send_result signal；MavlinkSender 沒有
if hasattr(sender, 'send_result'):
sender.send_result.connect(self._on_send_result)
# ------------------------------------------------------------------ 公開方法
def start(self, planned_waypoints):
"""啟動任務"""
if self.state == MissionState.RUNNING:
print("任務已在執行中")
return
@ -93,6 +116,9 @@ class MissionExecutor(QObject):
drone_ids = planned_waypoints['drone_ids']
waypoints_list = planned_waypoints['waypoints']
self.rendezvous_indices = set(
planned_waypoints.get('rendezvous_indices', []) or []
)
for i, drone_id in enumerate(drone_ids):
sysid = int(drone_id.split('_')[1])
@ -102,87 +128,232 @@ class MissionExecutor(QObject):
self._timer.start(self._interval_ms)
total_wps = sum(t.total_waypoints for t in self.tasks.values())
rv_info = (
f"rendezvous WP={sorted(self.rendezvous_indices)}"
if self.rendezvous_indices else "無 rendezvous (各自跑)"
)
print(f"任務啟動: {len(self.tasks)} 架無人機, "
f"共 {total_wps} 個航點, "
f"到達半徑={self.arrival_radius}m, "
f"發送週期={self._interval_ms}ms")
f"tick 週期={self._interval_ms}ms, "
f"barrier timeout={self.barrier_timeout_sec}s, "
f"{rv_info}")
def pause(self):
"""暫停任務"""
if self.state == MissionState.RUNNING:
self._timer.stop()
self.state = MissionState.PAUSED
print("任務暫停")
def resume(self):
"""恢復任務"""
if self.state == MissionState.PAUSED:
self._timer.start(self._interval_ms)
self.state = MissionState.RUNNING
print("任務恢復")
def stop(self):
"""停止任務"""
self._timer.stop()
self.tasks.clear()
self.rendezvous_indices = set()
self.state = MissionState.IDLE
print("任務停止")
# ------------------------------------------------------------------ 控制迴圈
def _tick(self):
"""控制迴圈"""
all_done = True
"""
控制 tick：
Phase 1: 每台機做到達判定；非 barrier 點直接推進，barrier 點改成 WAITING
Phase 2: 檢查 barrier 能不能釋放（全員到齊或 timeout）
Phase 3: 每台未送過當前航點的機送一次
Phase 4: 全部完成檢查
"""
now = time.monotonic()
# ---- Phase 1: 到達判定 ----
for task in self.tasks.values():
if task.done:
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
continue
# 已在 barrier 等待的不再跑到達判定
if task.status == TaskStatus.WAITING_AT_BARRIER:
continue
all_done = False
target = task.current_target
if target is None:
continue
# 讀取當前 GPS
gps = self.drone_gps.get(task.drone_id)
if gps is None:
continue
tgt_lat, tgt_lon, tgt_alt = target
tgt_lat, tgt_lon, _ = target
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
if distance >= self.arrival_radius:
continue
# 到達判定
if distance < self.arrival_radius:
task.wp_index += 1
if task.wp_index >= task.total_waypoints:
task.done = True
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → DONE "
f"({task.total_waypoints}/{task.total_waypoints})")
continue
else:
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
f"(距離: {distance:.1f}m)")
# 更新目標
tgt_lat, tgt_lon, tgt_alt = task.current_target
# 發送 setpoint
# 到達當前 wp_index
if (task.wp_index in self.rendezvous_indices
and task.wp_index < task.total_waypoints - 1):
# rendezvous 點 → 不推進，進入 barrier 等待
task.status = TaskStatus.WAITING_AT_BARRIER
task.waiting_since = now
print(f" {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
self.task_status_changed.emit(
task.drone_id, task.status.value,
f"waiting at WP {task.wp_index}"
)
continue
# 一般點 → 直接推進
self._advance_waypoint(task, distance)
# ---- Phase 2: barrier 釋放檢查 ----
self._check_barriers(now)
# ---- Phase 3: 發送未送過的目標 ----
for task in self.tasks.values():
if task.done or task.status in (
TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
):
continue
if task.sent_current_wp:
continue
target = task.current_target
if target is None:
continue
tgt_lat, tgt_lon, tgt_alt = target
task.sent_current_wp = True
self.sender.send_position_global(
task.sysid, tgt_lat, tgt_lon, tgt_alt
task.drone_id, task.sysid, tgt_lat, tgt_lon, tgt_alt
)
# 全部完成檢查
# ---- Phase 4: 完成檢查 ----
all_done = all(
t.done or t.status == TaskStatus.FALLBACK_LOITER
for t in self.tasks.values()
)
if all_done and self.tasks:
self._timer.stop()
self.state = MissionState.IDLE
self.mission_completed.emit()
print("===== 任務全部完成 =====")
def _advance_waypoint(self, task, arrived_distance):
"""把 task 推進一個航點，重置發送旗標。不處理 barrier 邏輯。"""
task.wp_index += 1
task.sent_current_wp = False
task.fail_count = 0
if task.wp_index >= task.total_waypoints:
task.done = True
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → DONE "
f"({task.total_waypoints}/{task.total_waypoints})")
return
self.drone_waypoint_reached.emit(
task.drone_id, task.wp_index, task.total_waypoints
)
print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
f"(到達距離: {arrived_distance:.1f}m)")
def _check_barriers(self, now):
"""檢查每個有人在等的 barrier 能不能釋放。"""
# 蒐集目前有哪些 barrier 有人卡著
waiting_groups = {} # barrier_idx -> [task, ...]
for task in self.tasks.values():
if task.status == TaskStatus.WAITING_AT_BARRIER:
waiting_groups.setdefault(task.wp_index, []).append(task)
if not waiting_groups:
return
for barrier_idx, waiting_tasks in waiting_groups.items():
# 判斷是否「全員到齊」：所有仍活著的機都要麼在此 barrier 等，要麼早已越過此 idx
everyone_arrived = True
for task in self.tasks.values():
if task.done or task.status == TaskStatus.FALLBACK_LOITER:
continue # 失敗機/完成機不擋
if (task.status == TaskStatus.WAITING_AT_BARRIER
and task.wp_index == barrier_idx):
continue # 在這裡等
if task.wp_index > barrier_idx:
continue # 早已越過（barrier 先前已對他釋放）
# 還在路上
everyone_arrived = False
break
force_reason = None
if not everyone_arrived:
# 檢查 timeout：以該 barrier 上最早進入等待的時間為準
oldest_wait = min(t.waiting_since for t in waiting_tasks)
if now - oldest_wait >= self.barrier_timeout_sec:
force_reason = f"timeout {self.barrier_timeout_sec:.0f}s"
print(f"⚠️ barrier WP {barrier_idx} {force_reason}，強制放行")
else:
continue # 還沒到齊、也還沒 timeout → 繼續等
# 釋放：把所有在此 barrier 等待的機一起推進
tag = "全員到齊" if force_reason is None else force_reason
print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})，"
f"推進 {len(waiting_tasks)} 架")
for task in waiting_tasks:
task.status = TaskStatus.NORMAL
msg = f"barrier WP {barrier_idx} released ({tag})"
self.task_status_changed.emit(
task.drone_id, task.status.value, msg
)
self._advance_waypoint(task, self.arrival_radius)
# ------------------------------------------------------------------ 結果回呼
def _on_send_result(self, drone_id, sysid, success, message):
"""Ros2CommandSender.send_result 的 slot"""
task = self.tasks.get(drone_id)
if task is None or task.done:
return
# 若已在 barrier 等待，舊指令的遲到回應不要再觸發重試/fallback
if task.status == TaskStatus.WAITING_AT_BARRIER:
return
if success:
if task.fail_count > 0 or task.status == TaskStatus.RETRYING:
task.status = TaskStatus.NORMAL
self.task_status_changed.emit(drone_id, task.status.value, "recovered")
task.fail_count = 0
return
task.fail_count += 1
print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}")
if task.fail_count < self.MAX_RETRY:
task.status = TaskStatus.RETRYING
task.sent_current_wp = False # 下個 tick 會重送
self.task_status_changed.emit(
drone_id, task.status.value,
f"retry {task.fail_count}/{self.MAX_RETRY}: {message}"
)
else:
task.status = TaskStatus.FALLBACK_LOITER
self.task_status_changed.emit(
drone_id, task.status.value,
f"fallback LOITER after {self.MAX_RETRY} fails: {message}"
)
print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER")
self._fallback_to_loiter(drone_id)
def _fallback_to_loiter(self, drone_id):
"""用 monitor.set_mode 切 LOITER。set_mode 是 coroutine，透過 event loop 派送。"""
if self.monitor is None:
print(f"⚠️ 無 monitor，無法將 {drone_id} 切到 LOITER")
return
try:
loop = asyncio.get_event_loop()
except RuntimeError:
loop = asyncio.new_event_loop()
asyncio.set_event_loop(loop)
loop.create_task(self.monitor.set_mode(drone_id, "LOITER"))
# ------------------------------------------------------------------ 工具函式
@ -196,4 +367,4 @@ def _haversine(lat1, lon1, lat2, lon2):
a = (math.sin(dlat / 2) ** 2 +
math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2)
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

38
src/GUI/mission_group.py
Unescape Escape View File

@ -35,6 +35,7 @@ class MissionGroup:
self.planned_waypoints = None # 規劃結果 dict
self.executor = None # MissionExecutor 實例（延遲建立）
self.center_origin = None # 規劃原點
self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID
@property
def state(self):
@ -415,6 +416,21 @@ class GroupPanel(QWidget):
self._param_widgets[key] = (row_w, inp)
self._param_rows.append(row_w)
# LEADER_FOLLOWER 專用：領隊下拉選單
self._leader_row = QWidget()
leader_layout = QHBoxLayout(self._leader_row)
leader_layout.setContentsMargins(0, 0, 0, 0)
leader_layout.setSpacing(3)
leader_lbl = QLabel("領隊")
leader_lbl.setStyleSheet(LBL)
self._leader_combo = QComboBox()
self._leader_combo.setStyleSheet(COMBO)
self._leader_combo.setFixedWidth(70)
self._leader_combo.currentTextChanged.connect(self._on_leader_changed)
leader_layout.addWidget(leader_lbl, 1)
leader_layout.addWidget(self._leader_combo)
param_col.addWidget(self._leader_row)
param_col.addStretch()
# 初始顯示對應的參數
@ -447,6 +463,27 @@ class GroupPanel(QWidget):
self.drone_list_label.setText("、".join(sorted_ids))
self.drone_list_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
self._refresh_leader_options()
def _refresh_leader_options(self):
"""依目前群組成員重新填充領隊下拉選單，保留目前選擇若仍有效"""
sorted_ids = sorted(self.group.drone_ids,
key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
current = self.group.leader_drone_id
self._leader_combo.blockSignals(True)
self._leader_combo.clear()
self._leader_combo.addItems(sorted_ids)
if current and current in sorted_ids:
self._leader_combo.setCurrentText(current)
elif sorted_ids:
self._leader_combo.setCurrentText(sorted_ids[0])
self.group.leader_drone_id = sorted_ids[0]
else:
self.group.leader_drone_id = None
self._leader_combo.blockSignals(False)
def _on_leader_changed(self, drone_id):
self.group.leader_drone_id = drone_id if drone_id else None
def update_status(self):
"""更新任務狀態顯示"""
@ -485,6 +522,7 @@ class GroupPanel(QWidget):
visible_keys = {d[0] for d in self._param_defs.get(mission_type, [])}
for key, (row_w, _inp) in self._param_widgets.items():
row_w.setVisible(key in visible_keys)
self._leader_row.setVisible(mission_type == 'LEADER_FOLLOWER')
def get_mission_params(self):
"""讀取當前顯示的參數值，回傳 dict"""

443
src/GUI/mission_planner.py
Unescape Escape 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 曲線轉彎版
步驟:
1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
隊形 (俯視):
| (前進方向) |
| D1 | ← 左偏移, 後 0m
| D2 | ← 右偏移, 後 5m
| D3 | ← 左偏移, 後 10m
| D4 | ← 右偏移, 後 15m
路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (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)
turn_margin = params.get('turn_margin', 0.35) # 轉彎切入距離佔段長比例
curve_resolution = params.get('curve_resolution', 8) # 每個彎道的插值點數
# Step 1: 建立帶 Bezier 轉彎的中心路徑
center_path = self._build_center_path(
route_wps_local, turn_margin, curve_resolution
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 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
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()
# Step 3: 偏移
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
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,
))
# 收尾：全員到 path 終點的 rank 位置（lon 歸零）
waypoints.append((
end['x'] + lat * (-end_dy),
end['y'] + lat * end_dx,
altitude,
))
all_waypoints[original_idx] = waypoints
all_waypoints[rank] = waypoints
return all_waypoints
# barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
# 不觸及末尾收尾點，直接沿用即可
return all_waypoints, barrier_indices
def _build_center_path(self, waypoints, turn_margin, curve_resolution):
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)
Args:
waypoints: 路徑航點 [(x, y), ...]
turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
curve_resolution: 每個彎道的 Bezier 插值點數
建立以「圓弧」連接直線段的中心路徑。
每個中間 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:
[(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]
# 切入距離 T: 取相鄰兩段中較短的 × turn_margin
T = min(len_in, len_out) * turn_margin
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)
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_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
# P1 = WP 本身 (控制點)
p1_x = waypoints[i][0]
p1_y = waypoints[i][1]
# P2 = post_turn (弧線結束)
p2_x = waypoints[i][0] + d_out_x * T
p2_y = waypoints[i][1] + d_out_y * T
# 加入 pre_turn 點 (方向 = incoming)
path.append((p0_x, p0_y, d_in_x, d_in_y))
# 加入 Bezier 插值點
for j in range(1, curve_resolution):
t = j / curve_resolution
# 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
# 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_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
path.append((bx, by, tdx, tdy))
# 加入 post_turn 點 (方向 = outgoing)
path.append((p2_x, p2_y, d_out_x, d_out_y))
# 最後一個航點
path.append((waypoints[-1][0], waypoints[-1][1],
segments[-1][0], segments[-1][1]))
# 平滑弧
beta = math.acos(dot)
if beta < 1e-4:
# 幾乎直線，不插弧
path.append({
'seg': 'straight',
'x': wp[0], 'y': wp[1],
'dir': u_in,
})
continue
return path
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
# ------------------------------------------------------------------ 柵狀掃描
@ -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}
)

4
src/GUI/validation/test_mission.py
Unescape Escape View File

@ -210,7 +210,7 @@ def main():
target_gps = (target_lat, target_lon, BASE_ALTITUDE)
print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})")
waypoints_per_drone, origin = planner.plan_formation_mission(
waypoints_per_drone, origin, _ = planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.M_FORMATION
)
@ -232,7 +232,7 @@ def main():
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(
waypoints_per_drone, origin, _ = planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
params={
'rect_corners': rect_corners,

6
src/GUI/validation/verify_waypoints.py
Unescape Escape View File

@ -365,7 +365,7 @@ def visualize_standalone():
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)
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))],
@ -380,7 +380,7 @@ def visualize_standalone():
# 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(
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}
)
@ -403,7 +403,7 @@ def visualize_standalone():
(24.12410, 120.56800),
(24.12420, 120.56790),
]
wps_lf, origin_lf = planner.plan_formation_mission(
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}

162
src/fc_network_apps/navigation.py
Unescape Escape View File

@ -2,6 +2,7 @@
from __future__ import annotations
import asyncio
import math
from dataclasses import dataclass
@ -122,10 +123,13 @@ class PositionTargetGlobalIntClient(Node):
並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
"""
def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
if not rclpy.ok():
rclpy.init(args=None)
super().__init__("fc_position_target_global_int_client")
if node_name is None:
import time
node_name = f"fc_position_target_global_int_client_{int(time.time() * 1000) % 100000}"
super().__init__(node_name)
self._client = self.create_client(MavPositionTargetGlobalInt, service_name)
def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
@ -240,7 +244,7 @@ class PositionTargetGlobalIntClient(Node):
):
lat_int = int(round(latitude_deg * 1e7))
lon_int = int(round(longitude_deg * 1e7))
return self._send_position_target_global_int(
target_sysid=target_sysid,
target_compid=target_compid,
@ -260,3 +264,155 @@ class PositionTargetGlobalIntClient(Node):
yaw_rate=0,
timeout_sec=timeout_sec,
)
# ============================================================================
# 非阻塞 async 版本（asyncio polling，對齊 longCommand.py 的模板）
# 避免在 Qt 事件迴圈或主執行緒中呼叫 spin_until_future_complete
# ============================================================================
async def _send_position_target_global_int_async(
self,
*,
target_sysid: int,
target_compid: int,
time_boot_ms: int,
coordinate_frame: int,
type_mask: int,
lat_int: int,
lon_int: int,
alt: float,
vx: float,
vy: float,
vz: float,
afx: float,
afy: float,
afz: float,
yaw: float,
yaw_rate: float,
timeout_sec: float,
) -> PositionTargetGlobalIntResult:
"""非阻塞 async 版本 - 使用 asyncio polling 而非 spin"""
req = MavPositionTargetGlobalInt.Request()
req.target_sysid = target_sysid
req.target_compid = target_compid
req.time_boot_ms = time_boot_ms
req.coordinate_frame = coordinate_frame
req.type_mask = type_mask
req.lat_int = lat_int
req.lon_int = lon_int
req.alt = float(alt)
req.vx = float(vx)
req.vy = float(vy)
req.vz = float(vz)
req.afx = float(afx)
req.afy = float(afy)
req.afz = float(afz)
req.yaw = float(yaw)
req.yaw_rate = float(yaw_rate)
req.timeout_sec = float(timeout_sec)
future = self._client.call_async(req)
timeout = float(timeout_sec) + 1.0
elapsed = 0.0
poll_interval = 0.01 # 10ms - 與 GUI timer 頻率一致
while elapsed < timeout:
if future.done():
try:
resp = future.result()
if resp is None:
return PositionTargetGlobalIntResult(
success=False,
message="Service returned None.",
ack_result=-1,
)
if resp.ack_result != 0:
return PositionTargetGlobalIntResult(
success=False,
message=resp.message,
ack_result=resp.ack_result,
)
echo_ok, echo_detail = _echo_position_target_matches(
type_mask=type_mask,
lat_int=lat_int,
lon_int=lon_int,
alt=alt,
vx=vx,
vy=vy,
vz=vz,
afx=afx,
afy=afy,
afz=afz,
yaw=yaw,
yaw_rate=yaw_rate,
time_boot_ms=time_boot_ms,
resp=resp,
)
return PositionTargetGlobalIntResult(
success=echo_ok,
message=echo_detail,
ack_result=resp.ack_result,
r_time_boot_ms=int(resp.r_time_boot_ms),
r_type_mask=int(resp.r_type_mask),
r_lat_int=int(resp.r_lat_int),
r_lon_int=int(resp.r_lon_int),
r_alt=float(resp.r_alt),
r_vx=float(resp.r_vx),
r_vy=float(resp.r_vy),
r_vz=float(resp.r_vz),
r_afx=float(resp.r_afx),
r_afy=float(resp.r_afy),
r_afz=float(resp.r_afz),
r_yaw=float(resp.r_yaw),
r_yaw_rate=float(resp.r_yaw_rate),
)
except Exception as e:
return PositionTargetGlobalIntResult(
success=False,
message=f"Error getting result: {e}",
ack_result=-1,
)
await asyncio.sleep(poll_interval)
elapsed += poll_interval
return PositionTargetGlobalIntResult(
success=False,
message=f"Service timeout after {timeout}s.",
ack_result=-1,
)
async def goto_position_only_async(
self,
*,
target_sysid: int,
target_compid: int = 0,
latitude_deg: float,
longitude_deg: float,
alt: float = 0.0,
timeout_sec: float = DEFAULT_TIMEOUT_SEC,
) -> PositionTargetGlobalIntResult:
"""非阻塞 async 版本的 goto_position_only"""
lat_int = int(round(latitude_deg * 1e7))
lon_int = int(round(longitude_deg * 1e7))
return await self._send_position_target_global_int_async(
target_sysid=target_sysid,
target_compid=target_compid,
time_boot_ms=0,
coordinate_frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
type_mask=POSITION_ONLY,
lat_int=lat_int,
lon_int=lon_int,
alt=alt,
vx=0,
vy=0,
vz=0,
afx=0,
afy=0,
afz=0,
yaw=0,
yaw_rate=0,
timeout_sec=timeout_sec,
)

Write Preview
Loading…
Cancel
Save