diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 60d54b9..835bfae 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -464,69 +464,47 @@ elif test_item == 22: 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:14550" - 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_state(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() - T = True - while T: - try: - # rclpy.spin(analyzer) - analyzer.emit_info() # 這邊是測試 node 的運行 + print("===> Start of Program .Test", test_item) + rclpy.init() + + # 1) 啟動 bridge(它已自動建立所有 publisher) + bridge = mo.mavlink_bridge() + bridge._init_node() + + # 2) 建立一條 UDP 通道並啟動 mavlink_object + mav_sock = mavutil.mavlink_connection("udp:127.0.0.1:14550") + mobj = mo.mavlink_object(mav_sock) # 自動分配 socket_id + mobj.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147] # HEARTBEAT + 常用訊息 + mobj.run() + + print("waiting for mavlink data ...") + time.sleep(2) # 給一點時間收 HEARTBEAT + + # 3) 顯示目前偵測到的 sysid 清單 + print("Current sysid list:", list(bridge.mavlink_systems.keys())) + + # 4) 主迴圈:持續將資料 emit 出去 + try: + last_timesync = time.time() + while rclpy.ok(): + now = time.time() + + # 每秒一次 + if now - last_timesync >= 1.0: + # 發送 TIMESYNC request + mav_sock.mav.timesync_send(0, int(now * 1e9)) + last_timesync = now + + bridge.emit_info() # 將所有 emitParams 發布到 ROS topic time.sleep(0.5) - except KeyboardInterrupt: - break - - analyzer.destroy_node() + except KeyboardInterrupt: + pass + + # -------- 清理 -------- + bridge.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 + mobj.stop(); mobj.thread.join() + bridge.stop(); bridge.thread.join() + mav_sock.close() + print("<=== End of Program")