Merge branch 'lunu' of http://140.120.108.238:49308/chiyu1468/AirTrapMine into lunu

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

.gitignore

@@ -26,3 +26,9 @@ Makefile
 **/*.pyc
 **/*.pyo
 **/.cursor/
+CLAUDE.md
+
+# 本地筆記與硬體 dump
+my_env_notes.md
+notes/
+**/eeprom.bin

README.md

@@ -97,8 +97,3 @@ python gui.py
 N. logs 是執行時期的記錄檔
 
 ===
-
-
-
-簡述0328
-主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。

serial_udp_update.py

@@ -0,0 +1,193 @@
+import asyncio
+import serial_asyncio
+import struct
+import os
+import sys
+import serial
+import signal
+import traceback
+from pymavlink import mavutil
+
+# === 設定區 ===
+SERIAL_PORT = 'COM15'  # 手動指定
+SERIAL_BAUDRATE = 57600
+UDP_REMOTE_IP = '127.0.0.1'
+UDP_REMOTE_PORT = 14550
+DEBUG_MODE = False
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
+
+# === 工具函數 ===
+def check_serial_port():
+    try:
+        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
+        ser.close()
+        return True
+    except serial.SerialException as e:
+        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
+        return False
+    except Exception as e:
+        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
+        return False
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+# === Serial Protocol 實作 ===
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol):
+        self.udp_protocol = udp_protocol
+        self.buffer = bytearray()
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_protocol, 'set_serial_transport'):
+            self.udp_protocol.set_serial_transport(self)
+        print(f"Serial connection established on {SERIAL_PORT}")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            pass
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[TX Error] 無法封裝或傳送資料: {e}")
+
+# === UDP Protocol 實作 ===
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self):
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print("UDP transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            pass
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[XBee 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            msg = self.mav_decoder.parse_char(data)
+            if msg:
+                if msg.get_type() == "HEARTBEAT":
+                    pass  # 不輸出任何訊息
+                else:
+                    pass
+        except Exception as e:
+            print(f"[MAVLink Decode Error] {e}")
+
+# === 主流程 ===
+async def main():
+    if not check_serial_port():
+        print("程式終止：串口檢查失敗")
+        return
+
+    loop = asyncio.get_running_loop()
+    if os.name != 'nt':  # Windows 不支援 add_signal_handler
+        for sig in (signal.SIGINT, signal.SIGTERM):
+            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
+
+    udp_handler = UDPHandler()
+
+    try:
+        udp_transport, _ = await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"無法創建 UDP 端點：{str(e)}")
+        return
+
+    sock = udp_transport.get_extra_info('socket')
+    print(f"UDP listening on {sock.getsockname()}")
+
+    try:
+        serial_proto = SerialToUDP(udp_handler)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"無法建立串口連接：{str(e)}")
+        traceback.print_exc()
+        udp_transport.close()
+        return
+
+    print("等待串口資料...")
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+async def shutdown(loop):
+    print("Shutting down...")
+    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
+    for task in tasks:
+        task.cancel()
+    await asyncio.gather(*tasks, return_exceptions=True)
+    loop.stop()
+
+if __name__ == '__main__':
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("程式被使用者中斷")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()

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
@@ -88,3 +87,79 @@ class MavlinkSender(CommandSender):
             self.mav.close()
             self.mav = None
             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

@@ -13,10 +13,11 @@ import sys
 import os
 import traceback
 from pymavlink import mavutil
-from geometry_msgs.msg import Point, Vector3
+from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
 from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
+from nav_msgs.msg import Odometry
 from mavros_msgs.srv import CommandBool, CommandTOL
 
 # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
@@ -36,6 +37,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)
 
@@ -467,14 +478,20 @@ class DroneMonitor(Node):
         self.serial_receivers = []
         
         # ================================================================================
-        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
         # ================================================================================
-        self.command_long_client = None
+        # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
-        try:
+        self.command_long_clients = {}  # {drone_id: CommandLongClient}
-            self.command_long_client = CommandLongClient()
+        self.client_lock = Lock()  # 保護 clients 字典的訪問
-        except Exception as e:
+        self.client_counter = 0  # 用於生成唯一的 client 節點名稱
-            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+        self.executor = None  # 將在 gui.py 中設置，用於添加新的 clients
-            self.command_long_client = None
+        # ================================================================================
+
+        # ================================================================================
+        # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto）
+        # ================================================================================
+        self.position_target_clients = {}  # {drone_id: PositionTargetGlobalIntClient}
+        self.pos_client_counter = 0
         # ================================================================================
         
         # 主题检测定时器
@@ -501,6 +518,58 @@ class DroneMonitor(Node):
             
             return self.socket_id_mapping[original_socket_id]
        
+    def get_or_create_client(self, drone_id):
+        """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
+        with self.client_lock:
+            if drone_id not in self.command_long_clients:
+                try:
+                    # 生成唯一的 client 節點名稱
+                    self.client_counter += 1
+                    unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
+                    client = CommandLongClient(node_name=unique_name)
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
+                    
+                    # 將新 client 添加到主執行器（這樣它的回調才能被處理）
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                    
+                except TypeError:
+                    # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
+                    client = CommandLongClient()
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
+                    
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                        
+                except Exception as e:
+                    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+)')
@@ -520,8 +589,25 @@ class DroneMonitor(Node):
                 # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
                 self.sys_to_socket_id[sys_id] = 0
             
-            if not hasattr(self, f'drone_{sys_id}_subs'):
+            subs_attr = f'drone_{sys_id}_subs'
+            if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
+            else:
+                # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
+                subs = getattr(self, subs_attr, {})
+                if isinstance(subs, dict) and 'position_ned' not in subs:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        pos_ned_sub = self.create_subscription(
+                            Odometry,
+                            f'{base_topic}/position_ned',
+                            lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                            10
+                        )
+                        subs['position_ned'] = pos_ned_sub
+                        setattr(self, subs_attr, subs)  # 明確保存更新後的字典
+                    except Exception as e:
+                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -568,6 +654,12 @@ class DroneMonitor(Node):
                 f'{base_topic}/vfr_hud',
                 lambda msg, sid=sys_id: self.hud_callback(sid, msg),
                 10
+            ),
+            'position_ned': self.create_subscription(
+                Odometry,
+                f'{base_topic}/position_ned',
+                lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                10
             )
         }
         
@@ -619,17 +711,19 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
             
-            if not self.command_long_client:
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
-                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.change_mode_async(
+            result = await client.change_mode_async(
                 target_sysid=sysid,
                 custom_mode=float(custom_mode),
                 target_compid=0,
                 base_mode=1.0,
-                timeout_sec=2.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
             )
             
             if result and result.success:
@@ -656,16 +750,18 @@ class DroneMonitor(Node):
             action_name = "解鎖" if arm else "上鎖"
             print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-            if not self.command_long_client:
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
-                print(f"❌ [ARM] CommandLongClient 未初始化")
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [ARM] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.arm_disarm_async(
+            result = await client.arm_disarm_async(
                 target_sysid=sysid,
                 arm=arm,
                 target_compid=0,
-                timeout_sec=2.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
             )
             
             if result and result.success:
@@ -691,16 +787,18 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
             
-            if not self.command_long_client:
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
-                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.takeoff_async(
+            result = await client.takeoff_async(
                 target_sysid=sysid,
                 altitude_m=float(altitude),
                 target_compid=0,
-                timeout_sec=2.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
             )
             
             if result and result.success:
@@ -805,6 +903,7 @@ class DroneMonitor(Node):
                 sys_num = sys_id.replace('sys', '')
                 actual_drone_id = f's{assigned_socket_id}_{sys_num}'
                 self.sys_to_actual_id[sys_id] = actual_drone_id
+                print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -885,6 +984,45 @@ class DroneMonitor(Node):
             'climb': msg.climb
         }
 
+    def position_ned_callback(self, sys_id, msg):
+        """處理 position_ned topic (nav_msgs/msg/Odometry)
+        NED 座標系中的位置（在 msg.pose.pose.position）：
+        - x: 北向位移 (m)
+        - y: 東向位移 (m)
+        - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
+        """
+        try:
+            # 使用映射獲取實際的 drone_id
+            actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+            # 如果還沒有從 summary 獲取到映射，則不處理
+            if actual_drone_id is None:
+                return
+            
+            # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
+            # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
+            x = msg.pose.pose.position.y  # NED 座標系中交換 x/y（與 local_pose 對齐）
+            y = msg.pose.pose.position.x
+            z = -msg.pose.pose.position.z  # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
+            
+            # 儲存高度信息
+            self.latest_data[(actual_drone_id, 'altitude')] = {
+                'altitude': z
+            }
+            
+            # 儲存本地位置信息
+            self.latest_data[(actual_drone_id, 'local_pose')] = {
+                'x': x,
+                'y': y,
+                'z': z
+            }
+            
+            # 發送信號給 GUI 更新高度顯示
+            self.signals.update_signal.emit('altitude', actual_drone_id, {
+                'altitude': z
+            })
+        except Exception as e:
+            pass
+
     def loss_rate_callback(self, drone_id, msg):
         self.latest_data[(drone_id, 'loss_rate')] = {
             'loss_rate': msg.data

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
@@ -33,7 +33,7 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.0.6'
+    VERSION = '2.0.8'
     
     def __init__(self):
         super().__init__()
@@ -52,9 +52,8 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
-        # 添加 CommandLongClient 到執行器（這樣它的回調才能被處理）
+        # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
-        if self.monitor.command_long_client:
+        self.monitor.executor = self.executor
-            self.executor.add_node(self.monitor.command_long_client)
         
         # 定时处理ROS事件
         self.timer = QTimer()
@@ -123,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)
         # ================================================================================
 
         # ================================================================================
@@ -450,11 +450,17 @@ class ControlStationUI(QMainWindow):
         loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}"))
 
     def handle_setpoint_selected(self):
+        """發送位置命令到 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
         try:
             x = float(self.x_input.text() or '0')
             y = float(self.y_input.text() or '0')
             z = float(self.z_input.text() or '0')
-            for drone_id in self.monitor.selected_drones:
+            for drone_id in group.selected_drone_ids:
                 if self.monitor.send_setpoint(drone_id, x, y, z):
                     self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000)
                 else:
