feat(mission): virtual-leader path following + 固定領隊 + barrier

- 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>

src/GUI/mission_executor.py

@@ -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
+            # 到達當前 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
 
-            # 發送 setpoint
+            # 一般點 → 直接推進
+            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))