Merge remote-tracking branch 'origin/master' into wenchun

README.md

@@ -1,43 +1,46 @@
 這是天雷系統的專案
 
-===
-專案核心框架
-1. ROS2 Humble
-2. Python 3.8.10
 
 
 ===
-必要相依套件 順便記錄我開發時的環境版本
+## 功能簡介
+1. mavlink 多對多支援平台
+2. 不允許進到 ros 系統有相同 sysid
+3. 假設一台載具上所有 component 共用同一 socket
 
-Python
-1. pymavlink -> Version: 2.4.42
-2. conda-forge 中的 pyserial-asyncio
-3. importlib_metadata -> Version: 8.5.0
-4. setuptools -> Version: 58.2.0 (版本太新不行)
-5. pyserial-asyncio
+===
+## 運行環境
 
+### 專案核心框架
+1. ROS2 Humble
+2. Python 3.8.10
 
-ROS2
+### 必要相依套件及版本
+- Python
+[Package] fc_network_adapter
+    1. pymavlink -> Version: 2.4.42
+    2. conda-forge 中的 pyserial-asyncio
+    3. importlib_metadata -> Version: 8.5.0
+    4. setuptools -> Version: 58.2.0 (版本太新不行)
+    5. pyserial-asyncio
+[Package] GUI
+    1. testresources
+    2. websockets
+    3. PyQt6
+    4. PyQt6-WebEngine
+
+- ROS2
 1. source ~/ros2_humble/install/setup.bash
