|
|
#!/usr/bin/env python3
|
|
|
"""
|
|
|
任務執行模組
|
|
|
管理多架無人機的 GUIDED 模式飛行控制迴圈
|
|
|
|
|
|
設計:
|
|
|
- 每架無人機持有一個航點序列，逐點推進
|
|
|
- 各自到達就各自切換到下一個航點
|
|
|
- 事件驅動發送：航點切換時送一次，收到 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
|
|
|
|
|
|
|
|
|
def _log(level, message):
|
|
|
print(f"[{level}] {message}")
|
|
|
|
|
|
|
|
|
class MissionState(Enum):
|
|
|
"""整體任務狀態"""
|
|
|
IDLE = "idle"
|
|
|
RUNNING = "running"
|
|
|
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',
|
|
|
'sent_current_wp', 'fail_count', 'status', 'waiting_since',
|
|
|
)
|
|
|
|
|
|
def __init__(self, drone_id, sysid, waypoints):
|
|
|
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):
|
|
|
if self.done or self.wp_index >= len(self.waypoints):
|
|
|
return None
|
|
|
return self.waypoints[self.wp_index]
|
|
|
|
|
|
@property
|
|
|
def total_waypoints(self):
|
|
|
return len(self.waypoints)
|
|
|
|
|
|
|
|
|
class MissionExecutor(QObject):
|
|
|
"""
|
|
|
任務執行器 (事件驅動版 + rendezvous barrier)
|
|
|
|
|
|
planned_waypoints 格式:
|
|
|
{
|
|
|
'drone_ids': ['s0_1', 's0_2', ...],
|
|
|
'waypoints': [
|
|
|
[(lat,lon,alt), ...], # drone 0
|
|
|
[(lat,lon,alt), ...], # drone 1
|
|
|
],
|
|
|
'rendezvous_indices': [3, 7, ...], # 選填；缺省 = 全程不同步（各自跑）
|
|
|
}
|
|
|
"""
|
|
|
|
|
|
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, 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 / 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:
|
|
|
_log("WARN", "任務已在執行中")
|
|
|
return
|
|
|
|
|
|
self.tasks.clear()
|
|
|
|
|
|
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])
|
|
|
self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i])
|
|
|
|
|
|
self.state = MissionState.RUNNING
|
|
|
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 (各自跑)"
|
|
|
)
|
|
|
_log(
|
|
|
"INFO",
|
|
|
f"任務已啟動: {len(self.tasks)} 架無人機, "
|
|
|
f"共 {total_wps} 個航點, "
|
|
|
f"到達半徑={self.arrival_radius}m, "
|
|
|
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
|
|
|
_log("INFO", "任務已暫停")
|
|
|
|
|
|
def resume(self):
|
|
|
if self.state == MissionState.PAUSED:
|
|
|
self._timer.start(self._interval_ms)
|
|
|
self.state = MissionState.RUNNING
|
|
|
_log("INFO", "任務已恢復")
|
|
|
|
|
|
def stop(self):
|
|
|
self._timer.stop()
|
|
|
self.tasks.clear()
|
|
|
self.rendezvous_indices = set()
|
|
|
self.state = MissionState.IDLE
|
|
|
_log("INFO", "任務已停止")
|
|
|
|
|
|
# ------------------------------------------------------------------ 控制迴圈
|
|
|
|
|
|
def _tick(self):
|
|
|
"""
|
|
|
控制 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 or task.status == TaskStatus.FALLBACK_LOITER:
|
|
|
continue
|
|
|
# 已在 barrier 等待的不再跑到達判定
|
|
|
if task.status == TaskStatus.WAITING_AT_BARRIER:
|
|
|
continue
|
|
|
|
|
|
target = task.current_target
|
|
|
if target is None:
|
|
|
continue
|
|
|
|
|
|
gps = self.drone_gps.get(task.drone_id)
|
|
|
if gps is None:
|
|
|
continue
|
|
|
|
|
|
tgt_lat, tgt_lon, _ = target
|
|
|
distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon)
|
|
|
if distance >= self.arrival_radius:
|
|
|
continue
|
|
|
|
|
|
# 到達當前 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
|
|
|
_log("INFO", 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.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()
|
|
|
_log("INFO", "任務全部完成")
|
|
|
|
|
|
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
|
|
|
)
|
|
|
_log(
|
|
|
"INFO",
|
|
|
f"{task.drone_id} 已完成所有航點 "
|
|
|
f"({task.total_waypoints}/{task.total_waypoints})",
|
|
|
)
|
|
|
return
|
|
|
self.drone_waypoint_reached.emit(
|
|
|
task.drone_id, task.wp_index, task.total_waypoints
|
|
|
)
|
|
|
_log(
|
|
|
"INFO",
|
|
|
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"
|
|
|
_log("WARN", f"barrier WP {barrier_idx} {force_reason}，強制放行")
|
|
|
else:
|
|
|
continue # 還沒到齊、也還沒 timeout → 繼續等
|
|
|
|
|
|
# 釋放：把所有在此 barrier 等待的機一起推進
|
|
|
tag = "全員到齊" if force_reason is None else force_reason
|
|
|
_log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag})，推進 {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
|
|
|
_log("WARN", 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}"
|
|
|
)
|
|
|
_log("ERROR", 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:
|
|
|
_log("WARN", 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"))
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------------ 工具函式
|
|
|
|
|
|
def _haversine(lat1, lon1, lat2, lon2):
|
|
|
"""計算兩個 GPS 座標間的水平距離 (公尺)"""
|
|
|
R = 6371000.0
|
|
|
lat1_r = math.radians(lat1)
|
|
|
lat2_r = math.radians(lat2)
|
|
|
dlat = math.radians(lat2 - lat1)
|
|
|
dlon = math.radians(lon2 - lon1)
|
|
|
|
|
|
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))
|