AirTrapMine/src/GUI/mission_executor.py

#!/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))