-2.
-
-===
-功能簡介
-1. mavlink 多對多支援平台
-2. 不允許進到 ros 系統有相同 sysid
-3. 假設所有 component 共用同一 socket
+2. geographic_info (https://github.com/ros-geographic-info/geographic_info.git) 已經作為 submodule 放在 external
+3. angles (https://github.com/ros/angles.git) 已經作為 submodule 放在 external
+4. mavros_msgs (https://github.com/mavlink/mavros) 這個專案中的一個資料夾 這邊手動複製的
 
-===
-開發用輔助專案
+### 開發用輔助專案
 1. Gazebo Garden
 2. Ardupilot
 
 ===
-依賴的 ROS 庫
-1. https://github.com/ros-geographic-info/geographic_info.git 記得要搞 ros2 版本的
-2. https://github.com/ros/angles.git
-3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的
-
+## 使用說明
 Clone 專案後 請先執行這些指令
 ```bash
 # 1.同步 submodule
@@ -49,45 +52,53 @@ colcon build --packages-select angles geographic_msgs
 colcon build --packages-select mavros_msgs # 這個依賴前面的
 colcon build --packages-select fc_interfaces # 自己定義的
 
+```
+1. 
+主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 路徑下 以模組化啟動程式 
+```bash
+# 記得先開啟 依賴 Package 到 overlay
+. ./install/local_setup.bash
+# 範例
+cd ~/AirTrapMine/src/ # 這是範例!!!
+python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
+python -m fc_network_adapter.tests.demo_integration
+python -m someotherpkg.src.example_wholeMoving
 ```
 
-
-=== 
-Package 簡述
-<<<<<<< HEAD
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
 
 ===
-簡述0328
-主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。
-=======
+## 資料夾說明
 1. unitdev 為各自協作者做開發時的測試區
     01 -> 晉凱(ken)
     02 -> 其宇(chiyu)
     03 -> 文鈞
     04 -> 倫渝
-2. fc_network_adapter 
+2. fc_network_adapter (核心)
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
     --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-3. fc_interfaces
-    自定義的介面檔
+3. fc_interfaces (重要)
+    自定義的 ROS2 介面檔
 4. fc_network_apps
     與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
-
+    使用者的外層包裝
+5. someotherpkg
+    如何使用 fc_network_apps 的範例檔案
+6. GUI 
+    由 PyQt6 開發的互動式介面
 N. logs 是執行時期的記錄檔
 
 ===
-主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式
 
-例如 在 ~/AirTrapMine/src/ 資料夾下 
-```bash
-# 記得先開啟 依賴 Package 到 overlay
-. ./install/local_setup.bash
-# 範例
-python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
-python -m fc_network_adapter.tests.demo_integration
 
-```
 
->>>>>>> chiyu
+簡述0328
+主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。

src/GUI/communication.py

@@ -4,10 +4,14 @@ import math
 import re
 import threading
 from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
 import asyncio
 import websockets
 import json
 import socket
+import sys
+import os
+import traceback
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -15,6 +19,23 @@ from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL
 
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+    sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+    from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
+    traceback.print_exc()
+    CommandLongClient = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -398,8 +419,13 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
+    _instance = None  # Singleton pattern to prevent duplicate nodes
+    
     def __init__(self):
-        super().__init__('drone_monitor')
+        # Use a unique node name with timestamp to avoid conflicts on restart
+        import time
+        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+        super().__init__(node_name)
         self.signals = DroneSignals()
         self.drone_topics = {}
         self.lock = Lock()
@@ -440,6 +466,17 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # ================================================================================
+        self.command_long_client = None
+        try:
+            self.command_long_client = CommandLongClient()
+        except Exception as e:
+            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+            self.command_long_client = None
+        # ================================================================================
+        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
@@ -536,44 +573,151 @@ class DroneMonitor(Node):
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
-    async def arm_drone(self, drone_id, arm):
-        if drone_id not in self.arm_clients:
-            return False
-        
-        client = self.arm_clients[drone_id]
-        if not client.wait_for_service(timeout_sec=1.0):
-            return False
-            
-        request = CommandBool.Request()
-        request.value = arm
-        
-        future = client.call_async(request)
+    # ================================================================================
+    # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
+    # ================================================================================
+    MODE_MAPPING = {
+        "STABILIZE": 0,
+        "ACRO": 1,
+        "ALT_HOLD": 2,
+        "AUTO": 3,
+        "GUIDED": 4,
+        "LOITER": 5,
+        "RTL": 6,
+        "CIRCLE": 7,
+        "POSITION": 8,
+        "LAND": 9,
+        "OF_LOITER": 10,
+        "DRIFT": 11,
+        "SPORT": 13,
+        "FLIP": 14,
+        "AUTOTUNE": 15,
+        "POSHOLD": 16,
+        "BRAKE": 17,
+        "THROW": 18,
+        "AVOID_ADSB": 19,
+        "GUIDED_NOGPS": 20,
+        "SMART_RTL": 21,
+    }
+    # ================================================================================
+
+    async def set_mode(self, drone_id, mode_name):
+        """使用 CommandLongClient 切換無人機飛行模式"""
         try:
-            response = await future
-            return response.success
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            # 獲取模式對應的 custom_mode 值
+            custom_mode = self.MODE_MAPPING.get(mode_name)
+            if custom_mode is None:
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                return False
+            
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
+            
+            def _call_set_mode():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.change_mode(
+                    target_sysid=sysid,
+                    custom_mode=float(custom_mode),
+                    target_compid=0,
+                    base_mode=1.0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
+            
+            result = await loop.run_in_executor(executor, _call_set_mode)
+            if result:
+                print(f"✅ [SET_MODE] 模式切換成功")
+                return True
+            else:
+                print(f"❌ [SET_MODE] 模式切換失敗")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Arm service call failed: {e}')
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
-    async def takeoff_drone(self, drone_id, altitude=10.0):
-        if drone_id not in self.takeoff_clients:
-            return False
+    async def arm_drone(self, drone_id, arm):
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
             
-        client = self.takeoff_clients[drone_id]
-        if not client.wait_for_service(timeout_sec=1.0):
-            return False
+            action_name = "解鎖" if arm else "上鎖"
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-        request = CommandTOL.Request()
-        request.altitude = altitude
-        request.min_pitch = 0.0
-        request.yaw = 0.0
-        
-        future = client.call_async(request)
+            if not self.command_long_client:
+                print(f"❌ [ARM] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [ARM] {action_name}成功")
+                return True
+            else:
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
+                return False
+        except Exception as e:
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude):
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
         try:
-            response = await future
-            return response.success
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            
+            if not self.command_long_client:
+                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=2.0,
+            )
+            
+            if result and result.success:
+                print(f"✅ [TAKEOFF] 起飛成功")
+                return True
+            else:
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Takeoff service call failed: {e}')
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     def send_setpoint(self, drone_id, x, y, z):

src/GUI/gui.py

@@ -12,6 +12,7 @@ import asyncio
 import json
 import subprocess
 import time
+import traceback
 
 # 導入分離的類別
 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
@@ -32,15 +33,18 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '1.0.1'
+    VERSION = '2.0.4'
     
     def __init__(self):
         super().__init__()
         self.setWindowTitle(f'GCS v{self.VERSION}')
         self.resize(1400, 900)
 
-        # 初始化ROS2
-        rclpy.init()
+        # 初始化消息隊列（用於線程安全的 GUI 更新）
+        import queue
+        self.message_queue = queue.Queue()
+
+        # 初始化ROS2 Monitor（ROS2 本身在 main() 中已初始化）
         self.monitor = DroneMonitor()
         self.monitor.signals.update_signal.connect(self.update_ui)
         
@@ -48,16 +52,31 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
+        # 添加 CommandLongClient 到執行器（這樣它的回調才能被處理）
+        if self.monitor.command_long_client:
+            self.executor.add_node(self.monitor.command_long_client)
+        
         # 定时处理ROS事件
         self.timer = QTimer()
         self.timer.timeout.connect(self.spin_ros)
         self.timer.start(10)
         
+        # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
+        # 這允許異步任務（如 arm_drone）能夠執行
+        self.asyncio_timer = QTimer()
+        self.asyncio_timer.timeout.connect(self._spin_asyncio)
+        self.asyncio_timer.start(10)  # 每 10ms 驅動一次 asyncio
+        
         # 初始化 panel 和 map 更新（10Hz）
         self.panel_map_timer = QTimer()
         self.panel_map_timer.timeout.connect(self._update_panel_and_map)
         self.panel_map_timer.start(100)  # 10Hz
         
+        # 消息隊列處理定時器（來自線程的 GUI 更新）
+        self.msg_queue_timer = QTimer()
+        self.msg_queue_timer.timeout.connect(self._process_message_queue)
+        self.msg_queue_timer.start(50)  # 每 50ms 檢查一次
+        
         # 快取消息數據，以便在沒有新消息時使用上一次的值
         self._message_cache = {}
         
@@ -164,20 +183,29 @@ class ControlStationUI(QMainWindow):
         
         # ========== 任務群組 Tab ==========
         group_header = QHBoxLayout()
+        
+        # 標題 + 收起/展開按鈕
+        title_layout = QHBoxLayout()
         group_title = QLabel("任務群組")
         group_title.setStyleSheet(
             "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
-        group_header.addWidget(group_title)
-        group_header.addStretch()
-
-        add_group_btn = QPushButton("+ 新增群組")
-        add_group_btn.setStyleSheet("""
-            QPushButton { background-color: #4A9EFF; color: white; border: none;
-                padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
-            QPushButton:hover { background-color: #3A8EEF; }
+        title_layout.addWidget(group_title)
+        
+        # 收起/展開按鈕
+        self.toggle_group_btn = QPushButton("▼")
+        self.toggle_group_btn.setStyleSheet("""
+            QPushButton { background-color: #555; color: white; border: none;
+                padding: 3px 8px; border-radius: 3px; font-size: 12px; font-weight: bold;
+                min-width: 30px; max-width: 30px; }
+            QPushButton:hover { background-color: #666; }
         """)
-        add_group_btn.clicked.connect(self._add_mission_group)
-        group_header.addWidget(add_group_btn)
+        self.toggle_group_btn.setToolTip("收起/展開任務群組")
+        self.toggle_group_btn.clicked.connect(self._toggle_group_panel)
+        title_layout.addWidget(self.toggle_group_btn)
+        title_layout.addStretch()
+        
+        group_header.addLayout(title_layout)
+        group_header.addStretch()
 
         clear_traj_btn = QPushButton("清除軌跡")
         clear_traj_btn.setStyleSheet("""
@@ -204,6 +232,9 @@ class ControlStationUI(QMainWindow):
         self.group_tab_widget.setFixedHeight(150)
         self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
         right_layout.addWidget(self.group_tab_widget)
+        
+        # 🌟 新增：保存群組面板的展開狀態
+        self.group_panel_expanded = True
 
         # 預設建立 Group A
         self._add_mission_group()
@@ -353,6 +384,8 @@ class ControlStationUI(QMainWindow):
                 status_label.setToolTip("運行中")
                 self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
             except Exception as e:
+                print(f"❌ Serial 連接啟動失敗: {str(e)}")
+                traceback.print_exc()
                 self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
     
     def remove_serial_connection(self, conn, panel):
@@ -377,22 +410,39 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_mode_change(self, drone_id):
+        print(f"\n📢 [GUI] handle_mode_change 被调用")
+        print(f"   drone_id: {drone_id}")
+        
         # 從 active group 的 mode_combo 讀取模式
         group = self._get_active_group()
         if group:
             panel = self.group_panels.get(group.group_id)
             mode = panel.mode_combo.currentText() if panel else "GUIDED"
+            print(f"   从群组读取模式: {mode}")
         else:
             mode = "GUIDED"
+            print(f"   没有活跃群组，使用默认模式: {mode}")
+        
         loop = asyncio.get_event_loop()
         future = self.monitor.set_mode(drone_id, mode)
         loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
 
     def handle_arm(self, drone_id):
+        print(f"\n📢 [GUI] handle_arm 被調用")
+        print(f"   drone_id: {drone_id}")
         loop = asyncio.get_event_loop()
+        print(f"   獲得事件循環: {loop}")
         arm_state = not self.monitor.get_arm_state(drone_id)
-        future = self.monitor.arm_drone(drone_id, arm_state)
-        loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
+        print(f"   當前鎖定狀態: {not arm_state}, 目標狀態: {arm_state}")
+        print(f"   準備調用 arm_drone(drone_id={drone_id}, arm={arm_state})")
+        coro = self.monitor.arm_drone(drone_id, arm_state)
+        print(f"   arm_drone() 返回類型: {type(coro)}")
+        print(f"   coroutine 對象: {coro}")
+        action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
+        print(f"   準備提交到事件循環: {action_text}")
+        task = loop.create_task(self.handle_service_response(coro, action_text))
+        print(f"   task 已創建: {task}")
+        print(f"   已提交到事件循環\n")
 
     def handle_takeoff(self, drone_id):
         loop = asyncio.get_event_loop()
@@ -425,20 +475,52 @@ class ControlStationUI(QMainWindow):
             self.statusBar().showMessage("座標格式錯誤", 3000)
 
     async def handle_service_response(self, future, action):
+        print(f"\n📥 [GUI] handle_service_response 開始處理: {action}")
+        print(f"   Future/Coroutine 類型: {type(future)}")
+        print(f"   Future/Coroutine 對象: {future}")
         try:
+            print(f"   ├─ 等待 future/coroutine 完成...")
+            print(f"   └─ (這是一個阻塞等待，直到服務返回)\n")
             result = await future
+            print(f"\n   ✓ Future/Coroutine 完成，接收到返回值！")
+            print(f"   ├─ 返回值類型: {type(result)}")
+            print(f"   ├─ 返回值內容: {result}")
+            print(f"   ├─ 返回值 == True: {result == True}")
+            print(f"   ├─ 返回值 is True: {result is True}")
+            print(f"   └─ bool(返回值): {bool(result)}")
+            
+            # 詳細檢查 result 結構（用於調試 fc_network 回傳值）
+            if hasattr(result, '__dict__'):
+                print(f"   └─ 返回值屬性: {result.__dict__}")
+            
             if result:
+                print(f"\n✅ {action} 成功 (result={result})")
                 self.statusBar().showMessage(f"{action} 成功", 3000)
             else:
+                print(f"\n❌ {action} 失敗 (result={result})")
                 self.statusBar().showMessage(f"{action} 失敗", 3000)
+        except asyncio.CancelledError:
+            print(f"⚠️  {action} 被取消")
         except Exception as e:
+            print(f"❌ {action} 錯誤: {str(e)}")
+            traceback.print_exc()
             self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
 
     def handle_arm_selected(self):
+        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 self.monitor.selected_drones:
-            future = self.monitor.arm_drone(drone_id, True)
-            loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}"))
+        for drone_id in selected:
+            print(f"   準備批次解鎖: {drone_id}")
+            coro = self.monitor.arm_drone(drone_id, True)
+            print(f"   arm_drone 返回: {coro}")
+            # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
+            asyncio.run_coroutine_threadsafe(
+                self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
+                loop
+            )
+        print(f"   handle_arm_selected 完成\n")
 
     def handle_takeoff_selected(self):
         loop = asyncio.get_event_loop()
@@ -456,6 +538,23 @@ class ControlStationUI(QMainWindow):
         self._group_counter += 1
         return gid
 
+    def _toggle_group_panel(self):
+        """🌟 收起/展開任務群組面板"""
+        if self.group_panel_expanded:
+            # 收起
+            self.group_tab_widget.setFixedHeight(0)
+            self.group_tab_widget.hide()
+            self.toggle_group_btn.setText("▶")
+            self.toggle_group_btn.setToolTip("展開任務群組")
+            self.group_panel_expanded = False
+        else:
+            # 展開
+            self.group_tab_widget.setFixedHeight(150)
+            self.group_tab_widget.show()
+            self.toggle_group_btn.setText("▼")
+            self.toggle_group_btn.setToolTip("收起任務群組")
+            self.group_panel_expanded = True
+
     def _add_mission_group(self):
         """新增一個任務群組"""
         gid = self._next_group_id()
@@ -475,6 +574,8 @@ class ControlStationUI(QMainWindow):
         panel.box_select_requested.connect(self._handle_box_select)
         panel.select_all_requested.connect(self._handle_select_all_for_group)
         panel.clear_group_requested.connect(self._handle_clear_group)
+        panel.add_group_requested.connect(self._add_mission_group)
+        panel.delete_group_requested.connect(self._handle_delete_group)
         self.group_panels[gid] = panel
 
         # 用帶顏色的 tab 標題
@@ -538,6 +639,19 @@ class ControlStationUI(QMainWindow):
             if panel:
                 panel.update_drone_list()
                 panel.update_status()
+            
+            # 同步更新左側面板的 checkbox 狀態
+            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)
 
@@ -611,23 +725,86 @@ class ControlStationUI(QMainWindow):
 
     def _handle_group_mode_change(self, group_id, mode):
         """切換群組內所有無人機的飛行模式"""
+        print(f"\n📢 [GUI] _handle_group_mode_change 被调用", flush=True)
+        print(f"   group_id: {group_id}, mode: {mode}", flush=True)
+        
         group = self.mission_groups.get(group_id)
         if not group:
+            print(f"❌ 找不到群組: {group_id}", flush=True)
+            return
+        
+        if not group.drone_ids:
+            print(f"⚠️  群組中沒有無人機", flush=True)
+            self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
             return
+        
+        print(f"   準備為 {len(group.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:
+                print(f"\n   【切換】{drone_id} → {mode}", flush=True)
+                try:
+                    result = await self.monitor.set_mode(drone_id, mode)
+                    
+                    if result:
+                        msg = f"✅ {drone_id} 切換成功"
+                        print(f"   {msg}", flush=True)
+                        self.message_queue.put((msg, 2000))
+                    else:
+                        msg = f"❌ {drone_id} 切換失敗"
+                        print(f"   {msg}", flush=True)
+                        self.message_queue.put((msg, 2000))
+                
+                except Exception as e:
+                    msg = f"❌ {drone_id} 錯誤: {str(e)}"
+                    print(f"   {msg}", flush=True)
+                    traceback.print_exc()
+                    self.message_queue.put((msg, 2000))
+            
+            print(f"\n   【完成】模式切換任務結束\n", flush=True)
+        
+        # 通過事件循環提交異步任務
+        print(f"   【排隊】將任務提交至事件循環", flush=True)
         loop = asyncio.get_event_loop()
-        for drone_id in group.drone_ids:
-            future = self.monitor.set_mode(drone_id, mode)
-            loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}"))
+        asyncio.run_coroutine_threadsafe(
+            do_mode_changes_async(),
+            loop
+        )
 
     def _handle_group_arm(self, group_id):
         """解鎖群組內所有無人機"""
+        print(f"\n📢 [GUI] _handle_group_arm 被調用")
+        print(f"   群組 ID: {group_id}")
         group = self.mission_groups.get(group_id)
+        print(f"   群組存在: {group is not None}")
         if not group:
+            print(f"   ⚠️ 群組不存在，返回\n")
             return
+        print(f"   群組內無人機: {group.drone_ids}")
         loop = asyncio.get_event_loop()
+        print(f"   事件循環: {loop}")
+        
         for drone_id in group.drone_ids:
-            future = self.monitor.arm_drone(drone_id, True)
-            loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}"))
+            print(f"\n   ┌─ 處理無人機: {drone_id}")
+            print(f"   ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
+            coro = self.monitor.arm_drone(drone_id, True)
+            print(f"   ├─ arm_drone 返回類型: {type(coro)}")
+            action_text = f"解鎖 {drone_id}"
+            print(f"   ├─ 準備提交到事件循環: {action_text}")
+            # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
+            # 這是 Qt + asyncio 整合的正確方式
+            future = asyncio.run_coroutine_threadsafe(
+                self.handle_service_response(coro, action_text),
+                loop
+            )
+            print(f"   ├─ Future 已創建: {future}")
+            print(f"   └─ Future 將在事件循環中執行")
+        
+        print(f"\n   _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
 
     def _handle_group_takeoff(self, group_id, altitude):
         """群組內所有無人機起飛"""
@@ -663,23 +840,59 @@ class ControlStationUI(QMainWindow):
         if panel:
             panel.update_drone_list()
             panel.update_status()
+        
+        # 同步更新左側面板的 checkbox 狀態
+        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)
 
     def _handle_select_all_for_group(self, group_id):
-        """全選所有可用無人機，直接分配到該群組"""
+        """全選/取消全選 - Toggle 邏輯"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
+        
         other = self._get_other_assigned(group_id)
         available = {did for did in self.drones.keys() if did not in other}
-        group.drone_ids = available
+        
+        # Toggle 邏輯：如果已全選，則清空；否則全選
+        if group.drone_ids == available:
+            # 已全選 → 清空
+            group.drone_ids = set()
+            self.monitor.selected_drones.clear()
+            msg_status = "已取消全選"
+        else:
+            # 未全選 → 全選
+            group.drone_ids = available
+            self.monitor.selected_drones = group.drone_ids.copy()
+            msg_status = f"全選分配 {len(available)} 台無人機"
+        
         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}: 全選分配 {len(available)} 台無人機", 3000)
+            f"Group {group_id}: {msg_status}", 3000)
 
     def _handle_clear_group(self, group_id):
         """清除群組的無人機分配"""
@@ -696,9 +909,59 @@ class ControlStationUI(QMainWindow):
             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 group_id not in self.mission_groups:
+            self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
+            return
+        
+        # 停止群組的執行（如果有）
+        group = self.mission_groups[group_id]
+        if group.executor:
+            group.executor.stop()
+        
+        # 移除地圖上的任務計畫
+        self.drone_map.clear_mission_plan_for_group(group_id)
+        
+        # 移除 tab
+        for i in range(self.group_tab_widget.count()):
+            if f"Group {group_id}" in self.group_tab_widget.tabText(i):
+                self.group_tab_widget.removeTab(i)
+                break
+        
+        # 移除資料
+        del self.mission_groups[group_id]
+        if group_id in self.group_panels:
+            del self.group_panels[group_id]
+        
+        # 更新 active group
+        if self.active_group_id == group_id:
+            if self.group_tab_widget.count() > 0:
+                self.group_tab_widget.setCurrentIndex(0)
+                # 更新 active_group_id 為當前 tab 的群組
+                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)
+
     def _on_group_mission_completed(self, group_id):
         """群組任務完成回呼"""
         panel = self.group_panels.get(group_id)
@@ -739,23 +1002,111 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_group_selection(self, socket_id, state):
+        """處理 socket 群組 checkbox 的勾選/取消勾選
+        
+        這個方法在用戶點擊 socket 群組的 checkbox 時被調用。
+        需要同時更新：
+        1. 該 socket 下所有無人機的 checkbox
+        2. self.monitor.selected_drones（用於控制面板同步）
+        3. 右侧活躍群組的無人機分配（新增）
+        
+        參數：
+            socket_id: socket ID (str)
+            state: Qt.CheckState 的整數值 (0=Unchecked, 1=PartiallyChecked, 2=Checked)
+        """
+        print(f"\n📢 [GUI] handle_group_selection 被調用", flush=True)
+        print(f"   socket_id: {socket_id}, state: {state}", flush=True)
+        print(f"   state 類型: {type(state)}", flush=True)
+        
+        # 獲取該 socket 下所有無人機
         group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        is_checked = state == Qt.CheckState.Checked.value
+        print(f"   該 socket 下的無人機: {group_drones}", flush=True)
+        
+        # 判斷是否勾選（只有 state == 2 時才是 Checked）
+        is_checked = (state == 2)  # Qt.CheckState.Checked.value == 2
+        print(f"   is_checked: {is_checked}", flush=True)
+        
+        # 更新該 socket 下所有無人機的 checkbox 狀態
         for drone_id in group_drones:
             checkbox = self.drones[drone_id].get_checkbox()
             if checkbox:
+                print(f"   └─ 更新 {drone_id}: setChecked({is_checked})", flush=True)
                 checkbox.blockSignals(True)
                 checkbox.setChecked(is_checked)
                 checkbox.blockSignals(False)
-                if is_checked: self.monitor.selected_drones.add(drone_id)
-                else: self.monitor.selected_drones.discard(drone_id)
+                
+                # 同時更新 monitor.selected_drones 以同步控制面板
+                if is_checked:
+                    self.monitor.selected_drones.add(drone_id)
+                else:
+                    self.monitor.selected_drones.discard(drone_id)
+        
+        # 👇 新增：同步更新右侧活躍群組的無人機分配
+        if self.active_group_id:
+            group = self.mission_groups.get(self.active_group_id)
+            panel = self.group_panels.get(self.active_group_id)
+            if group and panel:
+                print(f"   ├─ 同步右侧群組 {self.active_group_id}", flush=True)
+                if is_checked:
+                    # 勾選時：將該 socket 下的無人機添加到活躍群組
+                    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):
-        if state == Qt.CheckState.Checked.value:
+        is_checked = state == Qt.CheckState.Checked.value
+        if is_checked:
             self.monitor.selected_drones.add(drone_id)
         else:
             self.monitor.selected_drones.discard(drone_id)
         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:
+                # 無人機未被分配給任何群組，可以添加到當前活躍群組
+                group = self.mission_groups.get(self.active_group_id)
+                panel = self.group_panels.get(self.active_group_id)
+                if group and panel:
+                    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):
         group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
@@ -890,7 +1241,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -962,7 +1312,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -1042,7 +1391,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -1180,7 +1528,6 @@ class ControlStationUI(QMainWindow):
         self._map_update_count += 1
         now = time.time()
         if now - self._map_update_time >= 1.0:
-            print(f"[Panel/Map Update] {self._map_update_count} Hz")
             self._map_update_time = now
             self._map_update_count = 0
         
@@ -1314,32 +1661,135 @@ class ControlStationUI(QMainWindow):
 
 
 
+    def _process_message_queue(self):
+        """處理來自後台線程的消息隊列（更新 GUI 狀態欄）"""
+        try:
+            while True:
+                try:
+                    message, duration = self.message_queue.get_nowait()
+                    self.statusBar().showMessage(message, duration)
+                except:
+                    break
+        except Exception as e:
+            print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
+            traceback.print_exc()
+
+    def _spin_asyncio(self):
+        """驅動 asyncio 事件循環，允許異步任務執行
+        
+        關鍵修復：asyncio 事件循環不會自動運行。
+        這個定時器會定期執行事件循環，讓 run_coroutine_threadsafe() 提交的任務能夠執行。
+        """
+        try:
+            loop = asyncio.get_event_loop()
+            if loop and not loop.is_closed() and not loop.is_running():
+                # 執行事件循環直到沒有掛起的任務或超時
+                # 使用 run_until_complete() 配合一個快速返回的 coroutine
+                loop.run_until_complete(asyncio.sleep(0))
+        except Exception as e:
+            # 靜默忽略任何錯誤，防止 Qt 定時器出現異常
+            # 但仍然打印詳細的堆棧跟踪以便調試
+            traceback.print_exc()
+
     def spin_ros(self):
         try:
-            self.executor.spin_once(timeout_sec=0.01)
-            for (drone_id, msg_type), data in self.monitor.latest_data.items():
-                self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
-            self.monitor.latest_data.clear()
+            # 仅在 ROS2 正常工作时才尝试 spin
+            if rclpy.ok():
+                self.executor.spin_once(timeout_sec=0.01)
+                for (drone_id, msg_type), data in self.monitor.latest_data.items():
+                    self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
+                self.monitor.latest_data.clear()
+        except RuntimeError as e:
+            # ROS2 context 被破坏或不可用
+            if "Context" in str(e) or "context" in str(e).lower():
+                print(f"⚠️ ROS2 context 錯誤（忽略）: {e}", flush=True)
+            else:
+                print(f"ROS spin error: {e}", flush=True)
+                traceback.print_exc()
         except Exception as e:
-            print(f"ROS spin error: {e}")
+            print(f"ROS spin error: {e}", flush=True)
+            traceback.print_exc()
 
     def closeEvent(self, event):
-        for group in self.mission_groups.values():
-            if group.executor:
-                group.executor.stop()
-        self.command_sender.close()
-        for receiver in self.udp_receivers:
-            receiver.stop()
-        for receiver in self.monitor.ws_receivers:
-            receiver.stop()
-        self.monitor.destroy_node()
-        self.executor.shutdown()
-        rclpy.shutdown()
+        try:
+            for group in self.mission_groups.values():
+                if group.executor:
+                    group.executor.stop()
+            self.command_sender.close()
+            for receiver in self.udp_receivers:
+                receiver.stop()
+            for receiver in self.monitor.ws_receivers:
+                receiver.stop()
+            # Clean up serial receivers
+            for receiver in self.monitor.serial_receivers:
+                receiver.stop()
+            # Clean up CommandLongClient
+            if self.monitor.command_long_client:
+                try:
+                    self.monitor.command_long_client.destroy_node()
+                except:
+                    pass
+            self.monitor.destroy_node()
+            self.executor.shutdown()
+        except Exception as e:
+            print(f"⚠️ 清理資源時出錯: {e}", flush=True)
+            traceback.print_exc()
+        
+        # 安全地 shutdown ROS2
+        try:
+            if rclpy.ok():
+                rclpy.shutdown()
+        except RuntimeError as e:
+            print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
+            traceback.print_exc()
+        
         event.accept()
 
 
+def main():
+    """
+    GUI 應用程式的主入口點
+    
+    標準 ROS2 + Qt 架構：
+    1. 在最外層/最前面只做一次 rclpy.init()
+    2. 啟動 Qt 應用程式
+    3. 在 finally 中做 rclpy.shutdown()
+    
+    這樣可以確保所有 ROS2 相關操作都共享同一個初始化狀態
+    """
+    # 第一步：在最外層只初始化一次 ROS2（終極防護）
+    # 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
+    print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True)
+    if not rclpy.ok():
+        print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True)
+        rclpy.init()
+        print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True)
+    else:
+        print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True)
+    
+    try:
+        # 第二步：啟動 Qt 應用程式
+        print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True)
+        app = QApplication(sys.argv)
+        station = ControlStationUI()
+        station.show()
+        print("✓ [GUI 主程式] 應用程式已啟動", flush=True)
+        
+        # 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
+        print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True)
+        sys.exit(app.exec())
+        
+    finally:
+        # 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
+        # 這裡確保 ROS2 被正確且安全地關閉
+        print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True)
+        if rclpy.ok():
+            rclpy.shutdown()
+            print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True)
+        else:
+            print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True)
+        print("✓ [GUI 主程式] 應用程式完全退出", flush=True)
+
+
 if __name__ == '__main__':
-    app = QApplication(sys.argv)
-    station = ControlStationUI()
-    station.show()
-    app.exec()
+    main()

src/GUI/mission_group.py

@@ -146,6 +146,8 @@ class GroupPanel(QWidget):
     box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
     select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
     clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
+    add_group_requested = pyqtSignal()                 # 新增群組
+    delete_group_requested = pyqtSignal(str)           # group_id — 刪除群組
 
     BUTTON_STYLE = """
         QPushButton {{ background-color: {bg}; color: {fg}; border: none;
@@ -157,6 +159,8 @@ 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()
 
     def _make_sep(self):
@@ -296,7 +300,7 @@ class GroupPanel(QWidget):
         mid.addLayout(mid_body)
 
         # ============================
-        # 選取與分組（2x2 按鈕）
+        # 選取與分組（3x2 按鈕）
         # ============================
         right = QVBoxLayout()
         right.setSpacing(3)
@@ -310,9 +314,28 @@ class GroupPanel(QWidget):
         self.drone_list_label.setWordWrap(True)
         right.addWidget(self.drone_list_label)
 
-        # 2x2 按鈕網格
+        # 3x2 按鈕網格：第一行 框選 全選 新增群組
         grid_r1 = QHBoxLayout()
         grid_r1.setSpacing(3)
+        box_btn = QPushButton("框選")
+        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        box_btn.clicked.connect(
+            lambda: self.box_select_requested.emit(self.group.group_id))
+        all_btn = QPushButton("全選/取消")
+        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        all_btn.clicked.connect(self._on_all_select_clicked)
+        self.all_btn_ref = all_btn  # 保存按鈕參考（備用）
+        add_group_btn = QPushButton("+ 新增群組")
+        add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+        add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+        grid_r1.addWidget(box_btn)
+        grid_r1.addWidget(all_btn)
+        grid_r1.addWidget(add_group_btn)
+        right.addLayout(grid_r1)
+
+        # 第二行 編輯分配 清除分組 刪除群組
+        grid_r2 = QHBoxLayout()
+        grid_r2.setSpacing(3)
         assign_btn = QPushButton("編輯分配")
         assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
         assign_btn.clicked.connect(
@@ -321,22 +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))
-        grid_r1.addWidget(assign_btn)
-        grid_r1.addWidget(clear_btn)
-        right.addLayout(grid_r1)
-
-        grid_r2 = QHBoxLayout()
-        grid_r2.setSpacing(3)
-        box_btn = QPushButton("框選")
-        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
-        box_btn.clicked.connect(
-            lambda: self.box_select_requested.emit(self.group.group_id))
-        all_btn = QPushButton("全選")
-        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
-        all_btn.clicked.connect(
-            lambda: self.select_all_requested.emit(self.group.group_id))
-        grid_r2.addWidget(box_btn)
-        grid_r2.addWidget(all_btn)
+        delete_group_btn = QPushButton("刪除群組")
+        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        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)
         right.addLayout(grid_r2)
 
         right.addStretch()
@@ -410,13 +424,15 @@ class GroupPanel(QWidget):
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
         # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
-        cols.addLayout(left, 1)
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(mid, 1)
+        cols.addLayout(mid, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(param_col, 1)
+        cols.addLayout(param_col, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(right, 1)
+        cols.addLayout(right, 0)
+        cols.addStretch()  # 填充剩餘空間，使四列置左
 
         layout.addLayout(cols)
 
@@ -451,6 +467,14 @@ class GroupPanel(QWidget):
             self.status_label.setStyleSheet(
                 f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
 
+    def _on_all_select_clicked(self):
+        """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+        self.select_all_requested.emit(self.group.group_id)
+    
+    def set_all_select_state(self, is_selected):
+        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
+        self._is_all_selected = is_selected
+
     def _update_param_visibility(self, _=None):
         """根據當前任務類型，顯示/隱藏對應的參數列"""
         mission_type = self.type_combo.currentText()

src/GUI/overview_table.py

@@ -6,7 +6,7 @@ class OverviewTable(QTableWidget):
     """總覽表格，顯示所有無人機的狀態資訊"""
     
     # 默認的資訊類型和映射
-    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向",
+    DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "XY位置", "XY速度", "地速", "航向",
                           "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"]
     
     DEFAULT_INFO_TYPE_MAP = {

src/fc_interfaces/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
+  "srv/MavPositionTargetGlobalInt.srv"
   )
 
 ament_package()

src/fc_interfaces/srv/MavPositionTargetGlobalInt.srv

@@ -0,0 +1,36 @@
+# Request
+uint8  target_sysid
+uint8  target_compid
+
+uint32 time_boot_ms        # ms since system boot, 一般用 0 讓飛控自己填
+uint8  coordinate_frame    # MAV_FRAME_*
+uint16 type_mask           # POSITION_TARGET_TYPEMASK bitmask
+int32  lat_int             # 1e7 * latitude (deg)
+int32  lon_int             # 1e7 * longitude (deg)
+float32 alt                # altitude (m)
+float32 vx                 # X velocity (m/s)
+float32 vy                 # Y velocity (m/s)
+float32 vz                 # Z velocity (m/s)
+float32 afx                # X acceleration or force (m/s^2)
+float32 afy                # Y acceleration or force (m/s^2)
+float32 afz                # Z acceleration or force (m/s^2)
+float32 yaw                # yaw (rad)
+float32 yaw_rate           # yaw rate (rad/s)
+float32 timeout_sec        # 等待 POSITION_TARGET_GLOBAL_INT 回應的時間上限
+---
+# Response
+string message
+uint8  ack_result  
+uint32 r_time_boot_ms  
+uint16 r_type_mask           
+int32  r_lat_int            
+int32  r_lon_int            
+float32 r_alt                
+float32 r_vx 
+float32 r_vy 
+float32 r_vz 
+float32 r_afx          
+float32 r_afy      
+float32 r_afz              
+float32 r_yaw        
+float32 r_yaw_rate     

src/fc_network_adapter/fc_network_adapter/ackEnum.py

@@ -0,0 +1,31 @@
+
+'''
+單獨出來定義回傳碼的意義
+若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
+若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
+所以要避開重疊的號碼
+
+這邊備註 Mavlink 的 MAV_RESULT 參數意義
+0 : 封包正常接受並執行
+1 : 封包正常接受 但是正在忙不執行 需要等待後重試
+4 : 封包正常接受 但是載具的狀態異常 不執行
+
+2 : 封包不正常 參數可能給錯了
+3 : 封包不正常 根本認不出是啥
+
+'''
+
+
+from enum import IntEnum
+
+class serviceAckResult(IntEnum):
+    MAVLINK_TIMEOUT = 51 # mavlink communication timeout
+
+    MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair 
+
+    MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
+    MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
+
+    UNKNOWN = 90 # i did not capture this error, try read the logs
+
+

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -6,6 +6,7 @@
 # 開發此專案的注意事項
 - 預設 autopilot 的 component id = 1
 - 不允許 system id 重複
+- 預設同一載具僅會使用相同的 socket
 - 增加一個固定數值監控然後要到 ros2 topic 
   - mavlinkROS2Node.py 檔案內 
     - PublishRateController.topic_intervals 建立
@@ -126,6 +127,7 @@
 - *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) }
 - *self.mavlink_socket* 從 pymavlink 繼承的socket物件
 - *self.state* 描述這個 socket 物件的狀態
+- *self.MAVLink* pymavlink 的一個裝置物件 模擬自己是某一個裝置 用以對於該 mavlink bus 負責封包發送 
 ---
 - *process_data()* [async method] 核心方法
 - *remove_target_socket()* *add_target_socket()*
@@ -170,6 +172,45 @@
 ## mavlinkVehicleView.py
     這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用
 
+## mavlinkROS2Nodes.py
+    這裡支持了所有 ROS2 框架的對應功能 包涵 
+    1. 將載具的串流更新的狀態更新到對應的 topic 
+    2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
+
+### **[Class]** fc_ros_manager
+    MAVLink ROS2 節點管理器 管理兩個獨立的 Node
+    會開一個執行緒 讓兩個 Node 都跑在裡面
+    然後用 spin_once 保持 Node 的活性
+- *self.status_publisher* 物件實例
+- *self.command_service* 物件實例
+- *self.spin_thread* 執行緒
+---
+- *start()* 啟動自己
+- *stop()* 停下自己
+- *shutdown()* 關閉自己並清理
+
+### **[Class]** VehicleStatusPublisher
+    這整個組件都是自動的 目前沒有什麼需要 runtime 設定的地方
+- *self.fc_publishers* 儲存所有 publisher 的資料結構
+- *self.rate_controller* 儲存頻率參數的地方 也可以關掉某個 topic 的發佈
+---
+- *timer_callback()* 自動的抓出現有的 vehicle 中有的資訊 然後固定的將訊息丟到負責發佈的方法中
+- *_publish_vehicle_status()* 接 timer_callback 後去分配到各項發佈中
+- *_get_or_create_publisher()* 實際創建 topic 的地方
+- *_publish_XXX()* 各項發佈的方法
+
+### **[Class]** MavlinkCommandService
+    提供 ros2 service 讓 serial_object 去丟出 mavlink 封包
+    然後會從回應封包中挑出期待的回應 再傳給 ros2 request 
+    其中 PendingEntry 是很關鍵的東西
+    它的每個物件 代表需要等待的封包種類與必需包含的內容
+- *self._pending_by_sysid* 儲存 PendingEntry 的地方
+---
+- *return_router()* 檢查並消耗 return_packet_ring 然後將封包分配到 pending 去
+- *__init__()* 這邊登入要創建的 service 到 ros2 系統
+- *handle_XXX()* 這邊是實現 service 的具體方法
+
+
 # 開發記錄
 
 ## 已實現功能
@@ -182,9 +223,9 @@
 7. 終端機介面控制
 8. 基礎載具流量觀測
 9. 載具狀態收集與彙整
-10. a. ros2 topic 應用開發介面
+10. a. ros2 topic 應用開發介面 (基礎框架)
+    b. ros2 service 應用開發介面 (基礎框架)
 
 ### 待開發功能
 5-1. 建立 serial 連線 並可以對接收器下達AT指令
-5-2. 模組化 serial 連線機制 以利後期擴容其他模組
-10. a. ros2  應用開發介面
+5-2. 模組化 serial 連線機制 以利後期擴容其他模組

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
 from .utils.acquirePort import find_available_port
 
 logger = setup_logger(os.path.basename(__file__))
-VERSION_NO = "v0.60"
+PROJECT_VER = "v0.61"
 
 class PanelState:
     def __init__(self):
@@ -39,9 +39,9 @@ class PanelState:
         self.object_manager_state = "Stopped"
         self.serial_manager_state = "Stopped"
         self.ros2_manager_state = "Stopped"
-        self.socket_object_list = [] # 已有的 mavlink object 
+        self.socket_object_list = [] # 已有的 mavlink object
         self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string
-        self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 
+        self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息
 
         # 關於創建通道時的暫存資訊
         self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
@@ -102,36 +102,36 @@ class ControlPanel:
     def input_dialog(stdscr, prompt="請輸入文字: "):
         """顯示輸入對話框"""
         height, width = stdscr.getmaxyx()
-        
+
         # 建立輸入視窗
         dialog_height = 5
         dialog_width = min(60, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         # 建立視窗邊框
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(1, 2, prompt)
         dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消")
         dialog_win.refresh()
-        
+
         # 輸入區域
         input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2)
         input_win.keypad(True)
-        
+
         curses.echo()
         curses.curs_set(1)
-        
+
         user_input = ""
-        
+
         while True:
             input_win.clear()
             input_win.addstr(0, 0, user_input[-dialog_width+8:])  # 顯示輸入內容（滾動）
             input_win.refresh()
-            
+
             ch = input_win.getch()
-            
+
             if ch == 27:  # ESC
                 user_input = None
                 break
@@ -141,16 +141,16 @@ class ControlPanel:
                 user_input = user_input[:-1]
             elif 32 <= ch <= 126:  # 可打印字符
                 user_input += chr(ch)
-        
+
         curses.noecho()
         curses.curs_set(0)
-        
+
         # 清理視窗
         del input_win
         del dialog_win
         stdscr.clear()
         stdscr.refresh()
-        
+
         return user_input
 
     # ================ 關於 主要選單 的部份 ===================
@@ -179,9 +179,9 @@ class ControlPanel:
                 ]),
                 MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
                 MenuNode("Engineer Mode", "工程模式", children=[
-                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), 
-                    MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), 
-                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), 
+                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"),
+                    MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"),
+                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"),
                 ]),
                 MenuNode("Shutdown", "關閉整個系統", children=[
                     MenuNode("Return", "繼續運行", "BACK"),
@@ -191,7 +191,7 @@ class ControlPanel:
 
     def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
         stdscr = None
-    
+
         def cleanup():
             """清理 curses 狀態"""
             if stdscr:
@@ -209,7 +209,7 @@ class ControlPanel:
         def draw_menu(screen):
             nonlocal stdscr
             stdscr = screen
-            
+
             curses.curs_set(0)
             stdscr.nodelay(False)  # 阻塞讀鍵
             stdscr.keypad(True)
@@ -219,12 +219,12 @@ class ControlPanel:
             idx_stack = [0]  # 索引堆疊
 
             state.intoSTART() # 設定狀態為運行中
-            
+
             while not stop_evt.is_set():
 
                 current_menu = menu_stack[-1]
                 current_idx = idx_stack[-1]
-                
+
                 # 獲取終端機尺寸
                 height, width = stdscr.getmaxyx()
                 # 簡單暴力的限制視窗的大小
@@ -237,7 +237,7 @@ class ControlPanel:
                 if height < MIN_HEIGHT or width < 60:
                     logger.error("Terminal size too small for Control Panel.")
                     break
-                
+
                 stdscr.clear()
                 stdscr.border()
 
@@ -267,26 +267,26 @@ class ControlPanel:
                     elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
                         link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
                         desc = f"{child.desc} [{link_status}]"
-                    
+
                     line = f"{marker}{child.name:15s} – {desc}"
                     attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
                     stdscr.addstr(start_line + i, 4, line, attr)
 
-                # 顯示訊息區域 
+                # 顯示訊息區域
                 # info_start_line = start_line + len(current_menu.children) + 1
                 info_start_line = height - 8
                 max_msg_lines = 5  # 最多顯示 5 行訊息
                 current_time = time.time()
-                
+
                     # 清理過時的訊息
                 state.panel_info_msg_list = [
                     (msg, timestamp)  for msg, timestamp in state.panel_info_msg_list
                     if current_time - timestamp < 2.0 #秒數
                 ]
-                
+
                     # 只顯示最新的 max_msg_lines 條訊息
                 display_msgs = state.panel_info_msg_list[-max_msg_lines:]
-                
+
                 for i, msg_data in enumerate(display_msgs):
                     if info_start_line + i >= help_line - 1:  # 避免超出邊界
                         break
@@ -298,13 +298,13 @@ class ControlPanel:
 
                     stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)
 
-                
-                
+
+
                 # 操作說明
                 # help_line = start_line + len(current_menu.children) + 2
                 help_line = height - 2
                 stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
-                stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM)
+                stdscr.addstr(height-1 , width-12, f" {PROJECT_VER} ", curses.A_DIM)
 
                 stdscr.refresh()
 
@@ -319,8 +319,8 @@ class ControlPanel:
                         # continue
                         break
                     time.sleep(0.1)
-                    if (state.mavlink_bridge_state == "Stopped" and 
-                        state.object_manager_state == "Stopped" and 
+                    if (state.mavlink_bridge_state == "Stopped" and
+                        state.object_manager_state == "Stopped" and
                         state.serial_manager_state == "Stopped"):
                         state.intoSTOPPED()
                         # stop_evt.set()
@@ -330,14 +330,14 @@ class ControlPanel:
                 # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
                 stdscr.timeout(100)
                 ch = stdscr.getch()
-                
+
                 if ch == -1: # 沒有操作
                     continue
-                    
+
                 # 處理按鍵
                 if ch in (curses.KEY_UP, ord('k')):
                     idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
-                    
+
                 elif ch in (curses.KEY_DOWN, ord('j')):
                     idx_stack[-1] = (current_idx + 1) % len(current_menu.children)
 
@@ -347,14 +347,14 @@ class ControlPanel:
 
                 elif ch == (ord('o')):
                     # 離開工程模式
-                    state.intoSTART()                
-                    
+                    state.intoSTART()
+
                 elif ch == curses.KEY_LEFT:
                     # 返回上層
                     if len(menu_stack) > 1:
                         menu_stack.pop()
                         idx_stack.pop()
-                
+
                 elif ch == curses.KEY_RIGHT:
                     # 進入下層 (但不執行動作)
                     selected = current_menu.children[current_idx]
@@ -369,26 +369,26 @@ class ControlPanel:
 
                 elif ch in (curses.KEY_ENTER, 10, 13):
                     selected = current_menu.children[current_idx]
-                    
+
                     # 處理不同類型的動作
                     if selected.children:  # 有子選單
                         menu_stack.append(selected)
                         idx_stack.append(0)
-                        
+
                     elif selected.action == "BACK":
                         if len(menu_stack) > 1:
                             menu_stack.pop()
                             idx_stack.pop()
-                            
+
                     elif selected.action == "QUIT":
                         state.intoTERMINATION()
                         pre_panel_shutdown()
-                    
+
                     elif selected.action == "TEXT_UDP_IP":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
                         if result is not None:
                             state.udp_info_temp["IP"] = result
-                    
+
                     elif selected.action == "TEXT_UDP_PORT":
                         result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
                         if result is not None:
@@ -402,7 +402,7 @@ class ControlPanel:
                             idx_stack.pop()
                             # menu_stack.pop()
                             # idx_stack.pop()
-                    
+
                     elif selected.action == "CREATE_UDP_OUTBOUND":
                         cmd_q.put("CREATE_UDP_OUTBOUND")
                         # 確認後回到上兩層
@@ -457,8 +457,8 @@ class ControlPanel:
                     elif selected.action == "LIST_SERIAL_LINKS":
                         created_list_menu = self.create_linked_serial_menu(state, page=0)
                         menu_stack.append(created_list_menu)
-                        idx_stack.append(0)                        
-                    
+                        idx_stack.append(0)
+
                     elif selected.action == "INSPECT_LINKED_SERIAL":
                         # 顯示 Serial 連結詳細資訊
                         if hasattr(selected, 'serial_id'):
@@ -488,7 +488,7 @@ class ControlPanel:
                             current_list_menu = menu_stack[-1]
                             menu_stack.pop()
                             idx_stack.pop()
-                            
+
                             # 依據選單種類 重新建立分頁
                             if current_list_menu.name == "Serial Port List":
                                 created_list_menu = self.create_serial_port_menu(state, page=selected.page)
@@ -503,16 +503,16 @@ class ControlPanel:
                                 menu_stack.append(current_list_menu)
                                 idx_stack.append(0)
                                 continue
-                            
+
                             menu_stack.append(created_list_menu)
                             idx_stack.append(0)
-                    
+
                     elif selected.action == "INSPECT_MAV_OBJECT":
                         # 顯示物件詳細資訊
                         if hasattr(selected, 'socket_id'):
                             cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
                             self.show_object_info(stdscr, selected.socket_id, state)
-                    
+
                     elif selected.action == "REMOVE_MAV_OBJECT":
                         # 移除物件
                         if hasattr(selected, 'socket_id'):
@@ -575,7 +575,7 @@ class ControlPanel:
                         created_list_menu = self.create_vehicles_list_menu(state, page=0)
                         menu_stack.append(created_list_menu)
                         idx_stack.append(0)
-                    
+
                     elif selected.action == "INSPECT_VEHICLE":
                         # 顯示載具詳細資訊
                         if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
@@ -585,7 +585,7 @@ class ControlPanel:
                     elif callable(selected.action):
                         # 執行函式
                         cmd_q.put(selected.action)
-                        
+
         try:
             curses.wrapper(draw_menu)
         except KeyboardInterrupt:
@@ -593,12 +593,12 @@ class ControlPanel:
         finally:
             cleanup()
 
-    # ================ 關於 mavlink object 的部份 =================== 
+    # ================ 關於 mavlink object 的部份 ===================
 
     def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 mavlink_object 列表選單（支持分頁）"""
         children = []
-        
+
         if not state.socket_object_list:
             children.append(MenuNode("(Empty)", "目前沒有連結口", None))
         else:
@@ -606,7 +606,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的物件
             for socket_id in state.socket_object_list[start_idx:end_idx]:
                 # 為每個 socket 創建子選單
@@ -656,11 +656,11 @@ class ControlPanel:
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD)
-        
+
         # 這裡顯示基本資訊
         dialog_win.addstr(2, 2, f"Socket ID     : {socket_id}")
         dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}")
@@ -676,15 +676,15 @@ class ControlPanel:
         dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}")
 
         state.socket_info_single['InfoReady'] = False  # 重置狀態以便下次使用
-        
+
         dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
         dialog_win.refresh()
-        
+
         dialog_win.getch()
         del dialog_win
         stdscr.clear()
         stdscr.refresh()
-    
+
     def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False):
         """選擇目標 socket 的對話框"""
         height, width = stdscr.getmaxyx()
@@ -692,13 +692,13 @@ class ControlPanel:
         dialog_width = min(50, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.keypad(True)
-        
+
         title = "選擇要移除的目標" if remove_mode else "選擇轉發目標"
         available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id]
-        
+
         if not available_sockets:
             dialog_win.border()
             dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
@@ -710,24 +710,24 @@ class ControlPanel:
             stdscr.clear()
             stdscr.refresh()
             return None
-        
+
         selected_idx = 0
-        
+
         while True:
             dialog_win.clear()
             dialog_win.border()
             dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
-            
+
             for i, socket_id in enumerate(available_sockets):
                 marker = "➤" if i == selected_idx else " "
                 attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL
                 dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr)
-            
+
             dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消")
             dialog_win.refresh()
-            
+
             ch = dialog_win.getch()
-            
+
             if ch in (curses.KEY_UP, ord('k')):
                 selected_idx = (selected_idx - 1) % len(available_sockets)
             elif ch in (curses.KEY_DOWN, ord('j')):
@@ -749,11 +749,11 @@ class ControlPanel:
     def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 serial port 列表選單（支持分頁）"""
         children = []
-        
+
         # 獲取可用的 Serial 連接埠列表
         # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓
         serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
-        
+
         if not serial_ports:
             children.append(MenuNode("(Empty)", "目前沒有串口設備", None))
         else:
@@ -761,7 +761,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的串口
             for port in serial_ports[start_idx:end_idx]:
                 port_menu = MenuNode(f"{port}", children=[
@@ -781,7 +781,7 @@ class ControlPanel:
                 for child in port_menu.children:
                     child.port = port
                 children.append(port_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -793,7 +793,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -802,7 +802,7 @@ class ControlPanel:
     def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8):
         """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能"""
         children = []
-        
+
         if not state.linked_serial_dict:
             children.append(MenuNode("(Empty)", "目前沒有連結口", None))
         else:
@@ -810,7 +810,7 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # 顯示當前頁的物件
             linked_serial_id_list = list(state.linked_serial_dict.keys())
             for serial_id in linked_serial_id_list[start_idx:end_idx]:
@@ -825,7 +825,7 @@ class ControlPanel:
                 for child in obj_menu.children:
                     child.serial_id = serial_id
                 children.append(obj_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -837,7 +837,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -859,14 +859,14 @@ class ControlPanel:
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.border()
         dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD)
 
         # 從 linked_serial_dict 獲取資訊
         serial_info = state.linked_serial_dict.get(serial_id, {})
-        
+
         if not serial_info:
             dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊")
         else:
@@ -877,17 +877,17 @@ class ControlPanel:
             dialog_win.addstr(5, 2, f"Communication   : {state.serial_info_single.get('receiver_type', 'N/A')}")
             dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}")
             dialog_win.addstr(7, 2, f"Status          : {state.serial_info_single.get('status', 'Running')}")
-            
+
             # 如果有額外資訊可以繼續添加
             if 'thread_alive' in serial_info:
                 thread_status = "Alive" if serial_info['thread_alive'] else "Stopped"
                 dialog_win.addstr(8, 2, f"Thread Status   : {thread_status}")
-        
+
         state.serial_info_single['InfoReady'] = False  # 重置狀態以便下次使用
 
         dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
         dialog_win.refresh()
-        
+
         dialog_win.getch()
         del dialog_win
         stdscr.clear()
@@ -900,7 +900,7 @@ class ControlPanel:
         每個 vehicle-component 組合都是獨立的選單項目
         """
         children = []
-        
+
         if not state.connected_vehicles_dict:
             children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None))
         else:
