wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

整合後項目修正

Browse Source 
1. mavlinkObject.py 統合 msg_id 參數名稱
2. mavlinkDevice.py 移除在預期操作下 仍會大量跳錯誤訊息的警示
3. devRun.py 重新驗證 merge 項目與版本調整
This commit is contained in:
chiyu1468
2025-05-17 16:33:33 +08:00
parent 4d82b92a11
commit a25372979b
4 changed files with 92 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+9 -5
View File
@@ -1,22 +1,26 @@
這是天雷系統的專案
===
專案核心環境
專案核心框架
1. ROS2 Humble
2. Python
3. Ardupilot
2. Python 3.8.10
===
必要相依套件
ROS2 預啟動
Python
1. pymavlink
2.
ROS2
1. source ~/ros2_humble/install/setup.bash
2.
===
建議的開發測試
開發用輔助專案
1. Gazebo Garden
2. Ardupilot
===
Package 簡述
src/fc_network_adapter/fc_network_adapter/devRun.py
+78 -10
View File
@@ -14,8 +14,8 @@ import mavlinkDevice as md
# ====================== 分割線 =====================
test_item = 22
running_time = 20
test_item = 51
running_time = 30
print('test_item : ', test_item)
if test_item == 1:
@@ -55,7 +55,7 @@ if test_item == 1:
ret = mavlink_object_none.updateMultiplexingList()
print('execute result : ', ret)
print('End of Program')
print('<=== End of Program')
elif test_item == 2:
# 測試 mavlink_object 創建時 socket_id 是否正確
@@ -73,7 +73,7 @@ elif test_item == 2:
print(mavlink_object_none1._multiplexingList)
print('End of Program')
print('<=== End of Program')
elif test_item == 10:
# 需要開啟一個 ardupilot 的模擬器
@@ -128,7 +128,7 @@ elif test_item == 10:
mavlink_object1.running = False
mavlink_object1.thread.join()
mavlink_socket.close()
print('End of Program')
print('<=== End of Program')
elif test_item == 11:
# 需要開啟一個 ardupilot 的模擬器
@@ -179,7 +179,7 @@ elif test_item == 11:
analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance
analyzer.thread.join()
mavlink_socket.close()
print('End of Program')
print('<=== End of Program')
elif test_item == 12:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
@@ -252,7 +252,7 @@ elif test_item == 12:
mavlink_socket_out.close()
analyzer.stop()
print('End of Program')
print('<=== End of Program')
elif test_item == 20:
# 這邊測試 node 生成 topic 的功能
@@ -308,7 +308,7 @@ elif test_item == 20:
rclpy.shutdown()
analyzer.stop()
analyzer.thread.join()
print('End of Program')
print('<=== End of Program')
elif test_item == 21:
# 需要開啟一個 ardupilot 的模擬器
@@ -370,7 +370,7 @@ elif test_item == 21:
analyzer.thread.join()
mavlink_socket.close()
print('End of Program')
print('<=== End of Program')
elif test_item == 22:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
@@ -389,6 +389,7 @@ elif test_item == 22:
connection_string_in="udp:127.0.0.1:15551"
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
connection_string_out="udpout:127.0.0.1:14553"
@@ -460,4 +461,71 @@ elif test_item == 22:
analyzer.thread.join()
print('End of Program')
print('<=== End of Program')
elif test_item == 51:
# 晉凱的測試項目
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 關於 Node 的初始化
show_time = time.time()
analyzer._init_node() # 初始化 node
print('初始化 node 完成 耗時 : ',time.time() - show_time)
# 創建通道
connection_string="udp:127.0.0.1:15551"
mavlink_socket3 = mavutil.mavlink_connection(connection_string)
mavlink_object3 = mo.mavlink_object(mavlink_socket3)
# 設定通道流動
mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
mavlink_object3.multiplexingToReturn = [] #
# mavlink_object3.multiplexingToSwap = [] #
# 啟動通道
mavlink_object3.run()
print('waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
compid = 1
sysid = 1
start_time = time.time()
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
end_time = time.time()
print(f"Execution time for create all topic: {end_time - start_time} seconds")
print("start emit info")
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
# rclpy.spin(analyzer)
analyzer.emit_info() # 這邊是測試 node 的運行
time.sleep(0.5)
except KeyboardInterrupt:
break
analyzer.destroy_node()
rclpy.shutdown()
# 結束程式 退出所有 thread
mavlink_object3.stop()
mavlink_object3.thread.join()
analyzer.stop()
analyzer.thread.join()
mavlink_socket3.close()
print('<=== End of Program')
src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py
+1 -3
View File
@@ -37,7 +37,7 @@ class mavlink_device():
last_seq = self.components[compid].msg_seq
except KeyError:
# 這個 component id 還不存在
logger.error('System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid))
# logger.error('System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid)) # 因為初始化的之前 會有大量非 heartbeat 的訊息進來 這是正常現象 TODO 之後要幫這個類別加上初始化狀態 再進行這個判斷
return
if last_seq != None:
@@ -88,6 +88,4 @@ class mavlink_device():
self.emitParams = {}
# 用來存放每個 topic 的 publisher
# 內容格式 為 {topic_name(字串) : [publisher(物件), method(函式)]} (?
# 內容格式 為 {publisher(物件) : method(函式)} (?
# 還在測試哪個比較好
self.publishers = {}
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
+4 -3
View File
@@ -117,6 +117,7 @@ class mavlink_bridge(Node, mavlink_publisher):
msg = msg_pack[2]
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
msg_id = msg.get_msgId()
# 若這個 system id 還不存在 則建立 device object
if not sysid in self.mavlink_systems:
@@ -132,7 +133,7 @@ class mavlink_bridge(Node, mavlink_publisher):
# 若該 component id 不存在 又不是 heartbeat 則不處理
if compid in self.mavlink_systems[sysid].components:
this_component = self.mavlink_systems[sysid].components[compid]
elif msg.get_msgId() == 0:
elif msg_id == 0:
# 只有透過 heartbeat 可以創建一個新的 component object
this_component = this_device.mavlink_component()
this_device.components[msg.get_srcComponent()] = this_component
@@ -143,7 +144,7 @@ class mavlink_bridge(Node, mavlink_publisher):
# ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓
if msg.get_msgId() == 0: # HEARTBEAT 處理
if msg_id == 0: # HEARTBEAT 處理
this_component.emitParams['base_mode'] = msg.base_mode
this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg)
this_component.emitParams['flightMode'] = msg # debug
@@ -171,7 +172,7 @@ class mavlink_bridge(Node, mavlink_publisher):
# 若未定義的訊息類型則不處理 並跳出訊息
else:
logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type()))
logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type()))
continue
logger.info('End of mavlink_bridge._run_thread')