(tested) 完成 ros2 service 結構與 timesync command_long 協議

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -351,7 +351,7 @@ class mavlink_object:
             self.last_data = b''
         def write(self, data):
             self.last_data = bytes(data)
-            logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug
+            # logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug
             # 把收到的資料 bytes 放到 target_deque
             self.target_deque.append(data)
         def seek(self, pos): pass

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -462,14 +462,6 @@ class PendingEntry():
         self.event = event # 只會在外迴圈 set()
         self.result_mav_msg = None #只會在外迴圈 被寫入
         self.error = ""
-        
-
-    # def match_fn(self, compare_msg):
-    #     # 驗證是不是期待的封包
-    #     if self.match_msgid == compare_msg.get_msgId():
-    #         return True
-    #     else:
-    #         return False
 
 class MavlinkCommandService(Node):
     """
@@ -501,32 +493,6 @@ class MavlinkCommandService(Node):
         # pending 旗標物件的儲存庫
         self._pending_by_sysid = {} # sysid(int) : PendingEntry
 
-        # ═══════════════════════════════════════════════════════════════════
-        # 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)  # 同步調用
-        # ═══════════════════════════════════════════════════════════════════
-
         # ═══════════════════════════════════════════════════════════════════
         # Service : ADD_TWO 測試用，未來刪除
         # ═══════════════════════════════════════════════════════════════════
@@ -549,11 +515,11 @@ class MavlinkCommandService(Node):
         # ═══════════════════════════════════════════════════════════════════
         # Service : 發送 COMMAND_LONG
         # ═══════════════════════════════════════════════════════════════════
-        # self.srv_command_long = self.create_service(
-        #     fcsrv.MavCommandLong,
-        #     self.serviceString_prefix + '/send_command_long',
-        #     self.handle_command_long
-        # )
+        self.srv_command_long = self.create_service(
+            fcsrv.MavCommandLong,
+            self.serviceString_prefix + '/send_command_long',
+            self.handle_command_long
+        )
 
         logger.info("MavlinkCommandService initialized")
 
@@ -603,15 +569,11 @@ class MavlinkCommandService(Node):
         '''
         用 timesync 封包驗證來回的時間
         '''
-        fail_skip = False
-
         # 設定失效回應
         response.success = False
         response.message = "Unknown error"
-        
         response.rtt_ms = 0.0
         timeout_sec = 2.0
-        expect_recieve_msg_id = 111 # TIMESYNC
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
         if request.target_sysid in self._pending_by_sysid:
@@ -626,6 +588,7 @@ class MavlinkCommandService(Node):
 
         # 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()
         # 設定封包檢驗 
@@ -646,23 +609,19 @@ class MavlinkCommandService(Node):
         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"
-            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
+            fail_skip = True
 
         # 6) 處理回應封包
-        ack_msg = _pending.result_mav_msg
+        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 = ""
+            current_time = int(time.monotonic() * 1e6)
+            response.rtt_ms = (current_time - now_us) / 1e3
+            response.message = ""
 
         # 7) 接收封包系統 的重置
         msg_types = list(socketObject.return_msg_types)
@@ -671,6 +630,7 @@ class MavlinkCommandService(Node):
         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):
@@ -681,29 +641,16 @@ class MavlinkCommandService(Node):
             f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}"
         )
         time.sleep(1)
+
         return response
 
-
-    # # 感覺用不到
-    # 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: 發送 COMMAND_LONG
     # ═══════════════════════════════════════════════════════════════════════
     def handle_command_long(self, request, response):
         # 設定失效回應
         response.success = False
-        response.ack_result = 255
-        # 等待回應 timeout
+        response.message = "Unknown error"
         timeout_sec = request.timeout_sec
 
         # 1) 確認是否已經有相同 sysid 的其他需求正在 pending
@@ -718,7 +665,7 @@ class MavlinkCommandService(Node):
             return response
 
         # 3) 接收封包系統 的設定
-        expect_recieve_msg_id = mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK # 77
+        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()
 
@@ -741,15 +688,18 @@ class MavlinkCommandService(Node):
             request.param5, request.param6, request.param7
         )
 
+        fail_skip = False
         # 5) 等待回應封包
         if not evt.wait(timeout_sec):
             response.message = "waiting Timeout - CommLONG"
-            del evt
-            del self._pending_by_sysid[request.target_sysid]
-            return response
+            fail_skip = True
 
         # 6) 處理回應封包
-        ack_msg = _pending.result_mav_msg
+        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)
@@ -757,106 +707,10 @@ class MavlinkCommandService(Node):
             msg_types.remove(expect_recieve_msg_id)
         socketObject.set_return_message_types(msg_types)
         del evt
-        del self._pending_by_sysid[request.target_sysid]
+        self._pending_by_sysid.pop(request.target_sysid, None)
+
         return response
-        # target_sysid = request.target_sysid
-        # target_compid = request.target_compid
-        # timeout_sec = request.timeout_sec if request.timeout_sec > 0 else 1.0
 
-        # # 1) 找 vehicle/socket/mav_obj
-        # vehicle = mvv.vehicle_registry.get(target_sysid)
-
-        # if not vehicle:
-        #     response.success = False
-        #     response.ack_result = 255
-        #     response.message = f"Vehicle {target_sysid} not found"
-        #     return response
-        # socket_id = vehicle.custom_meta.get("socket_id")
-        # mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
-
-        # if mav_obj is None:
-        #     response.success = False
-        #     response.ack_result = 255
-        #     response.message = "mavlink_object not found"
-        #     return response
-
-        # # 2) 設定回應型別（至少含 COMMAND_ACK=77）
-        # mav_obj.set_return_message_types([mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK])
-
-        # # 3) 每機最多一筆 pending：busy 就直接回錯
-        # evt = threading.Event()
-        # pending = PendingEntry(
-        #     event=evt,
-        #     deadline_monotonic=time.monotonic() + timeout_sec,
-
-        #     result_msg=None,
-        #     error=""
-        # )
-
-        # def _match_ack(msg):
-        #     return (
-        #         msg.get_msgId() == mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK and
-        #         msg.command == request.command
-        #     )
-
-        # pending.match_fn = _match_ack
-
-        # with self._pending_lock:
-        #     if target_sysid in self._pending_by_sysid:
-        #         response.success = False
-        #         response.ack_result = 255
-        #         response.message = f"sysid {target_sysid} already has pending request"
-        #         return response
-        #     self._pending_by_sysid[target_sysid] = pending
-
-        # try:
-        #     # 4) 組封包
-        #     mav_obj.MAVLink.command_long_send(
-        #         target_sysid,
-        #         target_compid,
-        #         request.command,
-        #         request.confirmation,
-        #         request.param1, request.param2, request.param3, request.param4,
-        #         request.param5, request.param6, request.param7
-        #     )
-
-        #     if not mav_obj.outgoing_msgs:
-        #         raise RuntimeError("No encoded command_long bytes")
-        #     message_bytes = mav_obj.outgoing_msgs.popleft()
-
-        #     # 5) 發送
-        #     ok = mo.mavlink_bridge().send_message(message_bytes, target_sysid=target_sysid)
-        #     if not ok:
-        #         raise RuntimeError("send_message failed")
-
-        #     # 6) 等 Router 通知（不是掃 ring）
-        #     if not evt.wait(timeout_sec):
-        #         response.success = False
-        #         response.ack_result = 255
-        #         response.message = "Timeout waiting COMMAND_ACK"
-        #         return response
-        #     if pending.result_msg is None:
-        #         response.success = False
-        #         response.ack_result = 255
-        #         response.message = pending.error or "No ACK message"
-        #         return response
-
-        #     ack = pending.result_msg
-        #     response.ack_result = ack.result
-        #     response.success = (ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED)
-        #     response.message = f"COMMAND_ACK result={ack.result}"
-        #     return response
-
-        # except Exception as e:
-        #     response.success = False
-        #     response.ack_result = 255
-        #     response.message = str(e)
-        #     return response
-
-        # finally:
-        #     with self._pending_lock:
-        #         # 防止異常路徑殘留 pending
-        #         self._pending_by_sysid.pop(target_sysid, None)
     
     # ═══════════════════════════════════════════════════════════════════════
     # Service Handler: 參數請求
@@ -1118,5 +972,8 @@ ros2_manager = fc_ros_manager()
 4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
 5. 預留 MavlinkCommandService 結構（稍後實現）
 
+2026.03.27
+1. 完成 ros2 service 結構與 timesync command_long 協議
+
 '''