@@ -507,8 +513,18 @@ class ControlStationUI(QMainWindow):
             self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
 
     def handle_arm_selected(self):
+        """解鎖 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
+        selected = list(group.selected_drone_ids)
+        if not selected:
+            self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+            return
+        
         print(f"\n📢 [GUI] handle_arm_selected 被調用")
-        selected = list(self.monitor.selected_drones)
         print(f"   已選擇的無人機: {selected}")
         loop = asyncio.get_event_loop()
         for drone_id in selected:
@@ -523,8 +539,19 @@ class ControlStationUI(QMainWindow):
         print(f"   handle_arm_selected 完成\n")
 
     def handle_takeoff_selected(self):
+        """起飛 active group 的所有選中無人機"""
+        group = self._get_active_group()
+        if not group:
+            self.statusBar().showMessage("⚠ 請先建立任務群組", 3000)
+            return
+        
+        selected = list(group.selected_drone_ids)
+        if not selected:
+            self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000)
+            return
+        
         loop = asyncio.get_event_loop()
-        for drone_id in self.monitor.selected_drones:
+        for drone_id in selected:
             future = self.monitor.takeoff_drone(drone_id, 10.0)
             loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}"))
 
@@ -584,76 +611,175 @@ class ControlStationUI(QMainWindow):
         scroll.setWidgetResizable(True)
         idx = self.group_tab_widget.addTab(scroll, f"Group {gid}")
         self.group_tab_widget.tabBar().setTabTextColor(idx, QColor(color))
+
+        # 切換到新 group 的 tab
         self.group_tab_widget.setCurrentIndex(idx)
-        self.active_group_id = gid
+
         self.statusBar().showMessage(f"已新增 Group {gid}", 2000)
 
+        # 更新刪除按鈕的啟用/禁用狀態
+        self._update_delete_buttons_state()
+
     def _on_group_tab_changed(self, index):
-        """Tab 切換時更新 active group 並同步地圖模式"""
+        """切換 group tab — 只需切換 active group 並刷新 UI"""
         if index < 0:
             self.active_group_id = None
             return
-        # tab 標題是 "Group X"
+
         tab_text = self.group_tab_widget.tabText(index)
         gid = tab_text.replace("Group ", "")
-        if gid in self.mission_groups:
+        if gid not in self.mission_groups:
+            return
+
         self.active_group_id = gid
         group = self.mission_groups[gid]
+
+        # 同步地圖的任務模式
         self.drone_map.set_mission_mode(group.mission_type)
 
+        # 統一刷新所有 UI
+        self.refresh_selection_ui()
+
     def _get_active_group(self):
         """取得當前 active 的 MissionGroup"""
         if self.active_group_id and self.active_group_id in self.mission_groups:
             return self.mission_groups[self.active_group_id]
         return None
 
-    def _get_other_assigned(self, exclude_gid):
+    def refresh_selection_ui(self):
-        """取得其他群組已佔用的無人機 {drone_id: group_id}"""
+        """統一刷新所有 UI 元素：左側 drone 勾選、左側 socket 勾選、右側 group panel"""
-        assigned = {}
+        group = self._get_active_group()
-        for gid, group in self.mission_groups.items():
+        selected = group.selected_drone_ids if group else set()
-            if gid == exclude_gid:
+        current_group_id = group.group_id if group else None
-                continue
+
-            for did in group.drone_ids:
+        # 左側 drone checkbox
-                assigned[did] = gid
+        for drone_id, panel in self.drones.items():
-        return assigned
+            checkbox = panel.get_checkbox()
+            if checkbox:
+                blocked = (
+                    current_group_id is not None and
+                    self._is_drone_selected_by_other_group(drone_id, current_group_id)
+                )
+                checkbox.blockSignals(True)
+                checkbox.setStyleSheet(self._get_drone_checkbox_style(blocked))
+                checkbox.setChecked(drone_id in selected)
+                checkbox.setEnabled(not blocked or drone_id in selected)
+                checkbox.blockSignals(False)
+
+        # 左側 socket checkbox
+        for socket_id in self.socket_groups.keys():
+            self.refresh_socket_checkbox(socket_id, selected, current_group_id)
+
+        # 右側 group panel
+        if group:
+            panel = self.group_panels.get(group.group_id)
+            if panel:
+                panel.update_drone_list()
+                panel.update_status()
+
+    def _get_drone_checkbox_style(self, blocked=False):
+        """回傳 drone checkbox 樣式；被其他 group 佔用時顯示紅色填滿。"""
+        blocked_style = """
+            QCheckBox::indicator:disabled {
+                background-color: #D32F2F;
+                border: 2px solid #A32020;
+            }
+        """ if blocked else ""
+        return f"""
+            QCheckBox {{
+                color: #DDD;
+            }}
+            QCheckBox::indicator {{
+                width: 16px;
+                height: 16px;
+                border: 2px solid #888888;
+                border-radius: 3px;
+                background: transparent;
+            }}
+            QCheckBox::indicator:checked {{
+                background-color: #7FFFD4;
+                border: 2px solid #888888;
+            }}
+            {blocked_style}
+        """
+
+    def refresh_socket_checkbox(self, socket_id, selected_ids, current_group_id=None):
+        """根據 selected_ids 推導並更新 socket 的勾選狀態"""
+        drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+        if not drone_ids:
+            return
+
+        socket_widget = self.socket_groups.get(socket_id)
+        if not socket_widget:
+            return
+
+        checkbox = socket_widget.findChild(QCheckBox, f"socket_{socket_id}_checkbox")
+        if not checkbox:
+            return
+
+        effective_drone_ids = [
+            did for did in drone_ids
+            if current_group_id is None
+            or did in selected_ids
+            or not self._is_drone_selected_by_other_group(did, current_group_id)
+        ]
+        checked_count = sum(1 for did in effective_drone_ids if did in selected_ids)
+
+        checkbox.blockSignals(True)
+        if checked_count == 0:
+            checkbox.setCheckState(Qt.CheckState.Unchecked)
+        elif checked_count == len(effective_drone_ids):
+            checkbox.setCheckState(Qt.CheckState.Checked)
+        else:
+            checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
+        checkbox.setEnabled(len(effective_drone_ids) > 0)
+        checkbox.blockSignals(False)
+
+
+    # 【已遷移至 refresh_socket_checkbox()】舊的 update_group_checkbox_state() 已廢棄
 
     def _handle_assign_drones(self, group_id):
-        """開啟無人機分配對話框（已勾選的 checkbox 會預先帶入）"""
+        """開啟無人機分配對話框 — 過濾被占用的drone"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
         all_ids = list(self.drones.keys())
-        other_assigned = self._get_other_assigned(group_id)
 
-        # 將目前 checkbox 已勾選的無人機（且未被其他群組佔用）合併進 pre-selected
+        # 預先選中：目前 group 的 selected_drone_ids
-        currently_checked = self.get_selected_drones()
+        pre_selected = set(group.selected_drone_ids)
-        pre_selected = set(group.drone_ids)
-        for did in currently_checked:
-            if did not in other_assigned:
-                pre_selected.add(did)
 
-        dialog = DroneAssignDialog(self, all_ids, pre_selected, other_assigned)
+        dialog = DroneAssignDialog(self, all_ids, pre_selected, {})
         if dialog.exec() == QDialog.DialogCode.Accepted:
-            group.drone_ids = dialog.get_selected()
+            requested = dialog.get_selected()
+
+            allowed = set()
+            blocked = set()
+
+            for did in requested:
+                if self._is_drone_selected_by_other_group(did, group_id):
+                    blocked.add(did)
+                else:
+                    allowed.add(did)
+
+            group.selected_drone_ids = allowed
+
+            # 只有當操作目標組是 active 組時，才更新 UI
+            if group_id == self.active_group_id:
+                self.refresh_selection_ui()
+
             panel = self.group_panels.get(group_id)
             if panel:
                 panel.update_drone_list()
                 panel.update_status()
 
-            # 同步更新左側面板的 checkbox 狀態
+            if blocked:
-            self.monitor.selected_drones = group.drone_ids.copy()
-            for drone_id in all_ids:
-                if drone_id in self.drones:
-                    checkbox = self.drones[drone_id].get_checkbox()
-                    if checkbox:
-                        checkbox.blockSignals(True)
-                        checkbox.setChecked(drone_id in group.drone_ids)
-                        checkbox.blockSignals(False)
-                # 更新 socket 群組的 checkbox 狀態
-                self.update_group_checkbox_state(self.get_socket_id(drone_id))
-            
                 self.statusBar().showMessage(
-                f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
+                    f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選，未加入", 4000
+                )
+            else:
+                self.statusBar().showMessage(
+                    f"Group {group_id}: 已分配 {len(group.selected_drone_ids)} 台無人機", 3000
+                )
 
     def _handle_mission_type_changed(self, group_id, mission_type):
         """群組任務類型切換"""
@@ -669,10 +795,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
@@ -733,19 +861,19 @@ class ControlStationUI(QMainWindow):
             print(f"❌ 找不到群組: {group_id}", flush=True)
             return
         
-        if not group.drone_ids:
+        if not group.selected_drone_ids:
             print(f"⚠️  群組中沒有無人機", flush=True)
             self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
             return
         
-        print(f"   準備為 {len(group.drone_ids)} 台無人機切換模式...", flush=True)
+        print(f"   準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True)
         self.statusBar().showMessage(f"正在切換模式...", 1000)
         
         # 使用 asyncio 執行（通過事件循環）
         async def do_mode_changes_async():
             print(f"\n   【異步任務】開始執行模式切換", flush=True)
             
-            for drone_id in group.drone_ids:
+            for drone_id in group.selected_drone_ids:
                 print(f"\n   【切換】{drone_id} → {mode}", flush=True)
                 try:
                     result = await self.monitor.set_mode(drone_id, mode)
@@ -784,11 +912,11 @@ class ControlStationUI(QMainWindow):
         if not group:
             print(f"   ⚠️ 群組不存在，返回\n")
             return
-        print(f"   群組內無人機: {group.drone_ids}")
+        print(f"   群組內無人機: {group.selected_drone_ids}")
         loop = asyncio.get_event_loop()
         print(f"   事件循環: {loop}")
         
-        for drone_id in group.drone_ids:
+        for drone_id in group.selected_drone_ids:
             print(f"\n   ┌─ 處理無人機: {drone_id}")
             print(f"   ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
             coro = self.monitor.arm_drone(drone_id, True)
@@ -812,7 +940,7 @@ class ControlStationUI(QMainWindow):
         if not group:
             return
         loop = asyncio.get_event_loop()
-        for drone_id in group.drone_ids:
+        for drone_id in group.selected_drone_ids:
             future = self.monitor.takeoff_drone(drone_id, altitude)
             loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)"))
 
@@ -824,107 +952,106 @@ class ControlStationUI(QMainWindow):
             f"請在地圖上框選要分配到 Group {group_id} 的無人機", 5000)
 
     def _handle_drone_box_selected(self, drone_ids_json):