@@ -908,22 +908,22 @@ class ControlPanel:
             total_pages = (total_items + items_per_page - 1) // items_per_page
             start_idx = page * items_per_page
             end_idx = min(start_idx + items_per_page, total_items)
-            
+
             # vehicle_id_list 現在是 (sysid, compid) 的元組列表
             vehicle_comp_list = list(state.connected_vehicles_dict.keys())
-            
+
             # 顯示當前頁的物件
             for sysid, compid in vehicle_comp_list[start_idx:end_idx]:
                 # 建立顯示名稱
                 display_name = f"Vehicle #{sysid} - Comp #{compid}"
                 desc = f"載具 {sysid} 組件 {compid}"
-                
+
                 vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE")
                 # 將 sysid 和 compid 附加到選單項目上
                 vehicle_menu.sysid = sysid
                 vehicle_menu.compid = compid
                 children.append(vehicle_menu)
-            
+
             # 添加分頁控制
             if total_pages > 1:
                 children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
@@ -935,7 +935,7 @@ class ControlPanel:
                     next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                     next_node.page = page + 1
                     children.append(next_node)
-        
+
         children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
         menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children)
         menu.current_page = page
@@ -943,7 +943,7 @@ class ControlPanel:
 
     def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState):
         """顯示載具組件詳細資訊（動態更新，顯示變化率）"""
