整合後項目修正

1. mavlinkObject.py 統合 msg_id 參數名稱
2. mavlinkDevice.py 移除在預期操作下 仍會大量跳錯誤訊息的警示
3. devRun.py 重新驗證 merge 項目與版本調整

README.md

@@ -1,22 +1,26 @@
 這是天雷系統的專案
 
 ===
-專案核心環境
+專案核心框架
 1. ROS2 Humble
-2. Python
+2. Python 3.8.10
-3. Ardupilot
 
 
 ===
 必要相依套件
 
-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

@@ -14,8 +14,8 @@ import mavlinkDevice as md
 
 # ====================== 分割線 =====================
 
-test_item = 22
+test_item = 51
-running_time = 20
+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

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

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