-        """地圖框選完成 — 直接分配到指定群組"""
         group_id = self._pending_box_assign
         self._pending_box_assign = None
         if not group_id:
             return
+
         group = self.mission_groups.get(group_id)
         if not group:
             return
+
         drone_ids = json.loads(drone_ids_json)
-        other = self._get_other_assigned(group_id)
+
-        valid = {did for did in drone_ids if did not in other}
+        allowed = set()
-        group.drone_ids = valid
+        blocked = set()
+        for did in drone_ids:
+            if self._is_drone_selected_by_other_group(did, group_id):
+                blocked.add(did)
+            else:
+                allowed.add(did)
+
+        group.selected_drone_ids = allowed
+
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
+
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
 
-        # 同步更新左側面板的 checkbox 狀態
+        if blocked:
-        self.monitor.selected_drones = group.drone_ids.copy()
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(drone_id in group.drone_ids)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
             self.statusBar().showMessage(
-            f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
+                f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選，僅加入 {len(allowed)} 台", 4000
+            )
+        else:
+            self.statusBar().showMessage(
+                f"Group {group_id}: 框選分配 {len(allowed)} 台無人機", 3000
+            )
             
     def _handle_select_all_for_group(self, group_id):
-        """全選/取消全選 - Toggle 邏輯"""
+        """全選/取消全選 — Toggle 邏輯"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
 
-        other = self._get_other_assigned(group_id)
+        selectable_ids = self._get_selectable_drones_for_group(group_id) | set(group.selected_drone_ids)
-        available = {did for did in self.drones.keys() if did not in other}
 
         # Toggle 邏輯：如果已全選，則清空；否則全選
-        if group.drone_ids == available:
+        if group.selected_drone_ids == selectable_ids:
             # 已全選 → 清空
-            group.drone_ids = set()
+            group.selected_drone_ids.clear()
-            self.monitor.selected_drones.clear()
             msg_status = "已取消全選"
         else:
             # 未全選 → 全選
-            group.drone_ids = available
+            group.selected_drone_ids = set(selectable_ids)
-            self.monitor.selected_drones = group.drone_ids.copy()
+            msg_status = f"全選可用無人機 {len(selectable_ids)} 台"
-            msg_status = f"全選分配 {len(available)} 台無人機"
+
+        # 只有當操作目標組是 active 組時，才更新 UI
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
 
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
-            # 更新按鈕文本
-            panel.set_all_select_state(group.drone_ids == available)
-        
-        # 同步更新左側面板的 checkbox 狀態
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(drone_id in group.drone_ids)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
 
         self.statusBar().showMessage(
             f"Group {group_id}: {msg_status}", 3000)
 
     def _handle_clear_group(self, group_id):
-        """清除群組的無人機分配"""
+        """清除群組的無人機選擇"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
-        group.drone_ids = set()
+
+        group.selected_drone_ids.clear()
         group.planned_waypoints = None
         if group.executor:
             group.executor.stop()
         self.drone_map.clear_mission_plan_for_group(group_id)
+
+        # 只有當操作目標組是 active 組時，才更新 UI
+        if group_id == self.active_group_id:
+            self.refresh_selection_ui()
+
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
             panel.clear_mission_info()
 
-        # 同步更新左側面板的 checkbox 狀態（全部取消勾選）
-        self.monitor.selected_drones.clear()
-        for drone_id in self.drones.keys():
-            checkbox = self.drones[drone_id].get_checkbox()
-            if checkbox:
-                checkbox.blockSignals(True)
-                checkbox.setChecked(False)
-                checkbox.blockSignals(False)
-            self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
         self.statusBar().showMessage(
             f"Group {group_id}: 已清除分組", 3000)
 
     def _handle_delete_group(self, group_id):
         """刪除一個任務群組"""
+        # 檢查是否只有一個群組，如果是就禁止刪除
+        if len(self.mission_groups) <= 1:
+            self.statusBar().showMessage("⚠️ 至少需要保留一個群組！", 3000)
+            return
+        
         if group_id not in self.mission_groups:
             self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
             return
@@ -948,20 +1075,26 @@ class ControlStationUI(QMainWindow):
         if group_id in self.group_panels:
             del self.group_panels[group_id]
         
-        # 更新 active group
+        # 更新 active group — 讓 currentChanged signal 自動處理
         if self.active_group_id == group_id:
+            self.active_group_id = None
             if self.group_tab_widget.count() > 0:
                 self.group_tab_widget.setCurrentIndex(0)
-                # 更新 active_group_id 為當前 tab 的群組
+            # _on_group_tab_changed 會自動設定新的 active group
-                for gid, panel in self.group_panels.items():
-                    if panel == self.group_tab_widget.currentWidget().widget():
-                        self.active_group_id = gid
-                        break
-            else:
-                self.active_group_id = None
         
         self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
         
+        # 更新刪除按鈕的啟用/禁用狀態
+        self._update_delete_buttons_state()
+
+    def _update_delete_buttons_state(self):
+        """根據群組數量，更新所有群組的刪除按鈕啟用/禁用狀態"""
+        # 如果只有一個群組，禁用該群組的刪除按鈕
+        # 如果有多個群組，啟用所有刪除按鈕
+        should_enable = len(self.mission_groups) > 1
+        for gid, panel in self.group_panels.items():
+            panel.set_delete_enabled(should_enable)
+
     def _on_group_mission_completed(self, group_id):
         """群組任務完成回呼"""
         panel = self.group_panels.get(group_id)
@@ -1002,173 +1135,143 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_group_selection(self, socket_id, state):
-        """處理 socket 群組 checkbox 的勾選/取消勾選
+        """Socket 群組勾選 — 更新該 socket 下所有 drone 的選擇狀態"""
+        group = self._get_active_group()
+        if not group:
+            return
 
-        這個方法在用戶點擊 socket 群組的 checkbox 時被調用。
+        drone_ids = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        需要同時更新：
+        selectable = []
-        1. 該 socket 下所有無人機的 checkbox
+        blocked = []
-        2. self.monitor.selected_drones（用於控制面板同步）
+        for did in drone_ids:
-        3. 右侧活躍群組的無人機分配（新增）
+            if self._is_drone_selected_by_other_group(did, group.group_id):
+                blocked.append(did)
+            else:
+                selectable.append(did)
 
-        參數：
+        if not selectable:
-            socket_id: socket ID (str)
+            if blocked:
-            state: Qt.CheckState 的整數值 (0=Unchecked, 1=PartiallyChecked, 2=Checked)
+                self.statusBar().showMessage(
+                    f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選，無法操作", 4000
+                )
+            self.refresh_selection_ui()
+            return
+
+        all_selectable_selected = all(did in group.selected_drone_ids for did in selectable)
+
+        if all_selectable_selected:
+            group.selected_drone_ids.difference_update(selectable)
+        else:
+            group.selected_drone_ids.update(selectable)
+            if blocked:
+                self.statusBar().showMessage(
+                    f"⚠ 以下 drone 已被其他 group 勾選，略過: {', '.join(blocked)}", 4000
+                )
+
+        self.refresh_selection_ui()
+
+    def _is_drone_selected_by_other_group(self, drone_id, current_group_id):
         """
-        print(f"\n📢 [GUI] handle_group_selection 被調用", flush=True)
+        檢查該 drone 是否已被其他 group 勾選
-        print(f"   socket_id: {socket_id}, state: {state}", flush=True)
-        print(f"   state 類型: {type(state)}", flush=True)
         
-        # 獲取該 socket 下所有無人機
+        Args:
-        group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
+            drone_id: 無人機 ID
-        print(f"   該 socket 下的無人機: {group_drones}", flush=True)
+            current_group_id: 當前 group 的 ID
             
-        # 判斷是否勾選（只有 state == 2 時才是 Checked）
+        Returns:
-        is_checked = (state == 2)  # Qt.CheckState.Checked.value == 2
+            True 如果 drone 已被其他 group 選擇，否則 False
-        print(f"   is_checked: {is_checked}", flush=True)
+        """
+        for gid, group in self.mission_groups.items():
+            # 跳過當前 group
+            if gid == current_group_id:
+                continue
+            # 檢查該 drone 是否在其他 group 中被勾選
+            if drone_id in group.selected_drone_ids:
+                return True
+        return False
 
-        # 更新該 socket 下所有無人機的 checkbox 狀態
+    def _get_selectable_drones_for_group(self, group_id):
-        for drone_id in group_drones:
+        """
-            checkbox = self.drones[drone_id].get_checkbox()
+        取得指定 group 可以選擇的 drone 清單（不被其他 group 佔用的）
-            if checkbox:
-                print(f"   └─ 更新 {drone_id}: setChecked({is_checked})", flush=True)
-                checkbox.blockSignals(True)
-                checkbox.setChecked(is_checked)
-                checkbox.blockSignals(False)
         
-                # 同時更新 monitor.selected_drones 以同步控制面板
+        Args:
-                if is_checked:
+            group_id: group 的 ID
-                    self.monitor.selected_drones.add(drone_id)
-                else:
-                    self.monitor.selected_drones.discard(drone_id)
             
