chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
chiyu
Chiyu Chen 1 month ago
parent 5374e2b9d9
commit 60d6eba8cd
2 changed files with 31 additions and 174 deletions
Show Stats Download Patch File Download Diff File

2
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

@ -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

203
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
Unescape Escape View File

@ -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 協議
'''

Write Preview
Loading…
Cancel
Save