-        
+
         # 等待資訊準備
         start = time.time()
         while not state.vehicle_info_single.get('InfoReady', False):
@@ -951,30 +951,30 @@ class ControlPanel:
                 state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time()))
                 return
             time.sleep(0.05)
-        
+
         info = state.vehicle_info_single
         height, width = stdscr.getmaxyx()
         dialog_height = min(22, height - 4)
         dialog_width = min(70, width - 4)
         start_y = (height - dialog_height) // 2
         start_x = (width - dialog_width) // 2
-        
+
         dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
         dialog_win.nodelay(True)  # 非阻塞模式，允許動態更新
         dialog_win.keypad(True)
-        
+
         # MAV_TYPE 名稱對應
         MAV_TYPE_NAMES = {
             0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter",
             4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover",
             12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera"
         }
-        
+
         # 動態更新迴圈
         last_update = time.time()
         while True:
             current_time = time.time()
-            
+
             # 每 1 秒重新請求資料
             if current_time - last_update >= 1.0:
                 # 觸發資料更新（透過 Orchestrator）
@@ -989,58 +989,58 @@ class ControlPanel:
                 # 更新 info 參照
                 info = state.vehicle_info_single
                 last_update = current_time
-            
+
             dialog_win.clear()
             dialog_win.border()
             dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD)