-        # 👇 新增：同步更新右侧活躍群組的無人機分配
+        Returns:
-        if self.active_group_id:
+            set: 可選擇的 drone ID 集合
-            group = self.mission_groups.get(self.active_group_id)
+        """
-            panel = self.group_panels.get(self.active_group_id)
+        selectable = set()
-            if group and panel:
+        for did in self.drones.keys():
-                print(f"   ├─ 同步右侧群組 {self.active_group_id}", flush=True)
+            if not self._is_drone_selected_by_other_group(did, group_id):
-                if is_checked:
+                selectable.add(did)
-                    # 勾選時：將該 socket 下的無人機添加到活躍群組
+        return selectable
-                    for drone_id in group_drones:
-                        group.drone_ids.add(drone_id)
-                    print(f"   │  └─ 添加到群組: {group_drones}", flush=True)
-                else:
-                    # 取消勾選時：從活躍群組移除該 socket 下的無人機
-                    for drone_id in group_drones:
-                        group.drone_ids.discard(drone_id)
-                    print(f"   │  └─ 從群組移除: {group_drones}", flush=True)
-                
-                # 更新右側群組面板的顯示
-                panel.update_drone_list()
-                panel.update_status()
-                print(f"   │  └─ 已更新右侧群組面板", flush=True)
-        
-        print(f"   最終 selected_drones: {self.monitor.selected_drones}", flush=True)
-        print(f"✓ handle_group_selection 完成\n", flush=True)
 
     def handle_drone_selection(self, drone_id, state):
-        is_checked = state == Qt.CheckState.Checked.value
+        """單台 drone 勾選 — 只修改 active group 的 selected_drone_ids"""
+        group = self._get_active_group()
+        if not group:
+            return
+
+        is_checked = (state == Qt.CheckState.Checked.value)
+
         if is_checked:
-            self.monitor.selected_drones.add(drone_id)
+            if self._is_drone_selected_by_other_group(drone_id, group.group_id):
-        else:
+                self.statusBar().showMessage(
-            self.monitor.selected_drones.discard(drone_id)
+                    f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
-        self.update_group_checkbox_state(self.get_socket_id(drone_id))
-        
-        # 同步更新任務群組的無人機分配狀態
-        # 遍歷所有任務群組，更新已分配的無人機列表顯示
-        if not is_checked:
-            # 取消勾選時：從所有包含該無人機的群組中移除
-            for group_id, group in self.mission_groups.items():
-                if drone_id in group.drone_ids:
-                    group.drone_ids.discard(drone_id)
-                    panel = self.group_panels.get(group_id)
-                    if panel:
-                        panel.update_drone_list()
-                        panel.update_status()
-                        # 更新全選按鈕狀態
-                        other = self._get_other_assigned(group_id)
-                        available = {did for did in self.drones.keys() if did not in other}
-                        panel.set_all_select_state(group.drone_ids == available)
-        else:
-            # 勾選時：檢查該無人機是否已分配給其他群組，若未分配則添加到當前活躍群組
-            is_already_assigned = any(
-                drone_id in group.drone_ids 
-                for group in self.mission_groups.values()
                 )
-            if not is_already_assigned and self.active_group_id:
+                self.refresh_selection_ui()
-                # 無人機未被分配給任何群組，可以添加到當前活躍群組
+                return
-                group = self.mission_groups.get(self.active_group_id)
+            group.selected_drone_ids.add(drone_id)
-                panel = self.group_panels.get(self.active_group_id)
+        else:
-                if group and panel:
+            group.selected_drone_ids.discard(drone_id)
-                    group.drone_ids.add(drone_id)
-                    panel.update_drone_list()
-                    panel.update_status()
-                    # 更新全選按鈕狀態
-                    other = self._get_other_assigned(self.active_group_id)
-                    available = {did for did in self.drones.keys() if did not in other}
-                    panel.set_all_select_state(group.drone_ids == available)
 
-    def update_group_checkbox_state(self, socket_id):
+        self.refresh_selection_ui()
-        group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        if not group_drones: return
-        checked_count = sum(1 for did in group_drones if self.drones[did].is_checked())
-        if socket_id in self.socket_groups:
-            group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
-            if group_checkbox:
-                group_checkbox.blockSignals(True)
-                if checked_count == 0: group_checkbox.setCheckState(Qt.CheckState.Unchecked)
-                elif checked_count == len(group_drones): group_checkbox.setCheckState(Qt.CheckState.Checked)
-                else: group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked)
-                group_checkbox.blockSignals(False)
-
-    def handle_select_all(self):
-        all_selected = all(self.drones[did].is_checked() for did in self.drones)
-        new_state = not all_selected
-        for drone_id in self.drones:
-            self.drones[drone_id].set_checked(new_state)
-        for socket_id in self.socket_groups:
-            group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox")
-            if group_checkbox:
-                group_checkbox.blockSignals(True)
-                group_checkbox.setChecked(new_state)
-                group_checkbox.blockSignals(False)
 
     def handle_drone_clicked(self, drone_id):
-        if drone_id in self.drones:
+        """地圖上點擊 drone — 切換其選擇狀態"""
-            checkbox = self.drones[drone_id].get_checkbox()
+        group = self._get_active_group()
-            checkbox.setChecked(not checkbox.isChecked())
+        if not group:
+            return
 
-    def handle_clear_all_drone_selection(self):
+        if drone_id in group.selected_drone_ids:
-        for drone_id in self.drones.keys():
+            group.selected_drone_ids.remove(drone_id)
-            checkbox = self.drones[drone_id].get_checkbox()
+        else:
-            if checkbox:
+            # 嘗試勾選前先檢查是否被其他 group 使用
-                checkbox.blockSignals(True)
+            if self._is_drone_selected_by_other_group(drone_id, group.group_id):
-                checkbox.setChecked(False)
+                self.statusBar().showMessage(
-                checkbox.blockSignals(False)
+                    f"⚠ {drone_id} 已被其他 group 勾選，無法加入 Group {group.group_id}", 3000
-        self.monitor.selected_drones.clear()
+                )
-        for socket_id in self.socket_groups.keys():
+                self.refresh_selection_ui()
-            self.update_group_checkbox_state(socket_id)
+                return
+            group.selected_drone_ids.add(drone_id)
+
+        self.refresh_selection_ui()
     
     def handle_toggle_select_all_drones(self):
-        all_selected = all(self.drones[did].get_checkbox().isChecked() for did in self.drones.keys())
+        """全選 / 清空 — 切換 active group 的所有 drone"""
-        if all_selected:
+        group = self._get_active_group()
-            for drone_id in self.drones.keys():
+        if not group:
-                checkbox = self.drones[drone_id].get_checkbox()
+            return
-                if checkbox:
+
-                    checkbox.blockSignals(True)
+        selectable_ids = self._get_selectable_drones_for_group(group.group_id) | set(group.selected_drone_ids)
-                    checkbox.setChecked(False)
+
-                    checkbox.blockSignals(False)
+        if group.selected_drone_ids == selectable_ids:
-            self.monitor.selected_drones.clear()
+            # 已全選 → 清空
+            group.selected_drone_ids.clear()
         else:
-            for drone_id in self.drones.keys():
+            # 未全選 → 全選
-                checkbox = self.drones[drone_id].get_checkbox()
+            group.selected_drone_ids = set(selectable_ids)
-                if checkbox:
+
-                    checkbox.blockSignals(True)
+        self.refresh_selection_ui()
-                    checkbox.setChecked(True)
+
-                    checkbox.blockSignals(False)
+    def handle_clear_all_drone_selection(self):
-                    self.monitor.selected_drones.add(drone_id)
+        """清除 active group 的所有無人機選擇"""
-        for socket_id in self.socket_groups.keys():
+        group = self._get_active_group()
-            self.update_group_checkbox_state(socket_id)
+        if not group:
+            return
+        group.selected_drone_ids.clear()
+        self.refresh_selection_ui()
+        self.statusBar().showMessage("已清除所有選擇", 2000)
 
     # ================================================================================
     # 任務規劃 — 點擊地圖（路由到 active group）
@@ -1176,7 +1279,7 @@ class ControlStationUI(QMainWindow):
 
     def _get_group_drones(self, group):
         """取得群組的無人機 ID 列表（排序後）"""
-        return sorted(group.drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
+        return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
 
     def handle_map_click(self, lat, lon):
         """處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
@@ -1211,11 +1314,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)
 
@@ -1282,12 +1389,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)
 
@@ -1331,6 +1442,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)
@@ -1359,12 +1475,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)
 
@@ -1403,6 +1523,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)
+
     # ================================================================================
     # 輔助方法
     # ================================================================================
@@ -1469,9 +1600,6 @@ class ControlStationUI(QMainWindow):
                 print(f"  WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)")
         print(f"\n{'=' * 60}")
     
-    def get_selected_drones(self):
-        return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()]
-
     def update_field(self, panel, drone_id, field, text, color=None):
         if isinstance(panel, DronePanel):
             panel.update_field(field, text, color)
@@ -1495,8 +1623,9 @@ class ControlStationUI(QMainWindow):
             self.socket_groups[socket_id] = self.create_socket_group_panel(socket_id)
         self.socket_groups[socket_id].drones_layout.addWidget(panel)
         self.reorganize_socket_groups()
-        self.update_group_checkbox_state(socket_id)
         self.update_overview_table()
+        # 同步新 drone 到 UI
+        self.refresh_selection_ui()
 
     def reorganize_socket_groups(self):
         while self.drone_panel_layout.count():
@@ -1723,10 +1852,16 @@ class ControlStationUI(QMainWindow):
             # Clean up serial receivers
             for receiver in self.monitor.serial_receivers:
                 receiver.stop()
-            # Clean up CommandLongClient
+            # Clean up all CommandLongClient instances
-            if self.monitor.command_long_client:
+            for drone_id, client in self.monitor.command_long_clients.items():
                 try:
