diff --git a/README.md b/README.md index 852fcd0..9f6379f 100644 --- a/README.md +++ b/README.md @@ -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 簡述 diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index b0e83e2..110fb61 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -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') \ No newline at end of file + 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') \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index f090b41..4b2b07e 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/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 = {} \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 60ec91b..072d861 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/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 @@ -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')