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

(tested)

Browse Source 
1. 新增 ros2 接收 SET_POSITION_TARGET_LOCAL_NED 功能, 因應新增 MavPositionTargetGlobalInt.srv 的介面定義檔, 並相應新增使用者的外層包裝 navigation.py
2. 改善 ros2 service 的 response ack 數值對應, 新增 ackEnum.py
3. 改善 longCommand.py 內的呼叫層次, 把 longCommand 相關整合到同樣的 class 下, 僅以 def function 區分內部套用那種 MAV_CMD
4. 新增 example_position_goto.py, example_takeoff_land.py 作為使用範例 ( land.py arm_disarm.py ... 這些下個版本會被刪除)
5. 剩下就是一些註解的修正與更新
This commit is contained in:
chiyu1468
2026-04-07 12:28:11 +08:00
parent 44d53f51fb
commit eb2f0af58e
11 changed files with 848 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/ackEnum.py
+31
View File
@@ -0,0 +1,31 @@
'''
單獨出來定義回傳碼的意義
若跟飛控韌體有正常的通訊 (無論是否被拒絕) 則以 mavlink 定義的結果回傳
若跟飛控韌體無法通訊 才使用這邊的錯誤代碼
所以要避開重疊的號碼
這邊備註 Mavlink 的 MAV_RESULT 參數意義
0 : 封包正常接受並執行
1 : 封包正常接受 但是正在忙不執行 需要等待後重試
4 : 封包正常接受 但是載具的狀態異常 不執行
2 : 封包不正常 參數可能給錯了
3 : 封包不正常 根本認不出是啥
'''
from enum import IntEnum
class serviceAckResult(IntEnum):
MAVLINK_TIMEOUT = 51 # mavlink communication timeout
MAVLINK_SEND_BUT_NO_EXP_STEAM = 30 # some messages will not have obviously ack, so monitering feature message. however, we do NOT get the feature message. ex. 85/86 pair
MAVLINK_BUSY = 53 # there already has a command being execution for this sysid, try later
MAVLINK_DEV_NOTFOUND = 54 # this sysid not fount
UNKNOWN = 90 # i did not capture this error, try read the logs
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+1 -1
View File
@@ -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.60"
VERSION_NO = "v0.61"
class PanelState:
def __init__(self):
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+139 -19
View File
@@ -1,10 +1,15 @@
"""
MAVLink ROS2 Nodes
包含兩個獨立的 ROS2 Node：
主要包含兩個獨立的 ROS2 Node :
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
以最基礎的 mavlink microservice 會實現目標
並不會包含額外的功能
與一個節點管理器
- fc_ros_manager
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
"""
import os
@@ -26,6 +31,7 @@ import mavros_msgs.msg
# ROS2 Service imports
import fc_interfaces.srv as fcsrv
from .ackEnum import serviceAckResult
# 自定義 imports
from . import mavlinkVehicleView as mvv
@@ -521,6 +527,15 @@ class MavlinkCommandService(Node):
self.handle_command_long
)
# ═══════════════════════════════════════════════════════════════════
# Service : 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
# ═══════════════════════════════════════════════════════════════════
self.srv_pos_global_int = self.create_service(
fcsrv.MavPositionTargetGlobalInt,
self.serviceString_prefix + '/pos_global_int',
self.handle_set_position_target_global_int
)
logger.info("MavlinkCommandService initialized")
def _index(self, target_sysid, target_compid):
@@ -568,6 +583,7 @@ class MavlinkCommandService(Node):
def handle_mav_timesync_ping(self, request, response):
'''
用 timesync 封包驗證來回的時間
隸屬於 MavLink PING Protocol
'''
# 設定失效回應
response.success = False
@@ -612,7 +628,7 @@ class MavlinkCommandService(Node):
fail_skip = False
# 5) 等待回應封包
if not evt.wait(timeout_sec):
response.message = "waiting Timeout - TIMESYNC"
response.message = "mavlink timeout - TIMESYNC"
fail_skip = True
# 6) 處理回應封包
@@ -633,35 +649,31 @@ class MavlinkCommandService(Node):
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)
return response
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 COMMAND_LONG
# Service Handler: 發送 COMMAND_LONG (76)
# ═══════════════════════════════════════════════════════════════════════
def handle_command_long(self, request, response):
'''
用 command long 丟出 MAV_CMD
隸屬於 MavLink Command Protocol
'''
# 設定失效回應
response.success = False
response.message = "Unknown error"
response.ack_result = serviceAckResult.UNKNOWN
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"
response.ack_result = serviceAckResult.MAVLINK_BUSY
return response
# 2) 找到 socket 標地
socketObject = self._index(request.target_sysid, request.target_compid)
if socketObject is None:
response.message = "This system id not found."
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
return response
# 3) 接收封包系統 的設定
@@ -691,7 +703,8 @@ class MavlinkCommandService(Node):
fail_skip = False
# 5) 等待回應封包
if not evt.wait(timeout_sec):
response.message = "waiting Timeout - CommLONG"
response.message = "mavlink timeout - CommLONG"
response.ack_result = serviceAckResult.MAVLINK_TIMEOUT
fail_skip = True
# 6) 處理回應封包
@@ -711,7 +724,96 @@ class MavlinkCommandService(Node):
return response
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 SET_POSITION_TARGET_GLOBAL_INT (86)
# ═══════════════════════════════════════════════════════════════════════
def handle_set_position_target_global_int(self, request, response):
'''
隸屬於 MavLink Offboard Control Protocol
'''
# 設定失效回應
response.message = "Unknown error"
response.ack_result = serviceAckResult.UNKNOWN
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"
response.ack_result = serviceAckResult.MAVLINK_BUSY
return response
# 2) 找到 socket 標地
socketObject = self._index(request.target_sysid, request.target_compid)
if socketObject is None:
response.message = "This system id not found."
response.ack_result = serviceAckResult.MAVLINK_DEV_NOTFOUND
return response
# 3) 接收封包系統 的設定
# 在 socket 那邊先把要的封包種類導流進來
expect_recieve_msg_id = 87 # POSITION_TARGET_GLOBAL_INT
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
# 4) 送出封包
socketObject.MAVLink.set_position_target_global_int_send(
request.time_boot_ms,
request.target_sysid,
request.target_compid,
request.coordinate_frame,
request.type_mask,
request.lat_int, request.lon_int, request.alt,
request.vx, request.vy, request.vz,
request.afx, request.afy, request.afz,
request.yaw, request.yaw_rate
)
time.sleep(0.01) # 因應 mavlink 的廣播式回應 後置回應監聽 並且故意延遲10ms 讓系統代謝掉舊的87封包
_pending = PendingEntry(event = evt, match_fn = match_fn)
self._pending_by_sysid[request.target_sysid] = _pending
fail_skip = False
# 5) 等待回應封包
if not evt.wait(timeout_sec):
response.message = "mavlink timeout - SET POSITION 86"
response.ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
fail_skip = True
# 6) 處理回應封包
if not fail_skip:
ack_msg = _pending.result_mav_msg
response.message = ""
response.ack_result = 0
response.r_time_boot_ms = ack_msg.time_boot_ms
response.r_type_mask = ack_msg.type_mask
response.r_lat_int = ack_msg.lat_int
response.r_lon_int = ack_msg.lon_int
response.r_alt = ack_msg.alt
response.r_vx = ack_msg.vx
response.r_vy = ack_msg.vy
response.r_vz = ack_msg.vz
response.r_afx = ack_msg.afx
response.r_afy = ack_msg.afy
response.r_afz = ack_msg.afz
response.r_yaw = ack_msg.yaw
response.r_yaw_rate = ack_msg.yaw_rate
# 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
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 參數請求
# ═══════════════════════════════════════════════════════════════════════
@@ -790,7 +892,21 @@ class MavlinkCommandService(Node):
response.success = True
response.message = "Param request sent (placeholder implementation)"
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)
return response
def stop(self):
"""停止服務"""
self.running = False
@@ -973,7 +1089,11 @@ ros2_manager = fc_ros_manager()
5. 預留 MavlinkCommandService 結構（稍後實現）
2026.03.27
1. 完成 ros2 service 結構與 timesync command_long 協議
1. 完成 ros2 service 結構與 timesync 與 command_long 協議
2026.04.07
1. 完成 ros2 的 MavPositionTargetGlobalInt 區域
2. 優化 response.ack_result 回傳值的資訊
'''