-                    self.monitor.command_long_client.destroy_node()
+                    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()

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)
+    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,58 +128,121 @@ 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
+                    and task.wp_index < task.total_waypoints - 1):
+                # rendezvous 點 → 不推進，進入 barrier 等待
+                task.status = TaskStatus.WAITING_AT_BARRIER
+                task.waiting_since = now
+                print(f"  {task.drone_id} → 抵達 barrier WP {task.wp_index}，等待同伴")
+                self.task_status_changed.emit(
+                    task.drone_id, task.status.value,
+                    f"waiting at WP {task.wp_index}"
+                )
+                continue
+
+            # 一般點 → 直接推進
+            self._advance_waypoint(task, distance)
+
+        # ---- Phase 2: barrier 釋放檢查 ----
+        self._check_barriers(now)
+
+        # ---- Phase 3: 發送未送過的目標 ----
+        for task in self.tasks.values():
+            if task.done or task.status in (
+                TaskStatus.FALLBACK_LOITER, TaskStatus.WAITING_AT_BARRIER
+            ):
+                continue
+            if task.sent_current_wp:
+                continue
+            target = task.current_target
+            if target is None:
+                continue
+            tgt_lat, tgt_lon, tgt_alt = target
+            task.sent_current_wp = True
+            self.sender.send_position_global(
+                task.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(
@@ -161,27 +250,109 @@ class MissionExecutor(QObject):
             )
             print(f"  {task.drone_id} → DONE "
                   f"({task.total_waypoints}/{task.total_waypoints})")
-                    continue
+            return
-                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)")
+              f"(到達距離: {arrived_distance:.1f}m)")
-                    # 更新目標
-                    tgt_lat, tgt_lon, tgt_alt = task.current_target
 
-            # 發送 setpoint
+    def _check_barriers(self, now):
-            self.sender.send_position_global(
+        """檢查每個有人在等的 barrier 能不能釋放。"""
-                task.sysid, tgt_lat, tgt_lon, tgt_alt
+        # 蒐集目前有哪些 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)
 
-        # 全部完成檢查
+    # ------------------------------------------------------------------ 結果回呼
-        if all_done and self.tasks:
+
-            self._timer.stop()
+    def _on_send_result(self, drone_id, sysid, success, message):
-            self.state = MissionState.IDLE
+        """Ros2CommandSender.send_result 的 slot"""
-            self.mission_completed.emit()
+        task = self.tasks.get(drone_id)
-            print("===== 任務全部完成 =====")
+        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"))
 
 
 # ------------------------------------------------------------------ 工具函式

src/GUI/mission_group.py

@@ -30,11 +30,16 @@ class MissionGroup:
     def __init__(self, group_id, color):
         self.group_id = group_id             # 'A', 'B', 'C', ...
         self.color = color                   # 顏色 hex
-        self.drone_ids = set()            # 已分配的無人機 ID
+        
+        # 唯一真實資料來源：該 group 選中的 drone ID
+        # 代表：group 擁有的 drone、被選中的 drone、可以操作的 drone
+        self.selected_drone_ids = set()
+        
         self.mission_type = 'M_FORMATION'    # 預設任務類型
         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):
@@ -50,13 +55,13 @@ class MissionGroup:
 class DroneAssignDialog(QDialog):
     """無人機分配對話框"""
 
-    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned):
+    def __init__(self, parent, all_drone_ids, current_assigned, other_assigned=None):
         """
         Args:
             parent: 父 widget
             all_drone_ids: 所有可用無人機 ID 列表
             current_assigned: 當前群組已分配的無人機 set
-            other_assigned: 其他群組已佔用的無人機 dict {drone_id: group_id}
+            other_assigned: (忽略 — 現在允許多 group 分配)
         """
         super().__init__(parent)
         self.setWindowTitle("分配無人機")
@@ -66,7 +71,6 @@ class DroneAssignDialog(QDialog):
             QLabel { color: #DDD; }
             QCheckBox { color: #DDD; spacing: 8px; padding: 4px; }
             QCheckBox::indicator { width: 16px; height: 16px; }
-            QCheckBox:disabled { color: #666; }
         """)
 
         layout = QVBoxLayout(self)
@@ -89,12 +93,9 @@ class DroneAssignDialog(QDialog):
         for drone_id in sorted_ids:
             cb = QCheckBox(drone_id)
 
+            # 所有 drone 都能被選摘（支持多 group 分配）
             if drone_id in current_assigned:
                 cb.setChecked(True)
-            elif drone_id in other_assigned:
-                cb.setEnabled(False)
-                cb.setToolTip(f"已分配到 Group {other_assigned[drone_id]}")
-                cb.setText(f"{drone_id}  (Group {other_assigned[drone_id]})")
 
             self.checkboxes[drone_id] = cb
             scroll_layout.addWidget(cb)
@@ -159,7 +160,6 @@ class GroupPanel(QWidget):
     def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
-        self._is_all_selected = False  # 追蹤全選狀態
         self.all_btn_ref = None  # 保存全選按鈕的參考
         self._build_ui()
 
@@ -344,13 +344,13 @@ class GroupPanel(QWidget):
         clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
         clear_btn.clicked.connect(
             lambda: self.clear_group_requested.emit(self.group.group_id))
-        delete_group_btn = QPushButton("刪除群組")
+        self.delete_group_btn = QPushButton("刪除群組")
-        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
-        delete_group_btn.clicked.connect(
+        self.delete_group_btn.clicked.connect(
             lambda: self.delete_group_requested.emit(self.group.group_id))
         grid_r2.addWidget(assign_btn)
         grid_r2.addWidget(clear_btn)
-        grid_r2.addWidget(delete_group_btn)
+        grid_r2.addWidget(self.delete_group_btn)
         right.addLayout(grid_r2)
 
         right.addStretch()
@@ -415,6 +415,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()
 
         # 初始顯示對應的參數
@@ -438,15 +453,36 @@ class GroupPanel(QWidget):
 
     def update_drone_list(self):
         """更新無人機列表顯示"""
-        if not self.group.drone_ids:
+        if not self.group.selected_drone_ids:
             self.drone_list_label.setText("尚未分配")
             self.drone_list_label.setStyleSheet("color: #888; font-size: 11px;")
         else:
-            sorted_ids = sorted(self.group.drone_ids,
+            sorted_ids = sorted(self.group.selected_drone_ids,
                                 key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
             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.selected_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):
         """更新任務狀態顯示"""
@@ -461,7 +497,7 @@ class GroupPanel(QWidget):
             self.status_label.setText("⏸ 已暫停")
             self.status_label.setStyleSheet("color: #FFA000; font-size: 11px; font-weight: bold;")
         else:
-            n = len(self.group.drone_ids)
+            n = len(self.group.selected_drone_ids)
             total_wps = sum(len(wps) for wps in self.group.planned_waypoints['waypoints'])
             self.status_label.setText(f"● 已規劃 ({n}架/{total_wps}點)")
             self.status_label.setStyleSheet(
@@ -471,9 +507,9 @@ class GroupPanel(QWidget):
         """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
         self.select_all_requested.emit(self.group.group_id)
     
-    def set_all_select_state(self, is_selected):
+    def set_delete_enabled(self, enabled):
-        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
+        """啟用或禁用刪除群組按鈕"""
-        self._is_all_selected = is_selected
+        self.delete_group_btn.setEnabled(enabled)
 
     def _update_param_visibility(self, _=None):
         """根據當前任務類型，顯示/隱藏對應的參數列"""
@@ -481,6 +517,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 處理，避免倒退或衝出
 
-        隊形 (俯視):
+        解決前版「空間偏移」的三個 bug：
-            |     (前進方向)         |
+            1. 起點暴衝：s<0 clip → follower 起點就是 start 加橫向偏移，不往後倒退
-            |           D1          |  ← 左偏移, 後 0m
+            2. 弧段角度爆炸：lon 不再換算成弧度，直接沿 polyline lookup
-            |                D2     |  ← 右偏移, 後 5m
+            3. straight/arc 切換不連續：統一走 polyline 線性內插與 segment 切線
-            |           D3          |  ← 左偏移, 後 10m
+
-            |                D4     |  ← 右偏移, 後 15m
+        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 轉彎的中心路徑
+        # Step 1: 建立中心路徑（含圓弧、銳角單點；每點帶累積 s）
-        center_path = self._build_center_path(
+        center_path, barrier_indices = self._build_center_path(
-            route_wps_local, turn_margin, curve_resolution
+            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
 
-        projections = [
+        def lookup(s_target):
-            ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i)
+            """
-            for i, pos in enumerate(drone_positions)
+            在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。
-        ]
+            s<0 → clip 到 start；s>s_max → clip 到 end。
-        projections.sort()
+            tangent 方向取當前 polyline segment 的切線，避免對不連續的 dir 做插值。
+            """
+            if n_pts == 1:
+                pt = center_path[0]
+                return pt['x'], pt['y'], pt['dir']
 
-        # Step 3: 偏移
+            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,
+                ))
 
-            all_waypoints[original_idx] = waypoints
+            # 收尾：全員到 path 終點的 rank 位置（lon 歸零）
+            waypoints.append((
+                end['x'] + lat * (-end_dy),
+                end['y'] + lat * end_dx,
+                altitude,
+            ))
 
-        return all_waypoints
+            all_waypoints[rank] = waypoints
 
-    def _build_center_path(self, waypoints, turn_margin, curve_resolution):
+        # barrier 索引仍指向 center_path 內 (range [0, n_pts-1])，
+        # 不觸及末尾收尾點，直接沿用即可
+        return all_waypoints, barrier_indices
+
+    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
+                  d = R_base × tan(β/2)，並以相鄰段長的 45% 為上限
+                  R_actual = d / tan(β/2)
+                  tangent points T_in = WP - u_in·d, T_out = WP + u_out·d
+                  arc center = T_in + R_actual·sign·n_left，其中 sign=+1 左轉/CCW，-1 右轉/CW
+                  arc 由 theta_in 到 theta_out 以 arc_resolution 等分
+               path 內容: [T_in(straight,u_in), arc_samples..., T_out(straight,u_out)]
+               barrier 放在 T_out (轉完彎後的集合點)
 
-        Args:
+        path 是一個 list[dict]，每個 dict 至少含:
-            waypoints: 路徑航點 [(x, y), ...]
+            {'seg': 'straight' | 'arc',
-            turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5)
+             'x': float, 'y': float,
-            curve_resolution: 每個彎道的 Bezier 插值點數
+             '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
+            dot = u_in[0] * u_out[0] + u_in[1] * u_out[1]
-            T = min(len_in, len_out) * turn_margin
+            dot = max(-1.0, min(1.0, dot))
 
-            if T < 0.5:
+            # 銳角：hover + 原地轉向
-                # 段太短，不插彎，直接加一個平均方向點
+            if dot < sharp_turn_cos_threshold:
-                avg_dx = d_in_x + d_out_x
+                avg_dx = u_in[0] + u_out[0]
-                avg_dy = d_in_y + d_out_y
+                avg_dy = u_in[1] + u_out[1]
-                avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2)
+                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:
+                # 幾乎直線，不插弧
+                path.append({
+                    'seg': 'straight',
+                    'x': wp[0], 'y': wp[1],
+                    'dir': u_in,
+                })
+                continue
 
-            # P1 = WP 本身 (控制點)
+            half_tan = math.tan(beta / 2.0)
-            p1_x = waypoints[i][0]
+            d_ideal = R_base * half_tan
-            p1_y = waypoints[i][1]
+            d_max = 0.45 * min(len_in, len_out)
+            d = min(d_ideal, d_max)
+            R_actual = d / half_tan
 
-            # P2 = post_turn (弧線結束)
+            T_in = (wp[0] - u_in[0] * d, wp[1] - u_in[1] * d)
-            p2_x = waypoints[i][0] + d_out_x * T
+            T_out = (wp[0] + u_out[0] * d, wp[1] + u_out[1] * d)
-            p2_y = waypoints[i][1] + d_out_y * T
 
-            # 加入 pre_turn 點 (方向 = incoming)
+            # +1 CCW (左轉) / -1 CW (右轉)
-            path.append((p0_x, p0_y, d_in_x, d_in_y))
+            cross = u_in[0] * u_out[1] - u_in[1] * u_out[0]
+            sign = 1 if cross > 0 else -1
 
-            # 加入 Bezier 插值點
+            # 入向左法線
-            for j in range(1, curve_resolution):
+            n_left = (-u_in[1], u_in[0])
-                t = j / curve_resolution
+            center = (
+                T_in[0] + R_actual * sign * n_left[0],
+                T_in[1] + R_actual * sign * n_left[1],
+            )
 
-                # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2
+            theta_in = math.atan2(T_in[1] - center[1], T_in[0] - center[0])
-                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) → 切線方向
+            # T_in (straight, 方向 u_in)
-                tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x)
+            path.append({
-                tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y)
+                'seg': 'straight',
+                'x': T_in[0], 'y': T_in[1],
+                'dir': u_in,
+            })
 
