From 60d6eba8cdd30749469832ca86a0366376a2fcba Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 27 Mar 2026 08:11:51 +0800 Subject: [PATCH] =?UTF-8?q?(tested)=20=E5=AE=8C=E6=88=90=20ros2=20service?= =?UTF-8?q?=20=E7=B5=90=E6=A7=8B=E8=88=87=20timesync=20command=5Flong=20?= =?UTF-8?q?=E5=8D=94=E8=AD=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mavlinkObject.py | 2 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 203 +++--------------- 2 files changed, 31 insertions(+), 174 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 5d25ff8..694913c 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/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 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index a13d677..cebcc1b 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/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,20 +641,8 @@ 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") - + return response # ═══════════════════════════════════════════════════════════════════════ # Service Handler: 發送 COMMAND_LONG @@ -702,8 +650,7 @@ class MavlinkCommandService(Node): 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 協議 + '''