-            
+
             # === 基礎資訊 ===
             dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE)
             dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE)
-            
+
             # # MAV Type # 這個用不到
             # mav_type = info.get('vehicle_type', 'N/A')
             # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type)
             # dialog_win.addstr(3, 2, f"MAV Type       : {mav_type_str}")
-            
+
             # Component Type
             dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}")
-            
+
             # Autopilot Type
             if info.get('mav_autopilot') is not None:
                 dialog_win.addstr(4, 2, f"Autopilot      : {info.get('mav_autopilot', 'N/A')}")
-            
+
             # Connection Info
             dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}")
             dialog_win.addstr(4, 32, f"Socket ID  : #{info.get('socket_id', 'N/A')}")
-            
+
             # === 封包統計 ===
             stats = info.get('packet_stats', {})
             dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE)
-            
+
             received = stats.get('received', 0)
             lost = stats.get('lost', 0)
             loss_rate = stats.get('loss_rate', 0.0)
             last_seq = stats.get('last_seq', 'N/A')
-            
+
             # 計算變化
             received_delta = stats.get('received_delta', 0)
             lost_delta = stats.get('lost_delta', 0)
-            
+
             # 顯示變化率
             recv_str = f"{received:6d}"
             if received_delta > 0:
                 recv_str += f" (+{received_delta}↑)"
-            
+
             lost_str = f"{lost:4d}"
             if lost_delta > 0:
                 lost_str += f" (+{lost_delta}↑)"
-            
+
             dialog_win.addstr(8, 2, f"Received  : {recv_str}")
             dialog_win.addstr(8, 32, f"Lost       : {lost_str}")
             dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%")
             dialog_win.addstr(9, 32, f"Last Seq   : {last_seq}")
-            
+
             # 最後接收時間
             last_msg_time = stats.get('last_msg_time')
             if last_msg_time:
@@ -1050,50 +1050,50 @@ class ControlPanel:
                 dialog_win.addstr(10, 32, f"Elapsed    : {elapsed:.1f}s")
             else:
                 dialog_win.addstr(10, 2, "Last Time : N/A")
-            
+
             # === 訊息類型分佈 ===
             dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE)
-            
+
             msg_counts = info.get('msg_type_counts', {})
-            
+
             # MAVLink 訊息名稱對應（縮寫版本）
             MSG_NAMES = {
                 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
                 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                 74: "VFR_HUD", 147: "BATT_ST"
             }
-            
+
             # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
             msg_items = list(msg_counts.items())[:12]
             line = 13
             for i, (msg_id, count) in enumerate(msg_items):
                 msg_name = MSG_NAMES.get(msg_id, "???")
                 delta = stats.get(f'msg_delta_{msg_id}', 0)
-                
+
                 # 格式化數據
                 if delta > 0:
                     data_str = f"{count}(+{delta}↑)"
                 else:
                     data_str = f"{count}"
-                
+
                 # 格式化顯示：[ID]NAME  DATA (ID固定3字符寬度，右對齊)
                 display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}"
-                
+
                 # 左列(偶數索引)或右列(奇數索引)
                 col = 2 if i % 2 == 0 else 36
                 row = line + (i // 2)
-                
+
                 if row >= dialog_height - 3:  # 避免超出邊界
                     break
-                    
+
                 dialog_win.addstr(row, col, display_str)
-            
+
             # 確保跳過顯示區域
             line = line + 6
-            
+
             dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...")
             dialog_win.refresh()
-            
+
             # 檢查是否有按鍵（非阻塞）
             ch = dialog_win.getch()
             if ch in (ord('R'), ord('r')):
@@ -1103,10 +1103,10 @@ class ControlPanel:
                 break
             elif ch != -1:  # 有按鍵則退出
                 break
-            
+
             # 短暫延遲
             time.sleep(0.1)
-        
+
         state.vehicle_info_single['InfoReady'] = False
         del dialog_win
         stdscr.clear()
@@ -1140,19 +1140,20 @@ class Orchestrator:
         self.fc_ros_manager = mros.ros2_manager
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
-            'position': 1.0,   
-            'attitude': 0.0,    
-            'velocity': 0.0,     
-            'battery': 1.0,   
-            'vfr_hud': 1.0,    
-            'mode': 0.0,       
-            'summary': 1.0,    
+            'position': 1.0,
+            'position_ned': 1.0,
+            'attitude': 1.0,
+            'velocity': 0.0,
+            'battery': 1.0,
+            'vfr_hud': 1.0,
+            'mode': 0.0,
+            'summary': 1.0,
         }
 
     def engageWholeSystem(self):
         """啟動整個系統"""
         # === 1) 面板部分的啟動 ===
-        self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) 
+        self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt))
         self.panel_thread.start()
 
         # === 2) async_io_manager & mavlink_bridge 部分的啟動 ===
@@ -1215,7 +1216,7 @@ class Orchestrator:
                             for s_id in mo.mavlink_object.mavlinkObjects:
                                 s_obj = mo.mavlink_object.mavlinkObjects[s_id]
                                 if socket_id in s_obj.target_sockets:
-                                    self.remove_target_from_object(s_id, socket_id)   
+                                    self.remove_target_from_object(s_id, socket_id)
                             # 再移除該物件
                             self.delete_udp_object(socket_id)
                         elif action == "MAVOBJ_ADD_TARGET":
@@ -1262,7 +1263,7 @@ class Orchestrator:
                         elif action == "UNREGISTER_VEHICLE":
                             sysid = cmd[1]
                             self.vehicle_registry.unregister(sysid)
-                            
+
                     elif cmd == "CREATE_UDP_INBOUND":
                         self.panelState.udp_info_temp["direction"] = "inbound"
                         self.create_udp_object()
@@ -1291,8 +1292,8 @@ class Orchestrator:
             logger.error(f"Unexpected error in main loop: {e}")
         finally:
 
-            # 驗證並確保所有模組都被下達關閉訊號 
-            # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 
+            # 驗證並確保所有模組都被下達關閉訊號
+            # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發
             if self.bridge.thread.is_alive():
                 if self.bridge.running:
                     self.bridge.stop()
@@ -1318,7 +1319,7 @@ class Orchestrator:
                 self.panel_thread.join(timeout=2)
 
             time.sleep(0.5) # 等待各模組穩定關閉
-                
+
             logger.info("Main orchestrator END!")
 
     # =============== 面板動作 - Mavlink Object ===============
@@ -1339,10 +1340,11 @@ class Orchestrator:
                 self.occupied_ip_ports[ip].append(port)
             else:
                 self.occupied_ip_ports[ip] = [port]
+
         # 外放資訊部分
         elif self.panelState.udp_info_temp["direction"] == "outbound":
             connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
-        
+
         try:
             # 檢測這個 connection_string 是否能成功建立 mavlink 連結
             mavlink_socket = mavutil.mavlink_connection(connection_string)
@@ -1362,7 +1364,7 @@ class Orchestrator:
             mavlink_object.socket_type = socket_type
 
         self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time()))
-    
+
     def delete_udp_object(self, socket_id):
         """移除指定的 mavlink_object"""
 
@@ -1406,10 +1408,10 @@ class Orchestrator:
     def _update_vehicles_list(self):
         """更新已連線載具列表（從 vehicle_registry 獲取）"""
         vehicles_dict = {}
-        
+
         # 從 vehicle_registry 獲取所有載具
         all_vehicles = self.vehicle_registry.get_all()
-        
+
         for sysid, vehicle in all_vehicles.items():
             # 遍歷每個載具的所有組件
             for compid, component in vehicle.components.items():
@@ -1422,9 +1424,9 @@ class Orchestrator:
                     'connection_via': vehicle.connected_via.value,
                     'socket_id': vehicle.custom_meta.get('socket_id', 'N/A')
                 }
-        
+
         self.panelState.connected_vehicles_dict = vehicles_dict
-    
+
     def _prepare_vehicle_info(self, sysid, compid):
         """準備載具組件的詳細資訊（包含變化率計算）"""
         vehicle = self.vehicle_registry.get(sysid)
@@ -1433,37 +1435,37 @@ class Orchestrator:
             return
 
         socket_id = vehicle.custom_meta.get('socket_id', 'N/A')
-        
+
         component = vehicle.get_component(compid)
         if not component:
             logger.warning(f"Component {compid} not found in vehicle {sysid}")
             return
