1. (adding) mainOrchestrator 增加版本先驗程序

2. (adding) mavlinkVehicleView 增加 read_all 快速讀取
3. (modify) 優化 longCommand 與 navigation 使其更好引用
4. (adding) 提供一個完整的 example_wholeMoving 作為範例
4. (remove) 移除 fc_network_app 重複功能
5. (modify) 修改 overview_table 部分顯示文字

README.md

@@ -1,43 +1,46 @@
 這是天雷系統的專案
 
-===
-專案核心框架
-1. ROS2 Humble
-2. Python 3.8.10
 
 
 ===
-必要相依套件 順便記錄我開發時的環境版本
+## 功能簡介
-
-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
-
-
-ROS2
-1. source ~/ros2_humble/install/setup.bash
-2.
-
-===
-功能簡介
 1. mavlink 多對多支援平台
 2. 不允許進到 ros 系統有相同 sysid
 3. 假設一台載具上所有 component 共用同一 socket
 
 ===
-開發用輔助專案
+## 運行環境
+
+### 專案核心框架
+1. ROS2 Humble
+2. Python 3.8.10
+
+### 必要相依套件及版本
+- 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. 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,17 +52,29 @@ 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
 ```
 
+2. 
+GUI 介面
+在 ~/AirTrapMine/src/GUI 路徑下 直接啟動
+```bash
+cd ~/AirTrapMine/src/GUI
+python gui.py
+```
 
 ===
-Package 簡述
+## 資料夾說明
-<<<<<<< HEAD
-
-===
-簡述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)
@@ -77,19 +92,13 @@ Package 簡述
     使用者的外層包裝
 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/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_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.61"
+PROJECT_VER = "v0.61"
 
 class PanelState:
     def __init__(self):
@@ -304,7 +304,7 @@ class ControlPanel:
                 # 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()
 
@@ -1141,7 +1141,7 @@ class Orchestrator:
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
             'position': 1.0,
-            'attitude': 0.0,    
+            'attitude': 1.0,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,
@@ -1339,6 +1339,7 @@ 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']}"
@@ -1558,8 +1559,27 @@ class Orchestrator:
 
 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 +1613,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

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

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -39,6 +39,7 @@ from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
+MODULE_VER = "1.50"
 
 # ============================================================================
 #                       VehicleStatusPublisher Node
@@ -55,14 +56,16 @@ class PublishRateController:
         # 例如：'ekf_status': 1.0,  # EKF 狀態
         # ═══════════════════════════════════════════════════════════════
         # 各 topic 的發布間隔（秒）
+        # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
+        # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.5,       # GPS位置
+            'position': 0.0,       # GPS位置
-            'attitude': 0.5,       # 姿態
+            'attitude': 0.0,       # 姿態
-            'velocity': 0.5,       # 速度
+            'velocity': 0.0,       # 速度
-            'battery': 1.0,        # 電池
+            'battery': 0.0,        # 電池
-            'vfr_hud': 0.5,        # VFR HUD
+            'vfr_hud': 0.0,        # VFR HUD
-            'mode': 1.0,           # 飛行模式
+            'mode': 0.0,           # 飛行模式
-            'summary': 1.0,        # 載具摘要
+            'summary': 0.0,        # 載具摘要
             # 在這裡新增更多 topics...
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@@ -139,10 +142,8 @@ class VehicleStatusPublisher(Node):
         if not self.running:
             return
 
-        # 從 vehicle_registry 獲取所有載具
+        # 高頻快路徑：讀取 registry 的 immutable 快照
-        all_vehicles = mvv.vehicle_registry.get_all()
+        for sysid, vehicle in mvv.vehicle_registry.read_all():
-        
-        for sysid, vehicle in all_vehicles.items():
             self._publish_vehicle_status(vehicle)
     
     def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
@@ -232,7 +233,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:
@@ -257,6 +257,7 @@ class VehicleStatusPublisher(Node):
         
         publisher.publish(msg)
     
+    # TODO 這邊要改成 XY 的位置與速度
     def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
         """發布速度"""
         if not self.rate_controller.should_publish(sysid, 'velocity'):

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
@@ -426,9 +434,21 @@ class VehicleViewRegistry:
         """取得所有載具視圖"""
         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 .longCommand import CommandLongClient, CommandLongResult
-from .changeMode import change_mode
+from .navigation import PositionTargetGlobalIntClient, PositionTargetGlobalIntResult
-from .arm_disarm import arm_disarm
+
-from .takeoff import takeoff
-from .land import land
 
 __all__ = [
     "CommandLongClient",
-    "ChangeModeResult",
+    "PositionTargetGlobalIntClient",
-    "change_mode",
+    "CommandLongResult",
-    "arm_disarm",
+    "PositionTargetGlobalIntResult",
-    "takeoff",
-    "land",
 ]

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

@@ -25,10 +25,6 @@ class CommandLongResult:
     message: str
     ack_result: int
 
-
-ChangeModeResult = CommandLongResult
-
-
 class CommandLongClient(Node):
     """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 

src/fc_network_apps/navigation.py

@@ -99,7 +99,6 @@ def _echo_position_target_matches(
 @dataclass
 class PositionTargetGlobalIntResult:
     """對應 MavPositionTargetGlobalInt.srv 的 Response；success 依「送出與 r_* echo 是否一致」。"""
-
     success: bool
     message: str
     ack_result: int
@@ -117,7 +116,6 @@ class PositionTargetGlobalIntResult:
     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)，
@@ -230,51 +228,6 @@ class PositionTargetGlobalIntClient(Node):
             r_yaw_rate=float(resp.r_yaw_rate),
         )
 
-    # def set_position_target_global_int(
-    #     self,
-    #     *,
-    #     target_sysid: int,
-    #     target_compid: int = 0,
-    #     time_boot_ms: int = 0,
-    #     coordinate_frame: int = 6,
-    #     type_mask: int = 0,
-    #     lat_int: int = 0,
-    #     lon_int: int = 0,
-    #     alt: float = 0.0,
-    #     vx: float = 0.0,
-    #     vy: float = 0.0,
-    #     vz: float = 0.0,
-    #     afx: float = 0.0,
-    #     afy: float = 0.0,
-    #     afz: float = 0.0,
-    #     yaw: float = 0.0,
-    #     yaw_rate: float = 0.0,
-    #     timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    # ) -> PositionTargetGlobalIntResult:
-    #     """
-    #     送出 SET_POSITION_TARGET_GLOBAL_INT；coordinate_frame 預設 6 (MAV_FRAME_GLOBAL_INT)
-    #     可依飛控／任務覆寫。
-    #     """
-    #     return self._send_position_target_global_int(
-    #         target_sysid=target_sysid,
-    #         target_compid=target_compid,
-    #         time_boot_ms=time_boot_ms,
-    #         coordinate_frame=coordinate_frame,
-    #         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,
-    #         timeout_sec=timeout_sec,
-    #     )
-
     def goto_position_only(
         self,
         *,
@@ -307,47 +260,3 @@ class PositionTargetGlobalIntClient(Node):
             yaw_rate=0,
             timeout_sec=timeout_sec,
         )
-
-    # def set_position_target_global_int_deg(
-    #     self,
-    #     *,
-    #     target_sysid: int,
-    #     latitude_deg: float,
-    #     longitude_deg: float,
-    #     target_compid: int = 0,
-    #     time_boot_ms: int = 0,
-    #     coordinate_frame: int = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
-    #     type_mask: int = 0,
-    #     alt: float = 0.0,
-    #     vx: float = 0.0,
-    #     vy: float = 0.0,
-    #     vz: float = 0.0,
-    #     afx: float = 0.0,
-    #     afy: float = 0.0,
-    #     afz: float = 0.0,
-    #     yaw: float = 0.0,
-    #     yaw_rate: float = 0.0,
-    #     timeout_sec: float = DEFAULT_TIMEOUT_SEC,
-    # ) -> PositionTargetGlobalIntResult:
-    #     """以度為單位的緯經，轉成 MAVLink 的 lat_int / lon_int (1e7 * deg)。"""
-    #     lat_int = int(round(latitude_deg * 1e7))
-    #     lon_int = int(round(longitude_deg * 1e7))
-    #     return self.set_position_target_global_int(
-    #         target_sysid=target_sysid,
-    #         target_compid=target_compid,
-    #         time_boot_ms=time_boot_ms,
-    #         coordinate_frame=coordinate_frame,
-    #         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,
-    #         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_position_goto.py

@@ -1,48 +0,0 @@
-
-
-from __future__ import annotations
-
-from fc_network_apps.navigation import PositionTargetGlobalIntClient
-import time
-
-def main() -> None:
-    # 目標載具 system id（依你的連線修改）
-    target_sysid = 3
-
-    nav = PositionTargetGlobalIntClient()
-    if not nav.wait_for_service(timeout_sec=5.0):
-        print("Service /fc_network/vehicle/pos_global_int not available.")
-        return
-
-    result_deg = nav.goto_position_only(
-        target_sysid=target_sysid,
-        latitude_deg=-35.376655,
-        longitude_deg=149.157011,
-        alt=200.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)
-
-    result_deg = nav.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}")
-
-
-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_takeoff_land.py

@@ -1,64 +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=3.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    time.sleep(3)
-
-    result = commandAPI.arm_disarm(
-        target_sysid=3,
-        target_compid=0,
-        arm = True,
-    )
-
-    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.takeoff(
-        target_sysid=3,
-        target_compid=0,
-        altitude_m = 30.0,
-    )
-
-    print("=== change mode result ===")
-    print(f"success   : {result.success}")
-    print(f"ack_result: {result.ack_result}")
-    print(f"message   : {result.message}")
-
-    # time.sleep(20)
-
-    # result = commandAPI.land(
-    #     target_sysid=3,
-    #     target_compid=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_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

@@ -35,7 +35,6 @@ ros2 topic echo
 
 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}"
 
@@ -49,16 +48,7 @@ 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
 
-
+幾個要討論的
-colcon build --packages-select fc_interfaces
+1. 專案文件中 .mat 檔案是幹嘛的?
-
+2. GUI 的 print [Panel/Map Update] 11 Hz 希望關閉
-
+3. readme 那串文字來源? 用途?
-
--35.360150, 149.159659
--35.376655, 149.157011
-
-0b00 0000 00000 00000
-
-0b00 0011 01111 11000
-0b     11 01111 11000
-0b 1111 1101 1111 1000