-                # 正規化
+            # 弧段採樣 k=1..arc_resolution-1（不含兩端）
-                t_len = math.sqrt(tdx ** 2 + tdy ** 2)
+            for k in range(1, arc_resolution):
-                if t_len > 0.01:
+                t = k / arc_resolution
-                    tdx /= t_len
+                theta = theta_in + sign * beta * t
-                    tdy /= t_len
+                px = center[0] + R_actual * math.cos(theta)
-                else:
+                py = center[1] + R_actual * math.sin(theta)
-                    tdx, tdy = d_in_x, d_in_y
+                # 切線: 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,
+                })
 
-                path.append((bx, by, tdx, tdy))
+            # 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)
 
-            # 加入 post_turn 點 (方向 = outgoing)
+        # 終點
-            path.append((p2_x, p2_y, d_out_x, d_out_y))
+        path.append({
+            'seg': 'straight',
+            'x': waypoints[-1][0], 'y': waypoints[-1][1],
+            'dir': segments[-1][0],
+        })
 
-        # 最後一個航點
+        # 後處理：為每個 sample 加上累積 polyline 弧長 s
-        path.append((waypoints[-1][0], waypoints[-1][1],
+        path[0]['s'] = 0.0
-                      segments[-1][0], segments[-1][1]))
+        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
+        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/overview_table.py

@@ -7,7 +7,7 @@ class OverviewTable(QTableWidget):
     
     # 默認的資訊類型和映射
     DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