-        
+
         stats = component.packet_stats
-        
+
         # 取得之前的統計資料（用於計算變化）
         prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {})
         prev_received = prev_stats.get('received', 0)
         prev_lost = prev_stats.get('lost', 0)
         prev_msg_counts = prev_stats.get('msg_counts', {})
-        
+
         # 計算變化率
         received_delta = stats.received_count - prev_received
         lost_delta = stats.lost_count - prev_lost
-        
+
         # 準備訊息類型計數（排序後取前幾個）
         sorted_msg_counts = dict(sorted(
             stats.msg_type_count.items(),
             key=lambda x: x[1],
             reverse=True
         )[:12])  # 取前 12 個最常見的
-        
+
         # 計算每種訊息類型的變化
         msg_deltas = {}
         for msg_id, count in sorted_msg_counts.items():
             prev_count = prev_msg_counts.get(msg_id, 0)
             msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count
-        
+
         # 更新 vehicle_info_single
         socket_type = "N/A"
         socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None)
@@ -1481,7 +1483,7 @@ class Orchestrator:
             "packet_stats": {
                 "received": stats.received_count,
                 "lost": stats.lost_count,
-                "loss_rate": (stats.lost_count / stats.received_count * 100 
+                "loss_rate": (stats.lost_count / stats.received_count * 100
                              if stats.received_count > 0 else 0),
                 "last_seq": stats.last_seq,
                 "last_msg_time": stats.last_msg_time,
@@ -1508,8 +1510,8 @@ class Orchestrator:
                 (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time())
             )
             return
-        
-        # 獲取可用的 udp port 
+
+        # 獲取可用的 udp port
         udp_port_tmp = find_available_port(19000, 20000)
 
         # 定義通訊類型映射表
@@ -1526,7 +1528,7 @@ class Orchestrator:
                 ("Please select Communication Type first.", time.time())
             )
             return
-        
+
         # 查找對應的通訊類型
         comm_type_tmp = COMM_TYPE_MAP.get(comm_type)
         if comm_type_tmp is None:
@@ -1554,12 +1556,31 @@ class Orchestrator:
             self.panelState.udp_info_temp['Port'] = str(udp_port_tmp)
             self.panelState.udp_info_temp['direction'] = "inbound"
             self.create_udp_object("SERIAL_XBEE")
-        
+
 
 def main():
 
-    stop_evt = threading.Event()
+    # =========== 各項模組的版本先驗 ===========
+    # 除非你有在做這幾項模組的改版 不然動到這邊的版本號 代表執行環境有很大的問題!!!!!!
+    version_check = True
+    if mo.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkObkect")
+        version_check = False
+    if mros.MODULE_VER != "1.50":
+        print("Module Version Error! : mavlinkROS2Nodes")
+        version_check = False
+    if mvv.MODULE_VER != "1.00":
+        print("Module Version Error! : mavlinkVehicleView")
+        version_check = False
+    if sm.MODULE_VER != "0.60":
+        print("Module Version Error! : serialManager")
+        version_check = False
+    if version_check == False:
+        print("Environment Obstacle! Check YOUR Execution System Path First!!")
+        return
+    # ========================================
 
+    stop_evt = threading.Event()
     def signal_handler(signum, frame):
         """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組"""
         stop_evt.set()
@@ -1593,4 +1614,7 @@ if __name__ == "__main__":
 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
 5. 系統納入 mavlink ROS2 Manager 模組
 
+2025.04.13
+1. 加入各項模組的版本先驗 檢驗失敗就直接離開
+
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -28,9 +28,6 @@ mavlink_bridge:
     專門處理 stream_bridge_ring 裡面的訊息流
     會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖
 
-
-
-
 '''
 
 # 基礎功能的 import
@@ -61,6 +58,8 @@ from .utils import RingBuffer, setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
+
 
 stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255)
 return_packet_ring = RingBuffer(capacity=256, buffer_id=254)
@@ -386,7 +385,7 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -1,10 +1,15 @@
 """
 MAVLink ROS2 Nodes
-包含兩個獨立的 ROS2 Node：
+主要包含兩個獨立的 ROS2 Node :
 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
+    從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
+    以最基礎的 mavlink microservice 會實現目標 
+    並不會包含額外的功能
+
+與一個節點管理器
+- fc_ros_manager
 
-從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
 """
 
 import os
@@ -22,10 +27,12 @@ from rclpy.executors import MultiThreadedExecutor
 import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
+import nav_msgs.msg
 import mavros_msgs.msg
 
 # ROS2 Service imports
 import fc_interfaces.srv as fcsrv
+from .ackEnum import serviceAckResult
 
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
@@ -33,12 +40,13 @@ from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.50"
 
 # ============================================================================
-#                          頻率控制器
+#                       VehicleStatusPublisher Node
 # ============================================================================
 
+# 頻率控制器
 class PublishRateController:
     """發布頻率控制器 - 按時間間隔控制發布頻率"""
     
@@ -49,14 +57,17 @@ class PublishRateController:
         # 例如：'ekf_status': 1.0,  # EKF 狀態
         # ═══════════════════════════════════════════════════════════════
         # 各 topic 的發布間隔（秒）
+        # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
+        # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.5,       # GPS位置
-            'attitude': 0.5,       # 姿態
-            'velocity': 0.5,       # 速度
-            'battery': 1.0,        # 電池
-            'vfr_hud': 0.5,        # VFR HUD
-            'mode': 1.0,           # 飛行模式
-            'summary': 1.0,        # 載具摘要
+            'position': 0.0,       # GNSS位置
+            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
+            'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
+            'battery': 0.0,        # 電池
+            'vfr_hud': 0.0,        # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
+            'mode': 0.0,           # 飛行模式 (已經在 summary 裡 未來移除)
+            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -97,11 +108,6 @@ class PublishRateController:
         """重置所有計時器"""
         self.last_publish_time.clear()
 
-
-# ============================================================================
-#                       VehicleStatusPublisher Node
-# ============================================================================
-
 class VehicleStatusPublisher(Node):
     """
     載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
@@ -138,10 +144,8 @@ class VehicleStatusPublisher(Node):
         if not self.running:
             return
 
-        # 從 vehicle_registry 獲取所有載具
-        all_vehicles = mvv.vehicle_registry.get_all()
-        
-        for sysid, vehicle in all_vehicles.items():
+        # 高頻快路徑：讀取 registry 的 immutable 快照
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
             self._publish_vehicle_status(vehicle)
     
     def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
@@ -167,6 +171,7 @@ class VehicleStatusPublisher(Node):
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
         self._publish_position(sysid, status)
+        self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
         self._publish_battery(sysid, status)
@@ -222,6 +227,42 @@ class VehicleStatusPublisher(Node):
             msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
         
         publisher.publish(msg)
+
+    def _publish_position_ned(self, sysid: int, status: mvv.ComponentStatus):
+        """發布 LOCAL_POSITION_NED（NED 座標，含位置與速度）"""
+        if not self.rate_controller.should_publish(sysid, 'position_ned'):
+            return
+
+        local_pos = status.custom_status.get('local_position')
+        if not local_pos:
+            return
+
+        required_keys = ('x', 'y', 'z', 'vx', 'vy', 'vz')
+        if any(local_pos.get(k) is None for k in required_keys):
+            return
+
+        publisher = self._get_or_create_publisher(
+            sysid, 'position_ned', nav_msgs.msg.Odometry
+        )
+
+        if publisher.get_subscription_count() == 0:
+            return
+
+        msg = nav_msgs.msg.Odometry()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'local_ned'
+        msg.child_frame_id = 'base_link_ned'
+
+        # 保持 MAVLink LOCAL_POSITION_NED 座標定義，不進行 ENU 轉換
+        msg.pose.pose.position.x = float(local_pos['x'])
+        msg.pose.pose.position.y = float(local_pos['y'])
+        msg.pose.pose.position.z = float(local_pos['z'])
+
+        msg.twist.twist.linear.x = float(local_pos['vx'])
+        msg.twist.twist.linear.y = float(local_pos['vy'])
+        msg.twist.twist.linear.z = float(local_pos['vz'])
+
+        publisher.publish(msg)
     
     def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
         """發布姿態（IMU）"""
@@ -231,7 +272,6 @@ class VehicleStatusPublisher(Node):
         att = status.attitude
         if att.roll is None:
             return
-        
         publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
         
         if publisher.get_subscription_count() == 0:
@@ -471,12 +511,17 @@ class MavlinkCommandService(Node):
     - 作為 service server 等待 client 請求
     - 接收請求，組裝 MAVLink 封包
     - 調用 mavlinkObject 發送封包
-    - 處理 ACK 等待和超時（未來實現）
+    - 處理 ACK 等待和超時
     
     設計理念：回歸 MAVLink 純粹結構
     - 只負責將 ROS2 請求轉換為 MAVLink 封包
     - 不預設功能（如 ARM/DISARM) 保持模組化
     - 高層應用可透過此 service 實現各種功能
+
+    講白話一點就是
+    每次接到一個 service 請求 要整個系統丟某種指令給載具時
+    會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
+
     """
 
     serviceString_prefix = '/fc_network/vehicle'
@@ -487,8 +532,8 @@ class MavlinkCommandService(Node):
         # 狀態標記
         self.running = True
         
-        # mavlinkObject 的引用（將由外部設置）
-        self.mavlink_analyzer = None
+        # # mavlinkObject 的引用（將由外部設置） 用不到
+        # self.mavlink_analyzer = None
 
         # pending 旗標物件的儲存庫
         self._pending_by_sysid = {} # sysid(int) : PendingEntry
@@ -521,6 +566,15 @@ class MavlinkCommandService(Node):
             self.handle_command_long
         )
 
+        # ═══════════════════════════════════════════════════════════════════
+        # Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_pos_global_int = self.create_service(
+            fcsrv.MavPositionTargetGlobalInt,
+            self.serviceString_prefix + '/pos_global_int',
+            self.handle_set_position_target_global_int
+        )
+
         logger.info("MavlinkCommandService initialized")
 
     def _index(self, target_sysid, target_compid):
@@ -542,7 +596,8 @@ class MavlinkCommandService(Node):
 
     def return_router(self):
         '''
-        這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包
+        在節點管理器哪邊被呼叫
+        消耗 return_packet_ring 裡接收到的 mavlink 封包
         分送到各自的 pending 中 
         藉由 event.set() 解開 service 中的 block 
         '''
@@ -568,6 +623,7 @@ class MavlinkCommandService(Node):
     def handle_mav_timesync_ping(self, request, response):
         '''
         用 timesync 封包驗證來回的時間
+        隸屬於 MavLink PING Protocol
         '''
         # 設定失效回應
         response.success = False
@@ -612,7 +668,7 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - TIMESYNC"
+            response.message = "mavlink timeout - TIMESYNC"
             fail_skip = True
 
         # 6) 處理回應封包
@@ -633,35 +689,31 @@ class MavlinkCommandService(Node):
 
         return response
 
-    def handle_add_two_ints(self, request, response):
-        """測試用 Service Handler 未來刪除"""
-        logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
-        response.sum = request.a + request.b
-        logger.info(
-            f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
-        )
-        time.sleep(1)
-
-        return response
-
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 COMMAND_LONG
+    # Service Handler: 發送 COMMAND_LONG (76)
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
+        '''
+        用 command long 丟出  MAV_CMD 
+        隸屬於 MavLink Command Protocol
+        '''
         # 設定失效回應
         response.success = False
         response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
             response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
             return response
 
         # 2) 找到 socket 標地
         socketObject = self._index(request.target_sysid, request.target_compid)
         if socketObject is None:
             response.message = "This system id not found."
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
             return response
 
         # 3) 接收封包系統 的設定
@@ -691,7 +743,8 @@ class MavlinkCommandService(Node):
         fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - CommLONG"
+            response.message = "mavlink timeout - CommLONG"
+            response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
             fail_skip = True
 
         # 6) 處理回應封包
@@ -711,7 +764,96 @@ class MavlinkCommandService(Node):
 
         return response
 
-    
+    # ═══════════════════════════════════════════════════════════════════════
+    # Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
+    # ═══════════════════════════════════════════════════════════════════════
+    def handle_set_position_target_global_int(self, request, response):
+        '''
+        隸屬於 MavLink Offboard Control Protocol
+        '''
+        # 設定失效回應
+        response.message = "Unknown error"
+        response.ack_result = serviceAckResult.UNKNOWN
+        timeout_sec = request.timeout_sec
+
+        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
+        if request.target_sysid in self._pending_by_sysid:
+            response.message = f"sysid {request.target_sysid} already has pending request"
+            response.ack_result = serviceAckResult.MAVLINK_BUSY
+            return response
+
+        # 2) 找到 socket 標地
+        socketObject = self._index(request.target_sysid, request.target_compid)
+        if socketObject is None:
+            response.message = "This system id not found."
+            response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
+            return response
+
+        # 3) 接收封包系統 的設定
+        # 在 socket 那邊先把要的封包種類導流進來
+        expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT 
+        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
+        evt = threading.Event()
+        # 設定封包檢驗 
+        def match_fn(compare_msg):
+            if compare_msg.get_msgId() != expect_recieve_msg_id : 
+                return False
+            return True
+
+        # 4) 送出封包
+        socketObject.MAVLink.set_position_target_global_int_send(
+            request.time_boot_ms, 
+            request.target_sysid,
+            request.target_compid,
+            request.coordinate_frame,
+            request.type_mask,
+            request.lat_int, request.lon_int, request.alt,
+            request.vx, request.vy, request.vz,
+            request.afx, request.afy, request.afz,
+            request.yaw, request.yaw_rate
+        )
+
+        time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
+        _pending = PendingEntry(event = evt, match_fn = match_fn)
+        self._pending_by_sysid[request.target_sysid] = _pending
+
+        fail_skip = False
+        # 5) 等待回應封包
+        if not evt.wait(timeout_sec):
+            response.message = "mavlink timeout - SET POSITION 86"
+            response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
+            fail_skip = True
+
+        # 6) 處理回應封包
+        if not fail_skip:
+            ack_msg = _pending.result_mav_msg
+
+            response.message = ""
+            response.ack_result = 0
+            response.r_time_boot_ms = ack_msg.time_boot_ms 
+            response.r_type_mask = ack_msg.type_mask
+            response.r_lat_int = ack_msg.lat_int
+            response.r_lon_int = ack_msg.lon_int
+            response.r_alt = ack_msg.alt
+            response.r_vx = ack_msg.vx
+            response.r_vy = ack_msg.vy
+            response.r_vz = ack_msg.vz
+            response.r_afx = ack_msg.afx
+            response.r_afy = ack_msg.afy
+            response.r_afz = ack_msg.afz
+            response.r_yaw = ack_msg.yaw
+            response.r_yaw_rate = ack_msg.yaw_rate
+
+        # 7) 接收封包系統 的重置
+        msg_types = list(socketObject.return_msg_types)
+        if expect_recieve_msg_id in msg_types:
+            msg_types.remove(expect_recieve_msg_id)
+        socketObject.set_return_message_types(msg_types)
+        del evt
+        self._pending_by_sysid.pop(request.target_sysid, None)
+
+        return response
+
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 參數請求
     # ═══════════════════════════════════════════════════════════════════════
@@ -790,7 +932,21 @@ class MavlinkCommandService(Node):
         response.success = True
         response.message = "Param request sent (placeholder implementation)"
         return response
-    
+
+
+    def handle_add_two_ints(self, request, response):
+        """測試用 Service Handler 未來刪除"""
+        logger.info(f"Received add_two_ints request: {request.a} + {request.b}")
+        response.sum = request.a + request.b
+        logger.info(
+            f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
+        )
+        time.sleep(1)
+
+        return response
+
+
+
     def stop(self):
         """停止服務"""
         self.running = False
@@ -837,7 +993,8 @@ class fc_ros_manager:
         
         try:
             # 初始化 ROS2
-            rclpy.init()
+            if not rclpy.ok():
+                rclpy.init(args=None)
             
             # 創建節點 node
             self.status_publisher = VehicleStatusPublisher()
@@ -973,7 +1130,15 @@ ros2_manager = fc_ros_manager()
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
 2026.03.27
-1. 完成 ros2 service 結構與 timesync command_long 協議
+1. 完成 ros2 service 結構與 timesync 與 command_long 協議
+
+2026.04.07
+1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
+2. 優化 response.ack_result 回傳值的資訊
+
 
+TODO 
+1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
+2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
 '''
 

