.gitignore

@@ -25,4 +25,3 @@ Makefile
 **/*.class
 **/*.pyc
 **/*.pyo
-**/.cursor/

README.md

@@ -21,12 +21,6 @@ ROS2
 1. source ~/ros2_humble/install/setup.bash
 2.
 
-===
-功能簡介
-1. mavlink 多對多支援平台
-2. 不允許進到 ros 系統有相同 sysid
-3. 假設所有 component 共用同一 socket
-
 ===
 開發用輔助專案
 1. Gazebo Garden
@@ -47,8 +41,6 @@ git submodule update
 # 2. build 需要的 package
 colcon build --packages-select angles geographic_msgs
 colcon build --packages-select mavros_msgs # 這個依賴前面的
-colcon build --packages-select fc_interfaces # 自己定義的
-
 ```
 
 

src/fc_interfaces/CMakeLists.txt

@@ -1,17 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(fc_interfaces)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "srv/MavPing.srv"
-  "srv/MavCommandLong.srv"
-  )
-
-ament_package()

src/fc_interfaces/package.xml

@@ -1,21 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>fc_interfaces</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>rosidl_default_generators</build_depend>
-  <member_of_group>rosidl_interface_packages</member_of_group>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/fc_interfaces/srv/MavCommandLong.srv

@@ -1,18 +0,0 @@
-# Request
-uint8 target_sysid
-uint8 target_compid
-uint16 command
-uint8 confirmation
-float32 param1
-float32 param2
-float32 param3
-float32 param4
-float32 param5
-float32 param6
-float32 param7
-float32 timeout_sec
----
-# Response
-bool success
-string message
-uint8 ack_result

src/fc_interfaces/srv/MavPing.srv

@@ -1,9 +0,0 @@
-# Request
-uint8 target_sysid
-uint8 target_compid
-uint8 ping_seq
----
-# Response
-bool success
-string message
-float32 rtt_ms

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -1138,7 +1138,8 @@ class Orchestrator:
 
         # === 4) ros 部分的準備 ===
         self.fc_ros_manager = mros.ros2_manager
-        self.fc_ros_manager.initialize()
+        if not self.fc_ros_manager.initialized:
+            self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
             'position': 1.0,   
             'attitude': 0.0,    
@@ -1310,7 +1311,7 @@ class Orchestrator:
 
             if self.fc_ros_manager.spin_thread.is_alive():
                 if self.fc_ros_manager.running:
-                    self.fc_ros_manager.shutdown()
+                    self.fc_ros_manager.stop()
                 self.fc_ros_manager.spin_thread.join(timeout=2)
 
             # 關閉面板執行緒

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -45,8 +45,6 @@ from collections import deque
 
 # mavlink 的 import
 from pymavlink import mavutil
-from pymavlink.dialects.v20 import common as mav_common
-from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot
 
 # 自定義的 import
 from .mavlinkVehicleView import (
@@ -113,7 +111,7 @@ class mavlink_bridge:
             30: self._handle_attitude,        # ATTITUDE
             32: self._handle_local_position,  # LOCAL_POSITION_NED
             33: self._handle_global_position, # GLOBAL_POSITION_INT
-            74: self._handle_vfr_hud,         # VFR_HUD
+            74: self._handle_vfr_hud,        # VFR_HUD
             147: self._handle_battery_status, # BATTERY_STATUS
         }
 
@@ -344,19 +342,6 @@ class mavlink_object:
     mavlinkObjects = {}  # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) }
     socket_num = 0  # 用來記錄目前的 socket 數量
 
-    # 用來銜接 pymavlink 的 MavLink 實例的管道
-    class CapturingPipeline:
-        def __init__(self, target_deque):
-            self.target_deque = target_deque
-            self.last_data = b''
-        def write(self, data):
-            self.last_data = bytes(data)
-            # logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug
-            # 把收到的資料 bytes 放到 target_deque
-            self.target_deque.append(data)
-        def seek(self, pos): pass
-        def tell(self): return 0
-
     def __new__(cls, *args, **kwargs):
         # 每創建一個實例 就將其添加到 mavlinkObjects 列表中
         # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 
@@ -374,20 +359,16 @@ class mavlink_object:
         cls.mavlinkObjects[socket_id] = instance
         return instance
 
-    def __init__(self, socket, socket_dialect = 'common'):
+    def __init__(self, socket):
         # 登入所需的 socket
         self.mavlink_socket = socket
 
         # 用於主線程發送消息的緩衝區
         self.outgoing_msgs = deque()
-        self.mavlinkPipeline = self.CapturingPipeline(self.outgoing_msgs)
-
-        if socket_dialect == 'common':
-            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.return_msg_types = set([])
+        self.return_msg_types = set()
 
         # 轉發到別的 mavlink object 作為目標端口 的列表
         self.target_sockets = set()

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -24,12 +24,8 @@ import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
 
-# ROS2 Service imports
-import fc_interfaces.srv as fcsrv
-
 # 自定義 imports
 from . import mavlinkVehicleView as mvv
-from . import mavlinkObject as mo
 from .utils import setup_logger
 
 logger = setup_logger(os.path.basename(__file__))
@@ -454,263 +450,195 @@ class VehicleStatusPublisher(Node):
 #                       MavlinkCommandService Node
 # ============================================================================
 
-class PendingEntry():
-    def __init__(self, event, match_fn):
-        # self.match_msgid = match_msgid # 只會在初始時設定
-        self.match_fn = match_fn
-
-        self.event = event # 只會在外迴圈 set()
-        self.result_mav_msg = None #只會在外迴圈 被寫入
-        self.error = ""
-
 class MavlinkCommandService(Node):
     """
     MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
     
     職責:
-    - 作為 service server 等待 client 請求
+    - 作為 service server，等待 client 請求
     - 接收請求，組裝 MAVLink 封包
     - 調用 mavlinkObject 發送封包
     - 處理 ACK 等待和超時（未來實現）
     
     設計理念：回歸 MAVLink 純粹結構
     - 只負責將 ROS2 請求轉換為 MAVLink 封包
-    - 不預設功能（如 ARM/DISARM) 保持模組化
+    - 不預設功能（如 ARM/DISARM），保持模組化
     - 高層應用可透過此 service 實現各種功能
     """
-
-    serviceString_prefix = '/fc_network/vehicle'
     
     def __init__(self):
         super().__init__('mavlink_command_service')
         
-        # 狀態標記
-        self.running = True
-        
-        # mavlinkObject 的引用（將由外部設置）
-        self.mavlink_analyzer = None
-
-        # pending 旗標物件的儲存庫
-        self._pending_by_sysid = {} # sysid(int) : PendingEntry
-
         # ═══════════════════════════════════════════════════════════════════
-        # Service : ADD_TWO 測試用，未來刪除
+        # ROS2 Service 架構說明：
+        # 
+        # 1. Service 定義：由 .srv 檔案定義（Request + Response）
+        #    - Request: client 發送的請求內容
+        #    - Response: server 回傳的結果
+        # 
+        # 2. Service Server 創建：
+        #    self.create_service(srv_type, service_name, callback_function)
+        #    - srv_type: service 的訊息類型（需要自定義或使用標準）
+        #    - service_name: service 的名稱（client 用此名稱呼叫）
+        #    - callback_function: 處理請求的回調函數
+        # 
+        # 3. Callback 函數：
+        #    def callback(self, request, response):
+        #        # request: 包含 client 發送的數據
+        #        # response: 需要填充並返回給 client
+        #        return response
+        # 
+        # 4. Service Client 使用方式（在其他程式中）：
+        #    client = node.create_client(srv_type, service_name)
+        #    request = srv_type.Request()
+        #    future = client.call_async(request)  # 異步調用
+        #    # 或 response = client.call(request)  # 同步調用
         # ═══════════════════════════════════════════════════════════════════
-        from example_interfaces.srv import AddTwoInts
-        self.srv_test = self.create_service(
-            AddTwoInts,
-            'mavlink/add_two_ints',
-            self.handle_add_two_ints
-        )
-
+        
+        # 由於 ROS2 自定義 service 需要 .srv 檔案編譯
+        # 這裡先使用標準 String service 作為簡化實現
+        # TODO: 未來可創建專門的 .srv 檔案
+        from std_srvs.srv import Trigger
+        from example_interfaces.srv import SetBool
+        
         # ═══════════════════════════════════════════════════════════════════
-        # Service : TIMESYNC 可以作為模板
+        # Service 1: 發送 MAVLink Message（通用介面）
+        # 使用 Trigger 作為臨時實現，未來應使用自定義 service
         # ═══════════════════════════════════════════════════════════════════
-        self.srv_mav_ping = self.create_service(
-            fcsrv.MavPing,
-            self.serviceString_prefix + '/mav_ping',
-            self.handle_mav_timesync_ping
+        # TODO: 創建 SendMavlinkMessage.srv
+        # Request:
+        #   uint8 target_sysid
+        #   uint8 target_compid
+        #   uint16 message_id
+        #   string fields_json  # JSON 格式的字段數據
+        #   bool wait_response
+        #   uint16 response_msgid
+        #   float32 timeout
+        # Response:
+        #   bool success
+        #   string response_json
+        #   string error_message
+        
+        # 暫時使用簡化版本（僅示範架構）
+        self.srv_send_message = self.create_service(
+            Trigger,
+            '/mavlink/send_message',
+            self.handle_send_message
         )
-
+        
         # ═══════════════════════════════════════════════════════════════════
-        # Service : 發送 COMMAND_LONG
+        # Service 2: 發送 COMMAND_LONG
         # ═══════════════════════════════════════════════════════════════════
         self.srv_command_long = self.create_service(
-            fcsrv.MavCommandLong,
-            self.serviceString_prefix + '/send_command_long',
+            Trigger,
+            '/mavlink/send_command_long',
             self.handle_command_long
         )
-
+        
+        # ═══════════════════════════════════════════════════════════════════
+        # Service 3: 參數請求
+        # ═══════════════════════════════════════════════════════════════════
+        self.srv_param_request = self.create_service(
+            Trigger,
+            '/mavlink/param_request',
+            self.handle_param_request
+        )
+        
+        # 狀態標記
+        self.running = True
+        
+        # mavlinkObject 的引用（將由外部設置）
+        self.mavlink_analyzer = None
+        
         logger.info("MavlinkCommandService initialized")
-
-    def _index(self, target_sysid, target_compid):
-
-        # 找到對應的 vehicle
-        vehicle = mvv.vehicle_registry.get(target_sysid)
-        if not vehicle:
-            return None
-
-        socket_id = vehicle.custom_meta.get("socket_id")
-        if socket_id is None:
-            return None
-
-        # 提取主要的 socket_id
-        mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
-        if mav_obj is None:
-            return None
-        return mav_obj
-
-    def return_router(self):
-        '''
-        這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包
-        分送到各自的 pending 中 
-        藉由 event.set() 解開 service 中的 block 
-        '''
-        return_tuple = mo.return_packet_ring.get()
-        # 確認 return_packet_ring 有資料
-        if return_tuple == None: return
-
-        socketid, timestamp, msg = return_tuple
-        sysid = msg.get_srcSystem()
-
-        _pending = self._pending_by_sysid.get(sysid)
-        # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
-        if _pending == None:
-            return
-
-        if _pending.match_fn(msg):
-            _pending.result_mav_msg = msg
-            _pending.event.set()
-
+    
+    def set_mavlink_analyzer(self, mavlink_analyzer):
+        """
+        設置 mavlink_analyzer 引用
+        
+        Args:
+            mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例
+        """
+        self.mavlink_analyzer = mavlink_analyzer
+        logger.info("MavlinkCommandService: mavlink_analyzer set")
+    
     # ═══════════════════════════════════════════════════════════════════════
-    # Service Handler: 發送 TIMESYNC 可以作為模板
+    # Service Handler: 發送 MAVLink Message
     # ═══════════════════════════════════════════════════════════════════════
-    def handle_mav_timesync_ping(self, request, response):
-        '''
-        用 timesync 封包驗證來回的時間
-        '''
-        # 設定失效回應
-        response.success = False
-        response.message = "Unknown error"
-        response.rtt_ms = 0.0
-        timeout_sec = 2.0
-
-        # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
-        if request.target_sysid in self._pending_by_sysid:
-            response.message = f"sysid {request.target_sysid} already has pending request"
-            return response
-
-        # 2) 找到 socket 標地
-        socketObject = self._index(request.target_sysid, request.target_compid)
-        if socketObject is None:
-            response.message = "This system id not found."
+    def handle_send_message(self, request, response):
+        """
+        處理發送 MAVLink 訊息的請求
+        
+        ROS2 Service Callback 說明：
+        - 此函數會在 client 調用 service 時被執行
+        - request: 包含 client 傳入的參數
+        - response: 需要填充結果並返回給 client
+        - 必須 return response
+        
+        Args:
+            request: Trigger.Request (暫時使用，未來改為自定義)
+            response: Trigger.Response
+            
+        Returns:
+            response: 填充後的回應
+        """
+        logger.info("Received send_message request")
+        
+        # 檢查 mavlink_analyzer 是否已設置
+        if self.mavlink_analyzer is None:
+            response.success = False
+            response.message = "Error: mavlink_analyzer not set"
+            logger.error(response.message)
             return response
-
-        # 3) 接收封包系統 的設定
-        # 在 socket 那邊先把要的封包種類導流進來
-        expect_recieve_msg_id = 111 # TIMESYNC
-        socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id])
-        evt = threading.Event()
-        # 設定封包檢驗 
-        # 這邊是設定 tc1 不為 0 
-        def match_fn(compare_msg):
-            if compare_msg.get_msgId() != expect_recieve_msg_id : 
-                return False
-            # logger.debug(f"mark A : {compare_msg.get_srcSystem()} ,{compare_msg.get_msgId()}, {compare_msg.tc1}, {compare_msg.ts1}")
-            if compare_msg.tc1 == 0 :
-                return False
-            return True
-
-        _pending = PendingEntry(event = evt, match_fn = match_fn)
-
-        self._pending_by_sysid[request.target_sysid] = _pending
-
-        # 4) 送出封包
-        now_us = int(time.monotonic() * 1e6)
-        socketObject.MAVLink.timesync_send(0, now_us)
-
-        fail_skip = False
-        # 5) 等待回應封包
-        if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - TIMESYNC"
-            fail_skip = True
-
-        # 6) 處理回應封包
-        if not fail_skip:
-            ack_msg = _pending.result_mav_msg
-
-            current_time = int(time.monotonic() * 1e6)
-            response.rtt_ms = (current_time - now_us) / 1e3
-            response.message = ""
-
-        # 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
-
-    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)
-
+        
+        # TODO: 實際實現
+        # 1. 從 request 解析參數（target_sysid, message_id, fields 等）
+        # 2. 使用 pymavlink 組裝 MAVLink 封包
+        # 3. 調用 mavlink_analyzer.send_message() 發送
+        # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應
+        
+        # 暫時返回成功（示範用）
+        response.success = True
+        response.message = "Message sent (placeholder implementation)"
         return response
-
+    
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 發送 COMMAND_LONG
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
-        # 設定失效回應
-        response.success = False
-        response.message = "Unknown error"
-        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"
-            return response
-
-        # 2) 找到 socket 標地
-        socketObject = self._index(request.target_sysid, request.target_compid)
-        if socketObject is None:
-            response.message = "This system id not found."
+        """
+        處理發送 COMMAND_LONG 的請求
+        
+        COMMAND_LONG (MAVLink message ID=76):
+        - 用於發送簡單命令給載具
+        - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等
+        
+        Args:
+            request: Trigger.Request
+            response: Trigger.Response
+            
+        Returns:
+            response: 填充後的回應
+        """
+        logger.info("Received command_long request")
+        
+        if self.mavlink_analyzer is None:
+            response.success = False
+            response.message = "Error: mavlink_analyzer not set"
             return response
-
-        # 3) 接收封包系統 的設定
-        expect_recieve_msg_id = 77 # mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK 
-        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
-
-        _pending = PendingEntry(event = evt, match_fn = match_fn)
-
-        self._pending_by_sysid[request.target_sysid] = _pending
-
-        # 4) 送出封包
-        socketObject.MAVLink.command_long_send(
-            request.target_sysid,
-            request.target_compid,
-            request.command,
-            request.confirmation,
-            request.param1, request.param2, request.param3, request.param4,
-            request.param5, request.param6, request.param7
-        )
-
-        fail_skip = False
-        # 5) 等待回應封包
-        if not evt.wait(timeout_sec):
-            response.message = "waiting Timeout - CommLONG"
-            fail_skip = True
-
-        # 6) 處理回應封包
-        if not fail_skip:
-            ack_msg = _pending.result_mav_msg
-            response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED
-            response.message = "" # 沒有消息就是好消息
-            response.ack_result = ack_msg.result
-
-        # 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)
-
+        
+        # TODO: 實際實現
+        # 1. 從 request 解析 COMMAND_LONG 參數
+        #    - target_sysid, target_compid
+        #    - command (MAV_CMD_xxx)
+        #    - param1~param7
+        # 2. 組裝 COMMAND_LONG 封包
+        # 3. 發送並等待 COMMAND_ACK (message ID=77)
+        # 4. 解析 ACK 結果（ACCEPTED/FAILED 等）
+        
+        response.success = True
+        response.message = "Command sent (placeholder implementation)"
         return response
-
     
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 參數請求
@@ -791,6 +719,12 @@ class MavlinkCommandService(Node):
         response.message = "Param request sent (placeholder implementation)"
         return response
     
+    # ═══════════════════════════════════════════════════════════════════════
+    # 【新增 Service 位置 4/4】
+    # 若要新增 service，請在此處添加新的 handler 方法
+    # 並在 __init__ 中創建對應的 service server
+    # ═══════════════════════════════════════════════════════════════════════
+    
     def stop(self):
         """停止服務"""
         self.running = False
@@ -805,11 +739,6 @@ class fc_ros_manager:
     """
     MAVLink ROS2 節點管理器
     
-    因為物件創建在 mavlinkROS2Nodes.py 中，所以會分為 __init__ 跟 initialize 兩個階段。
-
-    stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
-    shutdown 是完全關閉 ROS2 並銷毀節點實例。
-
     管理兩個獨立的 ROS2 Node：
     - VehicleStatusPublisher
     - MavlinkCommandService
@@ -821,7 +750,7 @@ class fc_ros_manager:
         self.initialized = False
         self.running = False
         
-        # 两个 node 實例
+        # 两个 node 实例
         self.status_publisher: Optional[VehicleStatusPublisher] = None
         self.command_service: Optional[MavlinkCommandService] = None
         
@@ -843,7 +772,7 @@ class fc_ros_manager:
             self.status_publisher = VehicleStatusPublisher()
             self.command_service = MavlinkCommandService()
     
-            # 創建執行者 MultiThreadedExecutor 並把 node 加入其中
+            # 創建執行者 MultiThreadedExecutor
             self.executor = MultiThreadedExecutor()
             self.executor.add_node(self.status_publisher)
             self.executor.add_node(self.command_service)
@@ -884,14 +813,12 @@ class fc_ros_manager:
             self.running = False
             return False
     
-    # 循環執行的地方
     def _spin_executor(self):
         """在 thread 中運行的 executor"""
         try:
             # logger.info("ROS2 executor spinning...")
             while self.running:
                 self.executor.spin_once(timeout_sec=0.1)
-                self.command_service.return_router()
         except Exception as e:
             logger.error(f"fc_ros_manager error in spinning executor: {e}")
     
@@ -972,8 +899,4 @@ ros2_manager = fc_ros_manager()
 4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
-2026.03.27
-1. 完成 ros2 service 結構與 timesync command_long 協議
-
 '''
-

src/fc_network_adapter/fc_network_adapter/serialManager.py

@@ -15,7 +15,6 @@ import time
 import threading
 import struct
 from enum import Enum, auto
-from abc import ABC, abstractmethod
 
 # # XBee 模組
 # from xbee.frame import APIFrame
@@ -28,105 +27,6 @@ from .utils import setup_logger
 logger = setup_logger(os.path.basename(__file__))
 
 # ====================== 分割線 =====================
-
-# 定義 serial 連線的模式
-class SerialMode(Enum):
-    """連接類型"""
-    STRAIGHT = auto()      # 原始數據直通
-    XBEEAPI2AT = auto()    # XBee API 模式
-    NOT_USE = auto()       # 不使用
-
-
-# ====================== Frame Processor 基類與實現 =====================
-
-class FrameProcessor(ABC):
-    """協議處理器基類"""
-    
-    def __init__(self):
-        self.buffer = bytearray()
-    
-    @abstractmethod
-    def process_incoming(self, data: bytes):
-        """
-        處理接收到的數據
-        返回：已完整解析的 payload 列表
-        """
-        pass
-    
-    @abstractmethod
-    def process_outgoing(self, data: bytes) -> bytes:
-        """
-        封裝要發送的數據
-        返回：封裝後的完整幀
-        """
-        pass
-
-
-class RawFrameProcessor(FrameProcessor):
-    """原始數據直通處理器"""
-    
-    def process_incoming(self, data: bytes):
-        """直接返回原始數據，不進行緩衝"""
-        return [data] if data else []
-    
-    def process_outgoing(self, data: bytes) -> bytes:
-        """直接返回原始數據，不進行封裝"""
-        return data
-
-
-class XBeeFrameProcessor(FrameProcessor):
-    """XBee API 協議處理器"""
-    
-    def process_incoming(self, data: bytes):
-        """處理 XBee API 幀並提取 payload"""
-        self.buffer.extend(data)
-        payloads = []
-        
-        while len(self.buffer) >= 3:
-            # 尋找幀頭
-            if self.buffer[0] != 0x7E:
-                self.buffer.pop(0)
-                continue
-            
-            # 讀取 payload 長度
-            length = (self.buffer[1] << 8) | self.buffer[2]
-            full_length = 3 + length + 1  # 起始符(1) + 長度(2) + payload + 校驗和(1)
-            
-            # 等待完整幀
-            if len(self.buffer) < full_length:
-                break
-            
-            # 提取完整 frame 並從緩衝區移除
-            frame = bytes(self.buffer[:full_length])
-            del self.buffer[:full_length]
-            
-            # 判斷 frame 類型並處理
-            frame_type = frame[3]
-            
-            if frame_type == 0x90:  # RX Packet
-                payload = XBeeFrameHandler.decapsulate_data(frame)
-                if payload:
-                    payloads.append(payload)
-            elif frame_type == 0x88:  # AT Response
-                # 可以在這裡處理 AT 指令回應
-                # response = XBeeFrameHandler.parse_at_command_response(frame)
-                # 目前忽略
-                pass
-            elif frame_type == 0x8B:  # Transmit Status
-                # 傳輸狀態，目前忽略
-                pass
-            else:
-                logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}")
-        
-        return payloads
-    
-    def process_outgoing(self, data: bytes) -> bytes:
-        """將數據封裝為 XBee API 傳輸幀"""
-        return XBeeFrameProcessor.encapsulate_data(data)
-
-
-# ====================== XBee Frame Handler =====================
-
 class XBeeFrameHandler:
     """XBee API Frame 處理器"""
     
@@ -155,7 +55,7 @@ class XBeeFrameHandler:
     
     @staticmethod
     def parse_receive_packet(frame: bytes) -> dict:
-        """解析 RX Packet (0x90) - 未來擴展用"""
+        # """解析 RX Packet (0x90) - 未來擴展用"""
         # if len(frame) < 15 or frame[3] != 0x90:
         #     return None
         
@@ -191,12 +91,34 @@ class XBeeFrameHandler:
 
     @staticmethod
     def decapsulate_data(data: bytes):
+        # 這裡可以根據需要進行數據解封裝
+
+        # XBee API 幀格式: 
+        # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節)
         
+        # 檢查幀起始符 (0x7E)
+        if not data or len(data) < 5 or data[0] != 0x7E:
+            return data
+            
         # 獲取數據長度 (不包括校驗和)
+        # length = (data[1] << 8) + data[2]
         length = (data[1] << 8) | data[2]
-
-        rf_data_start = 3 + 12
-        return data[rf_data_start:3 + length]
+        
+        # 檢查幀完整性
+        if len(data) < length + 4:  # 起始符 + 長度(2字節) + 數據 + 校驗和
+            return data
+            
+        # 提取API標識符和數據
+        frame_type = data[3]
+        # frame_data = data[4:4+length-1]  # 減1是因為API標識符已經算在長度中
+        
+        # 根據不同的幀類型進行處理
+        if frame_type == 0x90:  # 例如，這是"接收數據包"類型
+            rf_data_start = 3 + 12
+            return data[rf_data_start:3 + length]
+        else:
+            return None          
+        return data
 
 
 class ATCommandHandler:
@@ -215,7 +137,7 @@ class ATCommandHandler:
         """根據 AT 指令類型分派處理"""
         if not response or not response['is_ok']:
             if response:
-                logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
+                print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}")
             return
         
         command = response['command']
@@ -224,122 +146,189 @@ class ATCommandHandler:
         if handler:
             handler(response['data'])
         else:
-            logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
+            print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}")
     
     def _handle_rssi(self, data: bytes):
         """處理 DB (RSSI) 回應"""
-        # 未來可實現 RSSI 處理邏輯
-        pass
+        if not data:
+            return
+        
+        rssi_value = data[0]
+        now = time.time()
+        
+        # 檢查是否最近有收到 MAVLink
+        last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0)
+        if now - last_mavlink_time > 0.5:
+            print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+            return
+        
+        # 取得對應的 sysid
+        sysid = serial_to_sysid.get(self.serial_port)
+        if sysid is None:
+            print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+            return
+        
+        # 記錄 RSSI
+        rssi_history[sysid].append(-rssi_value)
+        time_history[sysid].append(now)
+        # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
     
     def _handle_serial_high(self, data: bytes):
-        """處理 SH (Serial Number High)"""
+        # """處理 SH (Serial Number High) - 範例"""
+        # if len(data) >= 4:
+        #     serial_high = int.from_bytes(data[:4], 'big')
+        #     print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}")
         pass
     
     def _handle_serial_low(self, data: bytes):
-        """處理 SL (Serial Number Low)"""
+        # """處理 SL (Serial Number Low) - 範例"""
+        # if len(data) >= 4:
+        #     serial_low = int.from_bytes(data[:4], 'big')
+        #     print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}")
         pass
 
+# ====================== 分割線 =====================
 
-# ====================== Serial Handler =====================
-
-class SerialHandler(asyncio.Protocol):
-    """asyncio.Protocol 用於處理 Serial 收發"""
-    
-    def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode):
-        self.udp_handler = udp_handler  # UDP 的傳輸物件
+class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發
+    def __init__(self, udp_handler, serial_port_str):
+        self.udp_handler = udp_handler # UDP 的傳輸物件
         self.serial_port_str = serial_port_str
-        self.serial_mode = serial_mode
-        self.transport = None  # Serial 自己的傳輸物件
-        
-        # 根據模式創建對應的 processor
-        self.processor = self._create_processor(serial_mode)
-        
-        # AT 指令處理器（僅 XBee 模式使用）
-        if serial_mode == SerialMode.XBEEAPI2AT:
-            self.at_handler = ATCommandHandler(serial_port_str)
-        else:
-            self.at_handler = None
-
-    def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor:
-        """工廠方法：根據模式創建處理器"""
-        if serial_mode == SerialMode.STRAIGHT:
-            return RawFrameProcessor()
-        elif serial_mode == SerialMode.XBEEAPI2AT:
-            return XBeeFrameProcessor()
-        else:
-            logger.warning(f"Unknown serial mode: {serial_mode}, using Raw")
-            return RawFrameProcessor()
+        self.at_handler = ATCommandHandler(serial_port_str)
+
+        self.buffer = bytearray()  # 用於緩存接收到的資料
+        self.transport = None # Serial 自己的傳輸物件
+        # self.first_data = True  # 標記是否為第一次收到資料
+        # self.has_processed = False # 測試模式用 處理數據旗標 # debug
 
     def connection_made(self, transport):
-        """連接建立時的回調"""
         self.transport = transport
         if hasattr(self.udp_handler, 'set_serial_handler'):
             self.udp_handler.set_serial_handler(self)
-        logger.debug(f"Serial port {self.serial_port_str} connected")
+        # logger.info(f"Serial port {self.serial_port_str} connected.") # debug
 
+    # Serial 收到資料的處理過程
     def data_received(self, data):
-        """Serial 收到資料的處理過程"""
-        # 使用 processor 處理接收到的數據
-        payloads = self.processor.process_incoming(data)
-        
-        # 將所有解析完成的 payload 轉發到 UDP
-        for payload in payloads:
-            self.udp_handler.transport.sendto(
-                payload,
-                (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)
-            )
+        # 1. 把收到的資料加入緩衝區
+        self.buffer.extend(data) 
 
+        # 2. 需要完整的 header 才能解析
+        while len(self.buffer) >= 3:
+            # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) 
+            if self.buffer[0] != 0x7E:
+                self.buffer.pop(0) # 如果不是就丟掉
+                continue
+
+            # 4. 讀取 payload 長度
+            length = (self.buffer[1] << 8) | self.buffer[2]
+            full_length = 3 + length + 1
 
-# ====================== UDP Handler =====================
+            # 5. 等待完整封包
+            if len(self.buffer) < full_length:
+                break
 
-class UDPHandler(asyncio.DatagramProtocol):
-    """asyncio.DatagramProtocol 用於處理 UDP 收發"""
+            # 6. 提取完整 frame 並從緩衝區移除
+            an_frame = self.buffer[:full_length]
+            del self.buffer[:full_length]
+
+            # 7. 判斷 frame 類型
+            frame_type = an_frame[3]
+            
+            if frame_type == 0x88:
+                # 處理 AT Command 回應
+                # response = XBeeFrameHandler.parse_at_command_response(an_frame)
+                # self.at_handler.handle_response(response)
+                pass
+                
+            elif frame_type == 0x90:
+                # Receive Packet (RX) payload 先解碼 
+                processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame))
+                # 轉換失敗就捨棄了
+                if processed_data is None: 
+                    continue
+                # 再透過 UDP 送出
+                self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port))
+
+            elif frame_type == 0x8B:
+                pass
+
+            else:
+                # 其他類型的 frame 未來可擴展處理 現在忽略
+                logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}")
+
+            # # RSSI
+            # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包
+            #                                              # frame[5:7] == b'DB' -> API 封包的DB參數
+            #     status = frame[7]                        # 
+            #     if status == 0x00 and len(frame) > 8:    # status == 0x00 -> 這個封包是有效封包
+            #         rssi_value = frame[8]
+            #         now = time.time()
+
+            #         # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port
+            #         last_time = serial_last_mavlink_time.get(self.serial_port, 0)
+            #         if now - last_time <= 0.5:
+            #             sysid = serial_to_sysid.get(self.serial_port, None)
+            #             if sysid is not None:
+            #                 rssi_history[sysid].append(-rssi_value)
+            #                 time_history[sysid].append(now)
+            #                 # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm")
+            #             else:
+            #                 print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略")
+            #         else:
+            #             print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略")
+            #     else:
+            #         print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}")
+        
+
+class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發
     
-    LOCAL_HOST_IP = '127.0.0.1'  # 只送給本地端 IP
+    LOCAL_HOST_IP = '127.0.0.1'  # 只送給本地端IP
+
+    def __init__(self, target_port):
+        self.target_port = target_port # 目標 UDP 端口
 
-    def __init__(self, target_port, serial_mode: SerialMode):
-        self.target_port = target_port  # 目標 UDP 端口
-        self.serial_mode = serial_mode
-        self.serial_handler = None  # Serial 的傳輸物件
-        self.transport = None  # UDP 自己的傳輸物件
+        self.serial_handler = None # Serial 的傳輸物件
+        self.transport = None # UDP 自己的傳輸物件
+        self.remote_addr = None  # 儲存動態獲取的遠程地址 # debug
+        # self.has_processed = False # 測試模式用 處理數據旗標 # debug
 
     def connection_made(self, transport):
-        """連接建立時的回調"""
         self.transport = transport
-        logger.debug(f"UDP transport ready for port {self.target_port}")
+        # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug
 
     def set_serial_handler(self, serial_handler):
-        """設置對應的 Serial Handler"""
         self.serial_handler = serial_handler
 
+    # UDP 收到資料的處理過程
     def datagram_received(self, data, addr):
-        """UDP 收到資料的處理過程"""
-        if not self.serial_handler:
-            logger.warning("Serial handler not set, dropping UDP packet")
-            return
-        
-        # 使用 processor 封裝數據
-        processed_data = self.serial_handler.processor.process_outgoing(data)
+        # 儲存對方的地址（這樣就能向同一個來源回傳數據）
+        # self.remote_addr = addr # debug
+        # print(f"Received UDP data from {addr}, setting as remote address")
         
-        # 發送到串口
-        self.serial_handler.transport.write(processed_data)
+        processed_data = XBeeFrameHandler.encapsulate_data(data)
 
+        if self.serial_handler:
+            self.serial_handler.transport.write(processed_data)
+    
+#==================================================================
+
+class SerialReceiverType(Enum):
+    """連接類型"""
+    TELEMETRY = auto() 
+    XBEEAPI2AT = auto() 
+    OTHER = auto() 
 
-# ====================== Serial Manager =====================
 
 class serial_manager:
-    """串口管理器"""
 
     class serial_object:
-        """串口物件"""
-        def __init__(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
-            self.serial_port = serial_port  # /dev/ttyUSB or COM3 ...etc
+        def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+            self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc
             self.baudrate = baudrate
-            self.serial_mode = serial_mode
-            self.target_port = target_port  # 指向的 UDP 端口
+            self.receiver_type = receiver_type
+            self.target_port = target_port # 指向的 UPD 端口
 
-            self.transport = None
-            self.protocol = None
+            self.transport = None # TODO 這個變數可能沒有作用
+            self.protocol = None # TODO 這個變數可能沒有作用
             self.udp_handler = None
             self.serial_handler = None
 
@@ -348,21 +337,21 @@ class serial_manager:
         self.loop = None
         self.running = False
         self.serial_count = 0
-        self.serial_objects = {}  # serial id num : serial_object
+        self.serial_objects = {} # serial id num : serial_object
 
     def __del__(self):
         self.loop = None
         self.thread = None
 
     def start(self):
-        """啟動 serial_manager"""
+
         if self.running:
             logger.warning("serial_manager already running")
-            return False
+            return
         
         self.running = True
 
-        # 啟動獨立線程，命名為 SerialManager
+        # 啟動獨立線程 命名為 SerialManager
         self.thread = threading.Thread(
             target=self._run_event_loop, 
             name="SerialManager"
@@ -386,6 +375,7 @@ class serial_manager:
 
     def shutdown(self):
         """停止 serial_manager 和其管理的所有 serial_object"""
+        # 自己在 running 狀態下才執行停止程序
         if not self.running:
             logger.warning("serial_manager is not running")
             return
@@ -414,25 +404,37 @@ class serial_manager:
         self.loop = asyncio.new_event_loop()
         asyncio.set_event_loop(self.loop)
 
+        # # 為每個 serial_object 建立連接
+        # for serial_obj in self.serial_objects:
+        #     coro = serial_asyncio.create_serial_connection(
+        #         self.loop,
+        #         lambda: SerialProtocol(serial_obj.receiver_type),
+        #         serial_obj.serial_port,
+        #         baudrate=serial_obj.baudrate
+        #     )
+        #     transport, protocol = self.loop.run_until_complete(coro)
+        #     serial_obj.transport = transport
+        #     serial_obj.protocol = protocol
+
         try:
             self.loop.run_forever()
         finally:
             self.loop.close()
 
-    def create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
-        """創建串口連接"""
+    def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
+
         if not self.running or not self.loop:
             logger.error("Event loop not running, cannot create serial link")
             return False
 
-        # 檢查 serial port 有效性
+        # 檢查 serial port 有效
         if not self.check_serial_port(serial_port, baudrate):
             logger.error(f"Serial port {serial_port} validation failed")
             return False
 
         # 使用 run_coroutine_threadsafe 執行協程並獲取結果
         future = asyncio.run_coroutine_threadsafe(
-            self._async_create_serial_link(serial_port, baudrate, target_port, serial_mode),
+            self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type),
             self.loop
         )
 
@@ -440,8 +442,8 @@ class serial_manager:
             # 等待結果，設定合理的超時時間
             result = future.result(timeout=5.0)
             if result:
-                logger.info(f"Create Serial Link: {serial_port} ({serial_mode.name}) -> UDP {target_port}")
-            return result
+                logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}")
+            return True
         except asyncio.TimeoutError:
             logger.error(f"Timeout creating serial link for {serial_port}")
             return False
@@ -449,28 +451,27 @@ class serial_manager:
             logger.error(f"Failed to create serial link for {serial_port}: {e}")
             return False
 
-    async def _async_create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode):
+    async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType):
         """在事件循環線程中執行實際的連接創建"""
         try:
             # 創建 serial_object 實例
-            serial_obj = self.serial_object(serial_port, baudrate, target_port, serial_mode)
+            serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type)
 
             # 建立 UDP 處理器並指定目標端口位置
-            serial_obj.udp_handler = UDPHandler(target_port, serial_mode)
+            serial_obj.udp_handler = UDPHandler(target_port)
             
             # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配
             udp_transport, udp_protocol = await self.loop.create_datagram_endpoint(
                 lambda: serial_obj.udp_handler,
                 local_addr=('0.0.0.0', 0)  # 使用端口 0 讓系統自動分配可用端口
             )
-
             serial_obj.transport = udp_transport
             serial_obj.protocol = udp_protocol
             
-            logger.debug(f"UDP endpoint created for {serial_port}")
+            # logger.info(f"UDP endpoint created for {serial_port}") # debug
 
-            # 建立 Serial 處理器，將 UDP 處理器和模式傳給它
-            serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port, serial_mode)
+            # 建立 Serial 處理器，將 UDP 處理器傳給它
+            serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port)
 
             # 建立 Serial 連接
             serial_transport, _ = await serial_asyncio.create_serial_connection(
@@ -480,14 +481,14 @@ class serial_manager:
                 baudrate=baudrate
             )
             
-            logger.debug(f"Serial connection created for {serial_port}")
+            # logger.info(f"Serial connection created for {serial_port}") # debug
 
             # 將 serial_object 加入管理列表
             serial_id = self.serial_count + 1
             self.serial_objects[serial_id] = serial_obj
             self.serial_count += 1
             
-            logger.debug(f"Serial object {serial_id} added to manager")
+            # logger.info(f"Serial object {serial_id} added to manager") # debug
             return True
             
         except Exception as e:
@@ -500,10 +501,12 @@ class serial_manager:
 
     def remove_serial_link(self, serial_id):
         """移除串口連接（線程安全方式）"""
+        # 確保事件循環正在運行
         if not self.loop:
             logger.error("Event loop not running")
             return False
         
+        # 檢查 serial_id 是否存在
         if serial_id not in self.serial_objects:
             logger.warning(f"Serial object {serial_id} not found")
             return False
@@ -546,7 +549,7 @@ class serial_manager:
             
             # 從管理列表中移除
             del self.serial_objects[serial_id]
-            logger.debug(f"Serial object {serial_id} removed from manager")
+            # logger.info(f"Serial object {serial_id} removed from manager") # debug
             return True
             
         except Exception as e:
@@ -554,8 +557,7 @@ class serial_manager:
             return False
 
     def get_serial_link(self):
-        """取得所有串口連接資訊"""
-        ret = {}  # serial id num : serial_port string
+        ret = {} # serial id num : serial_port string
         for key, obj in self.serial_objects.items():
             ret[key] = obj.serial_port
         return ret
@@ -591,8 +593,6 @@ class serial_manager:
             return False
 
 
-# ====================== 測試代碼 =====================
-
 if __name__ == '__main__':
     sm = serial_manager()
     sm.start()
@@ -600,17 +600,12 @@ if __name__ == '__main__':
     SERIAL_PORT = '/dev/ttyUSB0'  # 手動指定
     SERIAL_BAUDRATE = 115200
     UDP_REMOTE_PORT = 14571
-    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT)
-
-    # SERIAL_PORT = '/dev/ttyACM0'  # 手動指定
-    # SERIAL_BAUDRATE = 115200
-    # UDP_REMOTE_PORT = 14571
-    # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT)
+    sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT)
 
     linked_serial = sm.get_serial_link()
     print(linked_serial)
-    time.sleep(60)
+    time.sleep(10)
 
     sm.remove_serial_link(1)
     time.sleep(3)
-    sm.shutdown()
+    sm.shutdown()

src/fc_network_adapter/tests/example_mavlink_client.py

@@ -1,180 +0,0 @@
-"""
-MavlinkCommandService 使用範例
-
-展示如何從其他 ROS2 節點調用 MAVLink 指令服務
-"""
-
-import rclpy
-from rclpy.node import Node
-from std_srvs.srv import Trigger
-import time
-
-
-class MavlinkClientExample(Node):
-    """
-    範例：MAVLink Service Client
-    
-    這個節點展示如何調用 MavlinkCommandService 提供的服務
-    """
-    
-    def __init__(self):
-        super().__init__('mavlink_client_example')
-        
-        # 創建 service client
-        self.client = self.create_client(
-            Trigger,
-            '/mavlink/send_command_long'
-        )
-        
-        # 等待服務可用
-        self.get_logger().info('等待 service 可用...')
-        self.client.wait_for_service()
-        self.get_logger().info('Service 已連接！')
-    
-    def send_arm_command(self):
-        """
-        範例：發送 ARM 指令
-        
-        實際使用時應該發送：
-        - message_id = 76 (COMMAND_LONG)
-        - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM)
-        - param1 = 1 (ARM)
-        """
-        self.get_logger().info('發送 ARM 指令...')
-        
-        request = Trigger.Request()
-        # TODO: 實際使用時應該是自定義的 SendCommandLong.Request
-        # request.target_sysid = 1
-        # request.target_compid = 1
-        # request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
-        # request.param1 = 1.0   # 1 = ARM, 0 = DISARM
-        # request.param2 = 0.0
-        # ... param3-7
-        # request.timeout = 3.0
-        
-        # 異步調用
-        future = self.client.call_async(request)
-        
-        # 等待結果
-        rclpy.spin_until_future_complete(self, future)
-        
-        if future.done():
-            response = future.result()
-            if response.success:
-                self.get_logger().info('✓ ARM 指令發送成功')
-            else:
-                self.get_logger().error(f'✗ ARM 指令失敗: {response.message}')
-        else:
-            self.get_logger().error('✗ Service 調用超時')
-
-
-def main():
-    """主函數"""
-    rclpy.init()
-    
-    # 創建客戶端節點
-    client = MavlinkClientExample()
-    
-    # 發送指令
-    client.send_arm_command()
-    
-    # 清理
-    client.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
-
-
-"""
-═══════════════════════════════════════════════════════════════════════════
-進階使用範例：高層應用程式
-
-假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具：
-
-```python
-class MissionController(Node):
-    def __init__(self):
-        super().__init__('mission_controller')
-        
-        # 創建各種 service clients
-        self.client_arm = self.create_client(
-            SendCommandLong, '/mavlink/send_command_long')
-        self.client_mode = self.create_client(
-            SendCommandLong, '/mavlink/send_command_long')
-        self.client_takeoff = self.create_client(
-            SendCommandLong, '/mavlink/send_command_long')
-        self.client_goto = self.create_client(
-            SendCommandInt, '/mavlink/send_command_int')
-    
-    def arm_vehicle(self, sysid=1):
-        \"\"\"解鎖載具\"\"\"
-        request = SendCommandLong.Request()
-        request.target_sysid = sysid
-        request.target_compid = 1
-        request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
-        request.param1 = 1.0   # ARM
-        
-        future = self.client_arm.call_async(request)
-        # 處理回應...
-    
-    def set_mode(self, sysid=1, mode='GUIDED'):
-        \"\"\"設置飛行模式\"\"\"
-        # 實現模式切換邏輯...
-        pass
-    
-    def takeoff(self, sysid=1, altitude=10.0):
-        \"\"\"起飛\"\"\"
-        request = SendCommandLong.Request()
-        request.target_sysid = sysid
-        request.target_compid = 1
-        request.command = 22  # MAV_CMD_NAV_TAKEOFF
-        request.param7 = altitude
-        
-        future = self.client_takeoff.call_async(request)
-        # 處理回應...
-    
-    def goto_position(self, sysid=1, lat=0, lon=0, alt=10):
-        \"\"\"前往指定位置\"\"\"
-        request = SendCommandInt.Request()
-        request.target_sysid = sysid
-        request.target_compid = 1
-        request.command = 192  # MAV_CMD_DO_REPOSITION
-        request.x = int(lat * 1e7)
-        request.y = int(lon * 1e7)
-        request.z = alt
-        
-        future = self.client_goto.call_async(request)
-        # 處理回應...
-    
-    def execute_mission(self):
-        \"\"\"執行完整任務\"\"\"
-        # 1. 解鎖
-        self.arm_vehicle()
-        time.sleep(1)
-        
-        # 2. 切換到 GUIDED 模式
-        self.set_mode(mode='GUIDED')
-        time.sleep(1)
-        
-        # 3. 起飛到 10 公尺
-        self.takeoff(altitude=10.0)
-        time.sleep(10)
-        
-        # 4. 前往目標點
-        self.goto_position(lat=25.033, lon=121.565, alt=10)
-        time.sleep(30)
-        
-        # 5. 返回並降落
-        # ...
-```
-
-這樣的設計讓高層應用可以：
-1. 完全不需要知道 MAVLink 協議細節
-2. 通過 ROS2 service 與載具互動
-3. 模組化開發不同功能
-4. 易於測試和維護
-
-═══════════════════════════════════════════════════════════════════════════
-"""

src/fc_network_apps/__init__.py

@@ -1,3 +0,0 @@
-from .changeMode import ChangeModeClient, ChangeModeResult, change_mode
-
-__all__ = ["ChangeModeClient", "ChangeModeResult", "change_mode"]

src/fc_network_apps/changeMode.py

@@ -1,127 +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
-
-
-class ChangeModeClient(Node):
-    """Small ROS2 client dedicated to change flight mode."""
-
-    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
-        super().__init__("fc_change_mode_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(
-        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:
-        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 = 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(
-                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,
-        )
-
-
-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[ChangeModeClient] = None
-    try:
-        node = ChangeModeClient(service_name=service_name)
-        if not node.wait_for_service(timeout_sec=timeout_sec):
-            return ChangeModeResult(
-                success=False,
-                message=f"Service not available: {service_name}",
-                ack_result=-1,
-            )
-        return node.change_mode(
-            target_sysid=target_sysid,
-            target_compid=target_compid,
-            custom_mode=custom_mode,
-            base_mode=base_mode,
-            confirmation=confirmation,
-            timeout_sec=timeout_sec,
-        )
-    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/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/unitdev02/unitdev02/devnote.txt

@@ -22,7 +22,6 @@
 自己的常用指令
 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
 
 ros2 topic list
 ros2 topic echo 
@@ -33,14 +32,4 @@ 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}"
 
-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}"
-
-MAV_CMD_DO_SET_MODE (176)
-
-
-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