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/command_sender.py

@@ -3,10 +3,16 @@
 指令發送模組
 負責將目標位置轉成具體的通訊指令發送到飛控
 
-現階段: MavlinkSender (直接 pymavlink 發送)
+目前使用 Ros2CommandSender，經 fc_network_adapter 的
-未來:   替換為 ROS2Sender (發到 ROS2 topic，由 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):
+    def send_position_global(self, drone_id, sysid, lat, lon, alt):
-        """
+        """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用，保留為介面相容。"""
-        發送 SET_POSITION_TARGET_GLOBAL_INT
-
-        注意:
-            - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
-              高度是相對起飛點的高度 (公尺)
-            - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
-              需要在外部先減去起飛點高度再傳入
-        """
         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 已關閉")

src/GUI/communication.py

@@ -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+)')

src/GUI/gui.py

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

src/GUI/mission_executor.py

@@ -6,11 +6,15 @@
 設計:
     - 每架無人機持有一個航點序列，逐點推進
     - 各自到達就各自切換到下一個航點
-    - 用 QTimer 驅動，在 Qt 主線程執行
+    - 事件驅動發送：航點切換時送一次，收到 send_result 才決定重送/前進
-    - 暫停 = 停止發送 setpoint，飛控自動懸停
+    - 失敗策略 (b)：單機重試 MAX_RETRY 次仍失敗 → 該機 fallback LOITER，其他機繼續
-    - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep)
+    - 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, ...],   # 選填；缺省 = 全程不同步（各自跑）
         }
     """
 
-    # 信號
+    MAX_RETRY = 3
-    drone_waypoint_reached = pyqtSignal(str, int, int)  # (drone_id, wp_index, total)
+
+    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,
+    def __init__(self, sender, drone_gps, monitor=None,
-                 arrival_radius=2.0, send_rate_hz=2.0):
+                 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
 
-            # 到達判定
+            # 到達當前 wp_index
-            if distance < self.arrival_radius:
+            if (task.wp_index in self.rendezvous_indices
-                task.wp_index += 1
+                    and task.wp_index < task.total_waypoints - 1):
-                if task.wp_index >= task.total_waypoints:
+                # rendezvous 點 → 不推進，進入 barrier 等待
-                    task.done = True
+                task.status = TaskStatus.WAITING_AT_BARRIER
-                    self.drone_waypoint_reached.emit(
+                task.waiting_since = now
-                        task.drone_id, task.wp_index, task.total_waypoints
+                print(f"  {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
-                    )
+                self.task_status_changed.emit(
-                    print(f"  {task.drone_id} → DONE "
+                    task.drone_id, task.status.value,
-                          f"({task.total_waypoints}/{task.total_waypoints})")
+                    f"waiting at WP {task.wp_index}"
-                    continue
+                )
-                else:
+                continue
-                    self.drone_waypoint_reached.emit(
+
-                        task.drone_id, task.wp_index, task.total_waypoints
+            # 一般點 → 直接推進
-                    )
+            self._advance_waypoint(task, distance)
-                    print(f"  {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} "
+
-                          f"(距離: {distance:.1f}m)")
+        # ---- Phase 2: barrier 釋放檢查 ----
-                    # 更新目標
+        self._check_barriers(now)
-                    tgt_lat, tgt_lon, tgt_alt = task.current_target
+
-
+        # ---- Phase 3: 發送未送過的目標 ----
-            # 發送 setpoint
+        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))

src/GUI/mission_group.py

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

src/GUI/mission_planner.py

@@ -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 曲線轉彎版
+        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化版 (virtual leader / arc-length)
-
+
-        步驟:
+        核心想法：
-            1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎
+            - center_path 建好後計算每個 sample 的累積弧長 s
-            2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移
+            - 每架 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 處理，避免倒退或衝出
-            |           D1          |  ← 左偏移, 後 0m
+
-            |                D2     |  ← 右偏移, 後 5m
+        解決前版「空間偏移」的三個 bug：
-            |           D3          |  ← 左偏移, 後 10m
+            1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
-            |                D4     |  ← 右偏移, 後 15m
+            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)      # 轉彎切入距離佔段長比例
+        min_inner_radius = params.get('min_inner_radius', 3.0)
-        curve_resolution = params.get('curve_resolution', 8)  # 每個彎道的插值點數
+        arc_resolution = params.get('arc_resolution', 12)
-
+        sharp_turn_cos = params.get('sharp_turn_cos_threshold', 0.0)  # cos(90°)=0
-        # Step 1: 建立帶 Bezier 轉彎的中心路徑
+
-        center_path = self._build_center_path(
+        # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
-            route_wps_local, turn_margin, curve_resolution
+        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: 固定排序
+        s_list = [pt['s'] for pt in center_path]
-        first_dir = (center_path[0][2], center_path[0][3])
+        s_max = s_list[-1]
-        first_perp = (-first_dir[1], first_dir[0])
+        n_pts = len(center_path)
-        C_x = sum(p[0] for p in drone_positions) / N
+
-        C_y = sum(p[1] for p in drone_positions) / N
+        def lookup(s_target):
-
+            """
-        projections = [
+            在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
-            ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
+            s<0 → clip 到 start；s>s_max → clip 到 end。
-            for i, pos in enumerate(drone_positions)
+            tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
-        ]
+            """
-        projections.sort()
+            if n_pts == 1:
-
+                pt = center_path[0]
-        # Step 3: 偏移
+                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
+                waypoints.append((
-                oy = cy + lat * perp_y - lon * dy
+                    x + lat * perp_x,
-                waypoints.append((ox, oy, altitude))
+                    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 處:
+        每個中間 waypoint 的處理：
-            1. 計算 pre_turn = WP - d_in × T
+            1. 計算入向 u_in、出向 u_out 的夾角 β = acos(u_in·u_out)
-            2. 計算 post_turn = WP + d_out × T
+            2. 若 cos β < sharp_turn_cos_threshold → 銳角：只插單點 (hover + 原地轉向)，
-            3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post
+               barrier 放在該點，讓隊伍整體停下再繼續
-            4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP)
+            3. 否則 → 平滑弧：
-
+                  R_base = formation_max_lateral + min_inner_radius
-        Args:
+                  d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
-            waypoints: 路徑航點 [(x, y), ...]
+                  R_actual = d / tan(β/2)
-            turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
+                  tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
-            curve_resolution: 每個彎道的 Bezier 插值點數
+                  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],
+        path.append({
-                      segments[0][0], segments[0][1]))
+            '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]
+            u_in, len_in = segments[i - 1]
-            d_out_x, d_out_y, len_out = segments[i]
+            u_out, len_out = segments[i]
-
+            wp = waypoints[i]
-            # 切入距離 T: 取相鄰兩段中較短的 × turn_margin
+
-            T = min(len_in, len_out) * turn_margin
+            dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
-
+            dot = max(-1.0, min(1.0, dot))
-            if T < 0.5:
+
-                # 段太短，不插彎，直接加一個平均方向點
+            # 銳角：hover + 原地轉向
-                avg_dx = d_in_x + d_out_x
+            if dot < sharp_turn_cos_threshold:
-                avg_dy = d_in_y + d_out_y
+                avg_dx = u_in[0] + u_out[0]
-                avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
+                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_dir = (avg_dx / avg_len, avg_dy / avg_len)
-                    avg_dy /= avg_len
                 else:
-                    avg_dx, avg_dy = d_in_x, d_in_y
+                    avg_dir = u_in
-                path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy))
+                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
+            beta = math.acos(dot)
-            p0_y = waypoints[i][1] - d_in_y * T
+            if beta < 1e-4:
-
+                # 幾乎直線，不插弧
-            # P1 = WP 本身 (控制點)
+                path.append({
-            p1_x = waypoints[i][0]
+                    'seg': 'straight',
-            p1_y = waypoints[i][1]
+                    'x': wp[0], 'y': wp[1],
-
+                    'dir': u_in,
-            # P2 = post_turn (弧線結束)
+                })
-            p2_x = waypoints[i][0] + d_out_x * T
+                continue
-            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]))
 
-        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,
+            'min_inner_radius': 3.0,
-            'curve_resolution': 8,
+            '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}
     )

src/GUI/validation/test_mission.py

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

src/GUI/validation/verify_waypoints.py

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

src/fc_network_apps/navigation.py

@@ -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,
+        )