src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py

@@ -5,14 +5,14 @@ VehicleView - Pure State Container
 """
 
 import os
-from typing import Dict, Optional, Any
+from typing import Dict, Optional, Any, Tuple
 from dataclasses import dataclass, field
 from enum import Enum
 
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
-
+MODULE_VER = "1.00"
 
 # ====================== Enums =====================
 
@@ -402,11 +402,18 @@ class VehicleViewRegistry:
     
     def __init__(self):
         self._vehicles: Dict[int, VehicleView] = {}
+        # 高頻讀取用 snapshot（弱一致性）
+        self._vehicles_snapshot_items: Tuple[Tuple[int, VehicleView], ...] = tuple()
+
+    def _refresh_snapshot(self) -> None:
+        """重建快照（僅在 key 集合變動時觸發）"""
+        self._vehicles_snapshot_items = tuple(self._vehicles.items())
     
     def register(self, sysid: int) -> VehicleView:
         """註冊新的載具視圖"""
         if sysid not in self._vehicles:
             self._vehicles[sysid] = VehicleView(sysid)
+            self._refresh_snapshot()
             logger.info(f"Registered new VehicleView for system {sysid}")
         return self._vehicles[sysid]
     
@@ -418,6 +425,7 @@ class VehicleViewRegistry:
         """註銷載具視圖"""
         if sysid in self._vehicles:
             del self._vehicles[sysid]
+            self._refresh_snapshot()
             logger.info(f"Unregistered VehicleView for system {sysid}")
             return True
         return False
@@ -425,10 +433,22 @@ class VehicleViewRegistry:
     def get_all(self) -> Dict[int, VehicleView]:
         """取得所有載具視圖"""
         return self._vehicles.copy()
+
+    def read_all(self) -> Tuple[Tuple[int, VehicleView], ...]:
+        """
+        高效讀取所有載具視圖快照（弱一致性）
+
+        注意:
+        - 回傳 immutable tuple 快照，適合高頻迭代
+        - 僅在 register/unregister/clear 後更新，可能不是最新資料
+        - 適合高頻讀取且可接受弱一致性的情境
+        """
+        return self._vehicles_snapshot_items
     
     def clear(self) -> None:
         """清空所有載具視圖"""
         self._vehicles.clear()
+        self._refresh_snapshot()
         logger.info("Cleared all VehicleViews")
     
     def __len__(self) -> int:
@@ -449,5 +469,8 @@ vehicle_registry = VehicleViewRegistry()
 2026.01.16
 1. 新增 重置指定組件的封包統計 功能
 
+2026.04.13
+1. 新增 VehicleViewRegistry.read_all 方法 讓 ROS2 Node 組件更有效率的讀取其中的資料
+
 '''
 

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -26,6 +26,7 @@ from .utils import setup_logger
 # ====================== 分割線 =====================
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "0.60"
 
 # ====================== 分割線 =====================
 

src/fc_network_apps/__init__.py

@@ -1,14 +1,10 @@
-from .longCommand import CommandLongClient, ChangeModeResult
-from .changeMode import change_mode
-from .arm_disarm import arm_disarm
-from .takeoff import takeoff
-from .land import land
+from .longCommand import CommandLongClient, CommandLongResult
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
+
 
 __all__ = [
     "CommandLongClient",
-    "ChangeModeResult",
-    "change_mode",
-    "arm_disarm",
-    "takeoff",
-    "land",
+    "PositionTargetGlobalIntClient",
+    "CommandLongResult",
+    "PositionTargetGlobalIntResult",
 ]

src/fc_network_apps/arm_disarm.py

@@ -1,93 +0,0 @@
-"""Simple wrapper for arm/disarm via fc_network ROS2 service (MAV_CMD_COMPONENT_ARM_DISARM)."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_COMPONENT_ARM_DISARM = 400
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class ArmDisarmResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def arm_disarm(
-    *,
-    target_sysid: int,
-    arm: bool,
-    target_compid: int = 0,
-    confirmation: int = 0,
-    param2: float = 0.0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ArmDisarmResult:
-    """One-shot MAV_CMD_COMPONENT_ARM_DISARM (400) wrapper.
-
-    param1: 1.0 to arm, 0.0 to disarm.
-    param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed.
-    """
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_arm_disarm_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ArmDisarmResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_COMPONENT_ARM_DISARM
-        req.confirmation = confirmation
-        req.param1 = 1.0 if arm else 0.0
-        req.param2 = float(param2)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return ArmDisarmResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ArmDisarmResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    r = arm_disarm(target_sysid=3, arm=True)
-    print(
-        f"arm_disarm success={r.success}, "
-        f"ack_result={r.ack_result}, message='{r.message}'"
-    )

src/fc_network_apps/changeMode.py

@@ -1,99 +0,0 @@
-"""Simple wrapper for mode change via fc_network ROS2 service.
-
-Reference CLI command:
-ros2 service call /fc_network/vehicle/send_command_long \
-  fc_interfaces/srv/MavCommandLong \
-  "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, \
-    param1: 1, param2: 4, param3: 0, param4: 0, param5: 0, param6: 0, \
-    param7: 0, timeout_sec: 2}"
-"""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_DO_SET_MODE = 176
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-@dataclass
-class ChangeModeResult:
-    """Return value for mode change requests."""
-
-    success: bool
-    message: str
-    ack_result: int
-
-def change_mode(
-    *,
-    target_sysid: int,
-    custom_mode: float,
-    target_compid: int = 0,
-    base_mode: float = 1.0,
-    confirmation: int = 0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> ChangeModeResult:
-
-    """One-shot helper for collaborators who want minimal code."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_change_mode_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return ChangeModeResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
-        req.confirmation = confirmation
-        req.param1 = float(base_mode)
-        req.param2 = float(custom_mode)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return ChangeModeResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return ChangeModeResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    # Example values are aligned with your terminal command.
-    result = change_mode(target_sysid=3, custom_mode=4)
-    print(
-        f"change_mode success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/land.py

@@ -1,90 +0,0 @@
-"""Simple wrapper for land via fc_network ROS2 service."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_NAV_LAND = 21
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class LandResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def land(
-    *,
-    target_sysid: int,
-    target_compid: int = 0,
-    yaw_deg: float = 0.0,
-    latitude: Optional[float] = None,
-    longitude: Optional[float] = None,
-    altitude_m: float = 0.0,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> LandResult:
-    """One-shot MAV_CMD_NAV_LAND wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_land_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return LandResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_LAND
-        req.confirmation = 0
-        req.param1 = 0.0
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return LandResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return LandResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = land(target_sysid=3)
-    print(
-        f"land success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/fc_network_apps/longCommand.py

@@ -1,3 +1,4 @@
+"""ROS2 client for fc_network MavCommandLong service (COMMAND_LONG 系列指令)."""
 
 from __future__ import annotations
 
@@ -10,66 +11,177 @@ from rclpy.node import Node
 from fc_interfaces.srv import MavCommandLong
 
 COMMAND_DO_SET_MODE = 176
+COMMAND_NAV_LAND = 21
+COMMAND_NAV_TAKEOFF = 22
+COMMAND_COMPONENT_ARM_DISARM = 400
+
 DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
 DEFAULT_TIMEOUT_SEC = 2.0
 
 
 @dataclass
-class ChangeModeResult:
-    """Return value for mode change requests."""
-
+class CommandLongResult:
     success: bool
     message: str
     ack_result: int
 
-
 class CommandLongClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
+    """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 
     def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        rclpy.init()
-        super().__init__("fc_change_mode_client")
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        super().__init__("fc_command_long_client")
         self._client = self.create_client(MavCommandLong, service_name)
 
     def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
         return self._client.wait_for_service(timeout_sec=timeout_sec)
 
-    def change_mode(
+    def _send_command_long(
         self,
         *,
         target_sysid: int,
-        custom_mode: float,
-        target_compid: int = 0,
-        base_mode: float = 1.0,
-        confirmation: int = 0,
-        timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult:
-
+        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:
         req = MavCommandLong.Request()
         req.target_sysid = target_sysid
         req.target_compid = target_compid
-        req.command = COMMAND_DO_SET_MODE
+        req.command = command
         req.confirmation = confirmation
-        req.param1 = float(base_mode)
-        req.param2 = float(custom_mode)
-        req.param3 = 0.0
-        req.param4 = 0.0
-        req.param5 = 0.0
-        req.param6 = 0.0
-        req.param7 = 0.0
+        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)
         rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0)
         if not future.done() or future.result() is None:
-            return ChangeModeResult(
+            return CommandLongResult(
                 success=False,
-                message="Service call timeout or no response.",
+                message="Service timeout.",
                 ack_result=-1,
             )
 
         response = future.result()
-        return ChangeModeResult(
+        return CommandLongResult(
             success=response.success,
             message=response.message,
             ack_result=response.ack_result,
-        )
+        )
+
+    def change_mode(
+        self,
+        *,
+        target_sysid: int,
+        custom_mode: float,
+        target_compid: int = 0,
+        base_mode: float = 1.0,
+        confirmation: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            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,
+        )
+
+    def takeoff(
+        self,
+        *,
+        target_sysid: int,
+        altitude_m: float,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_TAKEOFF,
+            confirmation=0,
+            param1=float(min_pitch_deg),
+            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,
+        )
+
+    def land(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_LAND,
+            confirmation=0,
+            param1=0.0,
+            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,
+        )
+
+    def arm_disarm(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        return self._send_command_long(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
+            confirmation=confirmation,
+            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,
+        )

src/fc_network_apps/navigation.py

@@ -0,0 +1,262 @@
+"""ROS2 client for fc_network MavPositionTargetGlobalInt service (SET_POSITION_TARGET_GLOBAL_INT / Offboard)."""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+
+import rclpy
+from rclpy.node import Node
+
+from fc_interfaces.srv import MavPositionTargetGlobalInt
+
+DEFAULT_SERVICE_NAME = "/fc_network/vehicle/pos_global_int"
+DEFAULT_TIMEOUT_SEC = 2.0
+
+# MAV_FRAME
+MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # 使用 WGS84 GNSS 座標系 高度為相對於 Home 點高度
+
+# POSITION_TARGET_TYPEMASK
+# 1  1      Ignore position x
+# 2  2      Ignore position y
+# 3  4      Ignore position z
+# 4  8      Ignore velocity x
+# 5  16     Ignore velocity y
+# 6  32     Ignore velocity z
+# 7  64     Ignore acceleration x
+# 8  128	Ignore acceleration y
+# 9  256	Ignore acceleration z
+# 10 512	Use force instead of acceleration
+# 11 1024	Ignore yaw
+# 12 2048   Ignore yaw rate
+# 13+       Not Used 
+#                 6543210987654321
+POSITION_ONLY = 0b0000110111111000 # 忽略速度、加速度、偏航
+
+
+# 回傳 echo 與送出值比對時，浮點欄位允許的絕對誤差（MAVLink float 轉傳常有小差）
+_FLOAT_ECHO_ABS_TOL = 1e-5
+def _float_echo_equal(a: float, b: float) -> bool:
+    return math.isclose(float(a), float(b), rel_tol=0.0, abs_tol=_FLOAT_ECHO_ABS_TOL)
+
+
+def _echo_position_target_matches(
+    *,
+    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,
+    time_boot_ms: int,
+    resp,
+) -> tuple[bool, str]:
+    """
+    比對本次送出的軌跡欄位與 server 填回的 r_* 是否一致。
+    若不符，回傳 (False, 簡短說明)；相符則 (True, "")。
+    """
+
+    _tm_sent = int(type_mask) & 0xFFF
+    _tm_recv = int(resp.r_type_mask) & 0xFFF
+    if _tm_recv != _tm_sent:
+        return False, f"type_mask echo: sent {_tm_sent:012b} != ack {_tm_recv:012b}"
+
+    coordinate_tolerance = 5 * 100
+    if abs(resp.r_lat_int - lat_int) >= coordinate_tolerance:
+        return False, f"lat_int echo: sent {lat_int} != ack {resp.r_lat_int}"
+    if abs(resp.r_lon_int - lon_int) >= coordinate_tolerance:
+        return False, f"lon_int echo: sent {lon_int} != ack {resp.r_lon_int}"
+    # TODO 我給飛控相對高度 它回給我絕對高度 看來這個比較方法 還有很多改良點 但是之後吧
+    # if not _float_echo_equal(resp.r_alt, alt):
+    #     return False, f"alt echo: sent {alt} != ack {resp.r_alt}"
+    if not _float_echo_equal(resp.r_vx, vx):
+        return False, f"vx echo: sent {vx} != ack {resp.r_vx}"
+    if not _float_echo_equal(resp.r_vy, vy):
+        return False, f"vy echo: sent {vy} != ack {resp.r_vy}"
+    if not _float_echo_equal(resp.r_vz, vz):
+        return False, f"vz echo: sent {vz} != ack {resp.r_vz}"
+    if not _float_echo_equal(resp.r_afx, afx):
+        return False, f"afx echo: sent {afx} != ack {resp.r_afx}"
+    if not _float_echo_equal(resp.r_afy, afy):
+        return False, f"afy echo: sent {afy} != ack {resp.r_afy}"
+    if not _float_echo_equal(resp.r_afz, afz):
+        return False, f"afz echo: sent {afz} != ack {resp.r_afz}"
+    if not _float_echo_equal(resp.r_yaw, yaw):
+        return False, f"yaw echo: sent {yaw} != ack {resp.r_yaw}"
+    if not _float_echo_equal(resp.r_yaw_rate, yaw_rate):
+        return False, f"yaw_rate echo: sent {yaw_rate} != ack {resp.r_yaw_rate}"
+    # if int(resp.r_time_boot_ms) != int(time_boot_ms):
+    #     return False, f"time_boot_ms echo: sent {time_boot_ms} != r {resp.r_time_boot_ms}"
+    return True, ""
+
+
+@dataclass
+class PositionTargetGlobalIntResult:
+    """對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
+    success: bool
+    message: str
+    ack_result: int
+    r_time_boot_ms: int = 0
+    r_type_mask: int = 0
+    r_lat_int: int = 0
+    r_lon_int: int = 0
+    r_alt: float = 0.0
+    r_vx: float = 0.0
+    r_vy: float = 0.0
+    r_vz: float = 0.0
+    r_afx: float = 0.0
+    r_afy: float = 0.0
+    r_afz: float = 0.0
+    r_yaw: float = 0.0
+    r_yaw_rate: float = 0.0
+
+class PositionTargetGlobalIntClient(Node):
+    """
+    ROS2 client：對 pos_global_int service 發送 SET_POSITION_TARGET_GLOBAL_INT (msg 86)，
+    並等待 POSITION_TARGET_GLOBAL_INT (msg 87) 回應（由 server 端處理）。
+    """
+
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        super().__init__("fc_position_target_global_int_client")
+        self._client = self.create_client(MavPositionTargetGlobalInt, service_name)
+
+    def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
+        return self._client.wait_for_service(timeout_sec=timeout_sec)
+
+    def _send_position_target_global_int(
+        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:
+        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)
+        rclpy.spin_until_future_complete(self, future, timeout_sec=float(timeout_sec) + 1.0)
+        if not future.done() or future.result() is None:
+            return PositionTargetGlobalIntResult(
+                success=False,
+                message="Service timeout.",
+                ack_result=-1,
+            )
+
+        resp = future.result()
+
+        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,
+        )
+        # out_message = resp.message
+        # if not echo_ok:
+        #     out_message = echo_detail if not out_message else f"{echo_detail} | server: {out_message}"
+
+        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),
+        )
+
+    def goto_position_only(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        latitude_deg: float,
+        longitude_deg: float,
+        alt: float = 0.0,
+        timeout_sec: float,
+    ):
+        lat_int = int(round(latitude_deg * 1e7))
+        lon_int = int(round(longitude_deg * 1e7))
+        
+        return self._send_position_target_global_int(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            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/fc_network_apps/takeoff.py

@@ -1,91 +0,0 @@
-"""Simple wrapper for takeoff via fc_network ROS2 service."""
-
-from __future__ import annotations
-
-from dataclasses import dataclass
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-
-from fc_interfaces.srv import MavCommandLong
-
-COMMAND_NAV_TAKEOFF = 22
-DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long"
-DEFAULT_TIMEOUT_SEC = 2.0
-
-
-@dataclass
-class TakeoffResult:
-    success: bool
-    message: str
-    ack_result: int
-
-
-def takeoff(
-    *,
-    target_sysid: int,
-    altitude_m: float,
-    target_compid: int = 0,
-    min_pitch_deg: float = 0.0,
-    yaw_deg: float = 0.0,
-    latitude: Optional[float] = None,
-    longitude: Optional[float] = None,
-    timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    service_name: str = DEFAULT_SERVICE_NAME,
-) -> TakeoffResult:
-    """One-shot MAV_CMD_NAV_TAKEOFF wrapper."""
-    rclpy.init(args=None)
-    node: Optional[Node] = None
-    try:
-        node = Node("fc_takeoff_client_once")
-        client = node.create_client(MavCommandLong, service_name)
-
-        if not client.wait_for_service(timeout_sec=timeout_sec):
-            return TakeoffResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-
-        req = MavCommandLong.Request()
-        req.target_sysid = target_sysid
-        req.target_compid = target_compid
-        req.command = COMMAND_NAV_TAKEOFF
-        req.confirmation = 0
-        req.param1 = float(min_pitch_deg)
-        req.param2 = 0.0
-        req.param3 = 0.0
-        req.param4 = float(yaw_deg)
-        req.param5 = float(latitude) if latitude is not None else 0.0
-        req.param6 = float(longitude) if longitude is not None else 0.0
-        req.param7 = float(altitude_m)
-        req.timeout_sec = float(timeout_sec)
-
-        future = client.call_async(req)
-        rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0)
-        if not future.done() or future.result() is None:
-            return TakeoffResult(
-                success=False,
-                message="Service call timeout or no response.",
-                ack_result=-1,
-            )
-
-        response = future.result()
-        return TakeoffResult(
-            success=response.success,
-            message=response.message,
-            ack_result=response.ack_result,
-        )
-    finally:
-        if node is not None:
-            node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    result = takeoff(target_sysid=3, altitude_m=10.0)
-    print(
-        f"takeoff success={result.success}, "
-        f"ack_result={result.ack_result}, message='{result.message}'"
-    )

src/someotherpkg/src/example2_change_mode.py

@@ -1,55 +0,0 @@
-
-
-from fc_network_apps import CommandLongClient
-import time
-
-def main():
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    commandAPI = CommandLongClient()
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=4.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    time.sleep(1)
-
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=3.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    time.sleep(1)
-    
-    result = commandAPI.change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=5.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_arm_disarm.py

@@ -1,30 +0,0 @@
-"""Usage example for arm/disarm helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_arm_disarm
-"""
-
-from fc_network_apps import arm_disarm
-
-
-def main() -> None:
-    # MAV_CMD_COMPONENT_ARM_DISARM (command=400)
-    # param1: 1 = arm, 0 = disarm
-    result = arm_disarm(
-        target_sysid=3,
-        target_compid=0,
-        arm=True,
-        timeout_sec=2.0,
-    )
-
-    print("=== arm result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    # To disarm instead:
-    # result = arm_disarm(target_sysid=3, target_compid=0, arm=False, timeout_sec=2.0)
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_change_mode.py

@@ -1,29 +0,0 @@
-"""Usage example for collaborators.
-
-Run after sourcing your ROS2 workspace:
-    source install/local_setup.bash
-    python src/fc_network_apps/example_change_mode.py
-"""
-
-from fc_network_apps import change_mode
-
-
-def main() -> None:
-    # Equivalent to:
-    # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4
-    result = change_mode(
-        target_sysid=3,
-        target_compid=0,
-        base_mode=1.0,
-        custom_mode=4.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_land.py

@@ -1,27 +0,0 @@
-"""Usage example for land helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_land
-"""
-
-from fc_network_apps import land
-
-
-def main() -> None:
-    # MAV_CMD_NAV_LAND (command=21)
-    # This example asks vehicle sysid=3 to land.
-    result = land(
-        target_sysid=3,
-        target_compid=0,
-        yaw_deg=0.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== land result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_takeoff.py

@@ -1,28 +0,0 @@
-"""Usage example for takeoff helper.
-
-Run from repo root with module mode:
-    python -m someotherpkg.src.example_takeoff
-"""
-
-from fc_network_apps import takeoff
-
-
-def main() -> None:
-    # MAV_CMD_NAV_TAKEOFF (command=22)
-    # This example asks vehicle sysid=3 to take off to 10 meters.
-    result = takeoff(
-        target_sysid=3,
-        target_compid=0,
-        altitude_m=10.0,
-        yaw_deg=0.0,
-        timeout_sec=2.0,
-    )
-
-    print("=== takeoff result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-
-if __name__ == "__main__":
-    main()

src/someotherpkg/src/example_wholeMoving.py

@@ -0,0 +1,111 @@
+
+
+import fc_network_apps as fap
+import time
+
+def main():
+    commandAPI = fap.CommandLongClient()
+    navAPI = fap.PositionTargetGlobalIntClient()
+
+    target_sysid = 3
+
+
+    print("=== change mode ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.change_mode(
+            target_sysid=target_sysid,
+            base_mode=1.0,
+            custom_mode=4.0,
+            timeout_sec=3.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== arm vehicle ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.arm_disarm(
+            target_sysid=target_sysid,
+            arm = True,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+    print("=== takeoff ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.takeoff(
+            target_sysid=target_sysid,
+            altitude_m = 100.0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+
+    time.sleep(15)
+
+    print("=== set_position_target_global_int_deg ===")
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.376655,
+        longitude_deg=149.157011,
+        alt=150.0,
+        timeout_sec=3.0,
+    )
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    result_deg = navAPI.goto_position_only(
+        target_sysid=target_sysid,
+        latitude_deg=-35.3632621,
+        longitude_deg=149.1652375,
+        alt=100.0,
+        timeout_sec=3.0,
+    )
+    print("=== set_position_target_global_int_deg ===")
+    print(f"success   : {result_deg.success}")
+    print(f"ack_result: {result_deg.ack_result}")
+    print(f"message   : {result_deg.message}")
+    print(f"r_lat_int : {result_deg.r_lat_int}  r_lon_int: {result_deg.r_lon_int}")
+
+    time.sleep(180)
+
+    print("=== land ===")
+    result = fap.CommandLongResult(success=False,message="",ack_result=-1)
+    while not result.success:
+        result.ack_result -= 1
+        result = commandAPI.land(
+            target_sysid=3,
+            target_compid=0,
+        )
+        print(f"success   : {result.success}")
+        print(f"ack_result: {result.ack_result}")
+        print(f"message   : {result.message}")
+        time.sleep(1)
+        if result.ack_result < -3:
+            return
+
+if __name__ == "__main__":
+    main()
+
+

src/unitdev02/unitdev02/devnote.txt

@@ -24,25 +24,34 @@ python -m fc_network_adapter.tests.test_vehicleStatusPublisher
 python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 
+python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
 ros2 topic list
-ros2 topic echo 
+ros2 topic echo /fc_network/vehicle/sys3/battery
 
 
 /home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7
 
 mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560
 
-
 ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}"
 ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}"
 
 ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}"
+ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, timeout_sec: 2}"
 
-MAV_CMD_DO_SET_MODE (176)
+ros2 service call /fc_network/vehicle/pos_global_int fc_interfaces/srv/MavPositionTargetGlobalInt "{target_sysid: 3, target_compid: 1, coordinate_frame: 6, type_mask: 3576, lat_int: -35376655, lon_int: 149157011, alt: 20.0, timeout_sec: 5.0}"
 
 
 sudo tcpdump -i lo 'udp dst port 14561' -X
 sudo tcpdump -i lo 'udp dst port 14550' -X -vv
-sudo tcpdump -i lo -X udp port 14550
+sudo tcpdump -i lo -X udp port 14550
+
+幾個要討論的
+1. 專案文件中 .mat 檔案是幹嘛的?
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
+3. readme 那串文字來源? 用途?
+4. GUI介面 加一下海拔高 跟 相對高 速度跟位置 變更為 XY速度 XY位置
+5. pitch yaw roll 出來了 弄一下
+6.