-                          "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
+                          "空速", "油門", "海拔高度", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {
         "mode": 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/longCommand.py

@@ -29,10 +29,14 @@ class CommandLongResult:
 class CommandLongClient(Node):
     """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 
-    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_command_long_client")
+        # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+        if node_name is None:
+            import time
+            node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
         self._client = self.create_client(MavCommandLong, service_name)
 
     def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
@@ -192,6 +196,83 @@ class CommandLongClient(Node):
     # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
     # ============================================================================
 
+    # ============================================================================
+    # 【重新實現】正確的非阻塞 async 方法（不使用 ThreadPoolExecutor）
+    # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_command_long_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        command: int,
+        confirmation: int,
+        param1: float,
+        param2: float,
+        param3: float,
+        param4: float,
+        param5: float,
+        param6: float,
+        param7: float,
+        timeout_sec: float,
+    ) -> CommandLongResult:
+        """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = command
+        req.confirmation = confirmation
+        req.param1 = param1
+        req.param2 = param2
+        req.param3 = param3
+        req.param4 = param4
+        req.param5 = param5
+        req.param6 = param6
+        req.param7 = param7
+        req.timeout_sec = float(timeout_sec)
+
+        # 發送異步調用
+        future = self._client.call_async(req)
+        
+        # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+        # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+        timeout = timeout_sec + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+        
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    response = future.result()
+                    if response is None:
+                        return CommandLongResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    return CommandLongResult(
+                        success=response.success,
+                        message=response.message,
+                        ack_result=response.ack_result,
+                    )
+                except Exception as e:
+                    return CommandLongResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+            
+            # 讓出控制權給事件循環，允許 GUI executor 執行
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+        
+        return CommandLongResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
+
     async def change_mode_async(
         self,
         *,
@@ -202,21 +283,21 @@ class CommandLongClient(Node):
         confirmation: int = 0,
         timeout_sec: float = DEFAULT_TIMEOUT_SEC,
     ) -> CommandLongResult:
-        """非阻塞 async 版本的 change_mode（在 ThreadPoolExecutor 中運行）"""
+        """非阻塞 async 版本的 change_mode"""
-        from concurrent.futures import ThreadPoolExecutor
+        return await self._send_command_long_async(
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.change_mode(
             target_sysid=target_sysid,
-                custom_mode=custom_mode,
             target_compid=target_compid,
-                base_mode=base_mode,
+            command=COMMAND_DO_SET_MODE,
             confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
             timeout_sec=timeout_sec,
         )
-        )
 
     async def arm_disarm_async(
         self,
@@ -228,21 +309,21 @@ class CommandLongClient(Node):
         param2: float = 0.0,
         timeout_sec: float = DEFAULT_TIMEOUT_SEC,
     ) -> CommandLongResult:
-        """非阻塞 async 版本的 arm_disarm（在 ThreadPoolExecutor 中運行）"""
+        """非阻塞 async 版本的 arm_disarm"""
-        from concurrent.futures import ThreadPoolExecutor
+        return await self._send_command_long_async(
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.arm_disarm(
             target_sysid=target_sysid,
-                arm=arm,
             target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
             confirmation=confirmation,
-                param2=param2,
+            param1=1.0 if arm else 0.0,
+            param2=float(param2),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
             timeout_sec=timeout_sec,
         )
-        )
 
     async def takeoff_async(
         self,
@@ -256,23 +337,21 @@ class CommandLongClient(Node):
         longitude: Optional[float] = None,
         timeout_sec: float = DEFAULT_TIMEOUT_SEC,
     ) -> CommandLongResult:
-        """非阻塞 async 版本的 takeoff（在 ThreadPoolExecutor 中運行）"""
+        """非阻塞 async 版本的 takeoff"""
-        from concurrent.futures import ThreadPoolExecutor
+        return await self._send_command_long_async(
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.takeoff(
             target_sysid=target_sysid,
-                altitude_m=altitude_m,
             target_compid=target_compid,
-                min_pitch_deg=min_pitch_deg,
+            command=COMMAND_NAV_TAKEOFF,
-                yaw_deg=yaw_deg,
+            confirmation=0,
-                latitude=latitude,
+            param1=float(min_pitch_deg),
-                longitude=longitude,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
             timeout_sec=timeout_sec,
         )
-        )
 
     async def land_async(
         self,
@@ -285,19 +364,18 @@ class CommandLongClient(Node):
         altitude_m: float = 0.0,
         timeout_sec: float = DEFAULT_TIMEOUT_SEC,
     ) -> CommandLongResult:
-        """非阻塞 async 版本的 land（在 ThreadPoolExecutor 中運行）"""
+        """非阻塞 async 版本的 land"""
-        from concurrent.futures import ThreadPoolExecutor
+        return await self._send_command_long_async(
-        loop = asyncio.get_event_loop()
-        executor = ThreadPoolExecutor(max_workers=1)
-        return await loop.run_in_executor(
-            executor,
-            lambda: self.land(
             target_sysid=target_sysid,
             target_compid=target_compid,
-                yaw_deg=yaw_deg,
+            command=COMMAND_NAV_LAND,
-                latitude=latitude,
+            confirmation=0,
-                longitude=longitude,
+            param1=0.0,
-                altitude_m=altitude_m,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
             timeout_sec=timeout_sec,
         )
-        )

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

src/unitdev04/test01.py

@@ -0,0 +1,229 @@
+import rclpy
+from pymavlink import mavutil
+from time import sleep
+import time
+
+import socket
+import struct
+
+# used at fdm_switch_example_two
+import threading
+import queue
+
+def mavlink_analyzer_simple(count = 500):
+    # rclpy.init()
+    # node = rclpy.create_node('mavlink_analyzer_simple')
+ 
+    print('Inintializing connection')
+
+    connection_string="udp:127.0.0.1:14550"
+    connection = mavutil.mavlink_connection(connection_string)
+
+    print('Catch messages')
+    msg_count = {}
+
+    start_time = time.time()
+
+    for _ in range(count):
+        msg = connection.recv_msg()
+        # msg = connection.recv_match(blocking=True)
+        if msg:
+            msg_type = msg.get_type()
+            if msg_type in msg_count:
+                msg_count[msg_type] += 1
+            else:
+                msg_count[msg_type] = 1
+
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                print(msg)
+                pass
+
+        else:
+            print('No message yet, 1 second for data to fill')
+            sleep(1)
+
+    print("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
+    print('Messages Type received:')
+    print(msg_count)
+
+    end_time = time.time()
+    print('Time elapsed: ', end_time - start_time)
+
+    print('Closing connection')
+    connection.close()
+
+
+def fdm_parser_example(data=None):
+    if data is None:
+        data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
+    # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
+
+
+    parse_format = 'HHI16H'
+
+    if len(data) != struct.calcsize(parse_format):
+        print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
+        exit(1)
+
+    decoded = struct.unpack(parse_format,data)
+
+    magic = decoded[0]
+    frame_rate_hz = decoded[1]
+    frame_count = decoded[2]
+    pwm = decoded[3:]
+
+
+    print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
+
+def fdm_switch_example_one():
+
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    沒有轉換數據格式
+    '''
+    # make a link get fdm from SITL, as a server
+    sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock_s2g.bind(('', 9002))
+    sock_s2g.settimeout(0.1)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    packet_count = 0
+    start_time = time.time()
+
+    while True:
+        try:
+            # 接到SITL的數據 並轉發給Gazebo
+            data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
+            sock_g2s.sendto(data_s2g, server_address)
+            # 得到Gazebo的回應 並轉發給SITL
+            data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
+            sock_s2g.sendto(data_g2s, addr_s2g)
+            packet_count += 1
+        except socket.timeout:
+            pass
+
+        current_time = time.time()
+        elapsed_time = current_time - start_time
+
+        if elapsed_time >= 1.0:
+            # print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack : {data_g2s}')
+            packet_count = 0
+            start_time = current_time
+            fdm_parser_example(data_s2g)
+
+
+def fdm_switch_example_two():
+    '''
+    這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
+    有轉換數據格式
+
+    time_usec
+    servo1_raw 
+    servo16_raw
+    '''
+    # read info from SITL via MAVLink
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection = mavutil.mavlink_connection(connection_string)
+
+    # set a target link pass JSON to Gazebo, as a client
+    sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    server_address = ('127.0.0.1', 19002)
+
+    data_queue = queue.Queue()
+    servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
+    data_queue.put(servo_out)
+
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
+        interval = 1.0 / frame_rate_fdm
+        while True:
+            start_time = time.time()
+            try:
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
+                if data is None:
+                    break
+                last_data = data
+            except queue.Empty:
+                data = last_data
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # debug
+
+            elapsed_time = time.time() - start_time
+            sleep_time = interval - elapsed_time
+            if sleep_time > 0:
+                sleep(sleep_time)
+
+    Running = True
+    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
+    thread.start()
+    print('Start sending data to Gazebo')
+
+    while Running:
+        msgb1 = None
+        msg = connection.recv_msg()
+        while msg :
+            if msg.get_type() == 'SERVO_OUTPUT_RAW':
+                msgb1 = msg
+                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
+            msg = connection.recv_msg()
+
+        if msgb1:
+            for i in range(16):
+                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
+                data_queue.put(servo_out)
+            msgb1 = None
+            # Running = False # debug
+        else:
+            # print('No message yet, 10 ms for data to fill')
+            sleep(0.01)
+
+        # Example of putting data into the queue
+        # data_queue.put(data)
+        # To stop the thread, you can put None into the queue
+        # data_queue.put(None)
+
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
+
+print('Start')
+# fdm_switch_example_two()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()

src/unitdev04/udptest4.py

@@ -0,0 +1,193 @@
+import asyncio
+import serial_asyncio
+import struct
+import os
+import sys
+import serial
+import signal
+import traceback
+from pymavlink import mavutil
+
+# === 設定區 ===
+SERIAL_PORT = 'COM15'  # 手動指定
+SERIAL_BAUDRATE = 57600
+UDP_REMOTE_IP = '127.0.0.1'
+UDP_REMOTE_PORT = 14550
+DEBUG_MODE = False
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'  # 廣播
+
+# === 工具函數 ===
+def check_serial_port():
+    try:
+        ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE)
+        ser.close()
+        return True
+    except serial.SerialException as e:
+        print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}")
+        return False
+    except Exception as e:
+        print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}")
+        return False
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+# === Serial Protocol 實作 ===
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol):
+        self.udp_protocol = udp_protocol
+        self.buffer = bytearray()
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_protocol, 'set_serial_transport'):
+            self.udp_protocol.set_serial_transport(self)
+        print(f"Serial connection established on {SERIAL_PORT}")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            pass
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[TX Error] 無法封裝或傳送資料: {e}")
+
+# === UDP Protocol 實作 ===
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self):
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print("UDP transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            pass
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[XBee 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            msg = self.mav_decoder.parse_char(data)
+            if msg:
+                if msg.get_type() == "HEARTBEAT":
+                    pass  # 不輸出任何訊息
+                else:
+                    pass
+        except Exception as e:
+            print(f"[MAVLink Decode Error] {e}")
+
+# === 主流程 ===
+async def main():
+    if not check_serial_port():
+        print("程式終止：串口檢查失敗")
+        return
+
+    loop = asyncio.get_running_loop()
+    if os.name != 'nt':  # Windows 不支援 add_signal_handler
+        for sig in (signal.SIGINT, signal.SIGTERM):
+            loop.add_signal_handler(sig, lambda: asyncio.create_task(shutdown(loop)))
+
+    udp_handler = UDPHandler()
+
+    try:
+        udp_transport, _ = await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"無法創建 UDP 端點：{str(e)}")
+        return
+
+    sock = udp_transport.get_extra_info('socket')
+    print(f"UDP listening on {sock.getsockname()}")
+
+    try:
+        serial_proto = SerialToUDP(udp_handler)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"無法建立串口連接：{str(e)}")
+        traceback.print_exc()
+        udp_transport.close()
+        return
+
+    print("等待串口資料...")
+    try:
+        await asyncio.Future()
+    except asyncio.CancelledError:
+        pass
+
+async def shutdown(loop):
+    print("Shutting down...")
+    tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()]
+    for task in tasks:
+        task.cancel()
+    await asyncio.gather(*tasks, return_exceptions=True)
+    loop.stop()
+
+if __name__ == '__main__':
+    try:
+        asyncio.run(main())
+    except KeyboardInterrupt:
+        print("程式被使用者中斷")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()

src/unitdev04/udptest8.py

@@ -0,0 +1,384 @@
+# === import 保持不變 ===
+import asyncio
+import serial_asyncio
+import struct
+import serial
+import traceback
+import time
+from collections import deque, defaultdict
+from pymavlink import mavutil
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import threading
+
+# === 多組設備設定 ===
+CONFIGS = [
+    {"serial_port": "/dev/ttyUSB2", "udp_port": 14551},
+    {"serial_port": "COM43", "udp_port": 14552},
+    {"serial_port": "COM15", "udp_port": 14553},
+]
+
+SERIAL_BAUDRATE = 115200
+UDP_REMOTE_IP = '127.0.0.1'
+TARGET_ADDR64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF'
+
+# === 數據存儲 ===
+rssi_history = defaultdict(lambda: deque(maxlen=5000))
+time_history = defaultdict(lambda: deque(maxlen=5000))
+
+# === 新增：丟包率追蹤 ===
+packet_loss_history = defaultdict(lambda: deque(maxlen=1000))
+packet_loss_time_history = defaultdict(lambda: deque(maxlen=1000))
+mavlink_sequence_tracker = defaultdict(dict)  # {sysid: {compid: {'last_seq': x, 'total_packets': y, 'lost_packets': z}}}
+packet_loss_stats = defaultdict(lambda: {'loss_rate': 0.0, 'total_received': 0, 'total_lost': 0})
+
+serial_to_sysid = {}
+serial_last_mavlink_time = {}  # 優化 1：追蹤各 serial port 最近的 MAVLink 時間
+
+def calculate_packet_loss(sysid, compid, current_seq):
+    """計算丟包率"""
+    global mavlink_sequence_tracker, packet_loss_stats
+    
+    tracker = mavlink_sequence_tracker[sysid]
+    if compid not in tracker:
+        # 第一次收到這個component的消息
+        tracker[compid] = {
+            'last_seq': current_seq,
+            'total_packets': 1,
+            'lost_packets': 0,
+            'last_update': time.time()
+        }
+        return 0.0
+    
+    comp_tracker = tracker[compid]
+    last_seq = comp_tracker['last_seq']
+    
+    # 計算序列號差異（處理255溢出）
+    if current_seq > last_seq:
+        expected_packets = current_seq - last_seq
+    elif current_seq < last_seq:
+        # 序列號溢出（0-255循環）
+        expected_packets = (255 - last_seq) + current_seq + 1
+    else:
+        # 重複的序列號，忽略
+        return comp_tracker.get('loss_rate', 0.0)
+    
+    # 更新統計
+    comp_tracker['total_packets'] += expected_packets
+    lost_packets = expected_packets - 1  # 實際收到1個，應該收到expected_packets個
+    comp_tracker['lost_packets'] += max(0, lost_packets)
+    comp_tracker['last_seq'] = current_seq
+    comp_tracker['last_update'] = time.time()
+    
+    # 計算丟包率
+    total_expected = comp_tracker['total_packets']
+    total_lost = comp_tracker['lost_packets']
+    loss_rate = (total_lost / total_expected) * 100.0 if total_expected > 0 else 0.0
+    comp_tracker['loss_rate'] = loss_rate
+    
+    # 更新全局統計（按sysid匯總所有component）
+    total_received = 0
+    total_lost_all = 0
+    total_expected_all = 0
+    
+    for comp_id, comp_data in tracker.items():
+        total_expected_all += comp_data['total_packets']
+        total_lost_all += comp_data['lost_packets']
+        total_received += comp_data['total_packets'] - comp_data['lost_packets']
+    
+    overall_loss_rate = (total_lost_all / total_expected_all) * 100.0 if total_expected_all > 0 else 0.0
+    
+    packet_loss_stats[sysid] = {
+        'loss_rate': overall_loss_rate,
+        'total_received': total_received,
+        'total_lost': total_lost_all
+    }
+    
+    # 記錄到歷史數據
+    now = time.time()
+    packet_loss_history[sysid].append(overall_loss_rate)
+    packet_loss_time_history[sysid].append(now)
+    
+    return overall_loss_rate
+
+def build_api_tx_frame(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes:
+    frame_type = 0x10
+    dest_addr16 = b'\xFF\xFE'
+    broadcast_radius = 0x00
+    options = 0x00
+    frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id)
+    frame += dest_addr64 + dest_addr16
+    frame += struct.pack(">BB", broadcast_radius, options) + data
+    checksum = 0xFF - (sum(frame) & 0xFF)
+    return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+
+class SerialToUDP(asyncio.Protocol):
+    def __init__(self, udp_protocol, serial_port):
+        self.udp_protocol = udp_protocol
+        self.serial_port = serial_port
+        self.buffer = bytearray()
+
+    def connection_made(self, transport):
+        self.transport = transport
+        if hasattr(self.udp_protocol, 'set_serial_transport'):
+            self.udp_protocol.set_serial_transport(self)
+        print(f"[{self.serial_port}] Serial connection established.")
+
+    def data_received(self, data):
+        self.buffer.extend(data)
+        while True:
+            if len(self.buffer) < 3:
+                return
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0)
+                continue
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
+            if len(self.buffer) < full_length:
+                return
+            frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+
+            if hasattr(self.udp_protocol, 'send_udp'):
+                self.udp_protocol.send_udp(bytes(frame))
+
+            if frame[3] == 0x88 and frame[5:7] == b'DB':
+                status = frame[7]
+                if status == 0x00 and len(frame) > 8:
+                    rssi_value = frame[8]
+                    now = time.time()
+
+                    # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
+                    last_time = serial_last_mavlink_time.get(self.serial_port, 0)
+                    if now - last_time <= 0.5:
+                        sysid = serial_to_sysid.get(self.serial_port, None)
+                        if sysid is not None:
+                            rssi_history[sysid].append(-rssi_value)
+                            time_history[sysid].append(now)
+                            # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+                        else:
+                            print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+                    else:
+                        print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+                else:
+                    print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
+
+    def write_to_serial(self, data):
+        try:
+            api_frame = build_api_tx_frame(data, TARGET_ADDR64)
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[{self.serial_port} TX Error] 無法封裝或傳送資料: {e}")
+
+    def send_at_command_db(self):
+        try:
+            frame_type = 0x08
+            frame_id = 0x52
+            at_command = b'DB'
+            parameter = b''
+            frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + at_command + parameter
+            checksum = 0xFF - (sum(frame) & 0xFF)
+            api_frame = b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum)
+            self.transport.write(api_frame)
+        except Exception as e:
+            print(f"[{self.serial_port}] 發送 DB 指令失敗: {e}")
+
+class UDPHandler(asyncio.DatagramProtocol):
+    def __init__(self, udp_port):
+        self.udp_port = udp_port
+        self.serial_transport = None
+        self.transport = None
+        self.mav_decoder = mavutil.mavlink.MAVLink(None)
+
+    def connection_made(self, transport):
+        self.transport = transport
+        print(f"[UDP:{self.udp_port}] transport ready.")
+
+    def set_serial_transport(self, serial_transport):
+        self.serial_transport = serial_transport
+
+    def datagram_received(self, data, addr):
+        if self.serial_transport:
+            self.serial_transport.write_to_serial(data)
+
+    def send_udp(self, data):
+        decoded_data = self.decapsulate_data(data)
+        if decoded_data is None:
+            return
+        self.decode_mavlink_data(decoded_data)
+        if self.transport:
+            self.transport.sendto(decoded_data, (UDP_REMOTE_IP, self.udp_port))
+
+    def decapsulate_data(self, data):
+        try:
+            if not data or data[0] != 0x7E:
+                return None
+            length = (data[1] << 8) | data[2]
+            if len(data) < length + 4:
+                return None
+            frame_type = data[3]
+            if frame_type == 0x90:
+                rf_data_start = 3 + 12
+                return data[rf_data_start:3 + length]
+            else:
+                return None
+        except Exception as e:
+            print(f"[UDP:{self.udp_port} 解封錯誤] {e}")
+            return None
+
+    def decode_mavlink_data(self, data):
+        try:
+            # 逐字節解析MAVLink
+            for byte in data:
+                msg = self.mav_decoder.parse_char(bytes([byte]))
+                if msg:
+                    sysid = msg.get_srcSystem()
+                    compid = msg.get_srcComponent()
+                    seq = msg.get_seq()
+                    
+                    if sysid == 0:
+                        continue
+                        
+                    if self.serial_transport:
+                        serial_to_sysid[self.serial_transport.serial_port] = sysid
+                        serial_last_mavlink_time[self.serial_transport.serial_port] = time.time()
+                        self.serial_transport.send_at_command_db()
+                    
+                    # === 新增：計算丟包率 ===
+                    try:
+                        loss_rate = calculate_packet_loss(sysid, compid, seq)
+                        # print(f"[SYSID:{sysid}] 丟包率: {loss_rate:.1f}%, SEQ: {seq}")
+                    except Exception as e:
+                        print(f"[UDP:{self.udp_port}] 丟包率計算錯誤: {e}")
+                        
+        except Exception as e:
+            print(f"[UDP:{self.udp_port} MAVLink Decode Error] {e}")
+
+def start_plotting():
+    # 創建子圖：上方RSSI，下方丟包率
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
+
+    def update(frame):
+        ax1.clear()
+        ax2.clear()
+        
+        # === RSSI 圖 ===
+        ax1.set_title("RSSI", fontsize=14)
+        ax1.set_xlabel("Time (s ago)")
+        ax1.set_ylabel("RSSI (dBm)")
+        ax1.set_xlim(30, 0)  # 顯示最近10秒
+        ax1.set_ylim(-100, -10)
+        ax1.grid(True, alpha=0.3)
+
+        # === 丟包率圖 ===
+        ax2.set_title("Packet Loss Rate", fontsize=14)
+        ax2.set_xlabel("Time (s ago)")
+        ax2.set_ylabel("Loss Rate (%)")
+        ax2.set_xlim(10, 0) 
+        ax2.set_ylim(0, 100)
+        ax2.grid(True, alpha=0.3)
+
+        now = time.time()
+        
+        # 顏色列表
+        colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown', 'pink', 'gray']
+        
+        for i, sysid in enumerate(sorted(rssi_history.keys())):
+            color = colors[i % len(colors)]
+            
+            # === 繪製 RSSI ===
+            rssi_recent_indices = [
+                idx for idx, ts in enumerate(time_history[sysid]) if now - ts <= 30
+            ]
+            if rssi_recent_indices:
+                rssi_t = [now - time_history[sysid][idx] for idx in rssi_recent_indices]
+                rssi_r = [rssi_history[sysid][idx] for idx in rssi_recent_indices]
+                if rssi_t and rssi_r:
+                    ax1.plot(rssi_t, rssi_r, label=f"SYSID:{sysid}", color=color, linewidth=2)
+                    
+                    # 在RSSI圖上顯示當前數值
+                    if rssi_r:
+                        latest_rssi = rssi_r[-1]
+                        ax1.text(2, latest_rssi, f'{latest_rssi:.0f}dBm', 
+                                bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                fontsize=10, fontweight='bold', color='white')
+
+            # === 繪製丟包率 ===
+            if sysid in packet_loss_history:
+                loss_recent_indices = [
+                    idx for idx, ts in enumerate(packet_loss_time_history[sysid]) if now - ts <= 10
+                ]
+                if loss_recent_indices:
+                    loss_t = [now - packet_loss_time_history[sysid][idx] for idx in loss_recent_indices]
+                    loss_r = [packet_loss_history[sysid][idx] for idx in loss_recent_indices]
+                    if loss_t and loss_r:
+                        ax2.plot(loss_t, loss_r, label=f"SYSID:{sysid}", color=color, linewidth=2, marker='o', markersize=3)
+                        
+                        # 在丟包率圖上顯示當前數值和統計
+                        if loss_r:
+                            latest_loss = loss_r[-1]
+                            stats = packet_loss_stats[sysid]
+                            
+                            # 顯示丟包率百分比
+                            ax2.text(2, latest_loss + (i * 8), f'{latest_loss:.1f}%', 
+                                    bbox=dict(boxstyle="round,pad=0.3", facecolor=color, alpha=0.7),
+                                    fontsize=10, fontweight='bold', color='white')
+                            
+                            # 在圖表左側顯示詳細統計
+                            stats_text = f"SYSID {sysid}:\nRecieved: {stats['total_received']}\nLoss: {stats['total_lost']}\nLoss Rate: {latest_loss:.1f}%"
+                            ax2.text(28, 85 - (i * 20), stats_text,
+                                    bbox=dict(boxstyle="round,pad=0.5", facecolor=color, alpha=0.8),
+                                    fontsize=9, color='white', verticalalignment='top')
+
+        # 設置圖例
+        ax1.legend(loc="upper right", fontsize=10)
+        ax2.legend(loc="upper right", fontsize=10)
+        
+    ani = animation.FuncAnimation(fig, update, interval=1000)
+    plt.tight_layout()
+    plt.show()
+
+async def setup_bridge(config):
+    serial_port = config["serial_port"]
+    udp_port = config["udp_port"]
+    try:
+        ser = serial.Serial(serial_port, SERIAL_BAUDRATE)
+        ser.close()
+    except Exception as e:
+        print(f"[{serial_port}] 串口打開失敗: {e}")
+        return
+    loop = asyncio.get_running_loop()
+    udp_handler = UDPHandler(udp_port)
+    try:
+        await loop.create_datagram_endpoint(
+            lambda: udp_handler,
+            local_addr=('0.0.0.0', 0)
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法創建 UDP: {e}")
+        return
+    try:
+        serial_proto = SerialToUDP(udp_handler, serial_port)
+        await serial_asyncio.create_serial_connection(
+            loop, lambda: serial_proto, serial_port, baudrate=SERIAL_BAUDRATE
+        )
+    except Exception as e:
+        print(f"[{serial_port}] 無法建立串口連接：{e}")
+        traceback.print_exc()
+        return
+    print(f"[{serial_port}] Serial <--> UDP:{udp_port} bridge ready.")
+
+async def main():
+    tasks = [setup_bridge(cfg) for cfg in CONFIGS]
+    await asyncio.gather(*tasks)
+    await asyncio.Future()
+
+if __name__ == '__main__':
+    try:
+        threading.Thread(target=lambda: asyncio.run(main()), daemon=True).start()
+        start_plotting()
+    except KeyboardInterrupt:
+        print("使用者中斷程式")
+    except Exception as e:
+        print("程式執行錯誤：")
+        traceback.print_exc()