From f7f725a2abc4362dd27880ebf885c310cf9828f5 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 9 Apr 2025 20:35:56 +0800 Subject: [PATCH 01/14] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/mavlink.py | 147 ++++++++++++++++++----------- 1 file changed, 90 insertions(+), 57 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index 9078880..b047a0e 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -1,7 +1,6 @@ import sys import time import math -import rclpy from PyQt5 import QtWidgets from PyQt5.QtCore import QThread, pyqtSignal from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox @@ -20,10 +19,11 @@ class MAVLinkWorker(QThread): loss_signal = pyqtSignal(str, float) frequency_signal = pyqtSignal(str, float) param_signal = pyqtSignal(str, int) + kbps_signal = pyqtSignal(str, float) def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): super().__init__() - self.namespaces = ['UAV1', 'UAV4', 'UAV5'] + self.namespaces = ['UAV1', 'UAV2', 'UAV3'] self.connection = mavutil.mavlink_connection(usb, baud=57600) self.connection.wait_heartbeat() for sysid in self.namespaces: @@ -31,38 +31,48 @@ class MAVLinkWorker(QThread): self.set_sr_params(sysid) self.running = True - # 設置需要監控的訊息類型 - self.messages_to_monitor = ["HEARTBEAT", "BATTERY_STATUS", "GLOBAL_POSITION_INT", "GPS_RAW_INT", "LOCAL_POSITION_NED", "ATTITUDE", "VFR_HUD", "TIMESYNC"] + # 用於計算頻率丟包 self.message_count = {} self.frequency = {} self.start_time = {} - - # 用於計算丟包 self.seq_numbers = {} self.packet_loss_count = {} self.total_packet_count = {} self.loss_percentage = {} + self.total_bytes_received = {} + self.throughput_KBps = {} for namespace in self.namespaces: self.request_param(namespace, "SR1_EXTRA1") + self.connection.mav.timesync_send( + 0, #tc1 + int(time.time() * 1e9) #ts1 + ) def run(self): while self.running: try: - current_time = time.time() msg = self.connection.recv_msg() + current_time = time.time() if not msg: continue sysid = msg.get_srcSystem() if sysid == 0: continue namespace = f"UAV{sysid}" - print(sysid) msg_type = msg.get_type() if msg_type =="BAD_DATA": continue + - print(msg_type) + if sysid not in self.total_bytes_received: + self.total_bytes_received[sysid] = 0 + self.throughput_KBps[sysid] = 0 + + # 計算訊息大小 + msg_bytes = msg.get_msgbuf() # 取得封包的 bytes + if msg_bytes: + self.total_bytes_received[sysid] += len(msg_bytes) # Packet loss calculation if sysid not in self.seq_numbers: @@ -71,7 +81,7 @@ class MAVLinkWorker(QThread): self.total_packet_count[sysid] = 1 else: current_seq = msg.get_seq() - expected_seq = (self.seq_numbers[sysid] + 1) % 256 # MAVLink sequence numbers are modulo 256 + expected_seq = (self.seq_numbers[sysid] + 1) % 256 self.total_packet_count[sysid] += 1 if current_seq != expected_seq: # Packet(s) lost @@ -91,27 +101,32 @@ class MAVLinkWorker(QThread): self.message_count[sysid] += 1 # 每隔一段時間更新 - if current_time - self.start_time[sysid] >= 1: + elapsed_time = current_time - self.start_time[sysid] + if elapsed_time >= 1: # 每秒頻率 - self.frequency[sysid] = self.message_count[sysid] / (current_time - self.start_time[sysid]) - self.start_time[sysid] = current_time - self.message_count[sysid] = 0 + self.frequency[sysid] = self.message_count[sysid] / elapsed_time self.frequency_signal.emit(namespace, self.frequency[sysid]) # 發送 timesync request self.connection.mav.timesync_send( 0, #tc1 - int(time.time() * 1e9) #ts1 + int(current_time * 1e9) #ts1 ) - #self.send_heartbeat + #吞吐量 + self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time + self.kbps_signal.emit(namespace, self.throughput_KBps[sysid]) + + self.start_time[sysid] = current_time + self.message_count[sysid] = 0 + self.total_bytes_received[sysid] = 0 if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) self.state_signal.emit(namespace, mode) elif msg_type == "BATTERY_STATUS": - percentage = msg.battery_remaining/100 - self.battery_signal.emit(namespace, percentage) + voltage = msg.voltages[0] / 1000 + self.battery_signal.emit(namespace, voltage) elif msg_type == "GLOBAL_POSITION_INT": latitude = msg.lat / 1e7 @@ -140,11 +155,11 @@ class MAVLinkWorker(QThread): self.groundspeed_signal.emit(namespace, groundspeed) elif msg_type == "TIMESYNC": - round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6 + round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6 if(round_trip_time<1e6): self.ping_signal.emit(namespace, round_trip_time) - elif msg.get_type() == 'PARAM_VALUE': + elif msg_type == 'PARAM_VALUE': param_name = "SR1_EXTRA1" if msg.param_id == param_name: self.param_signal.emit(namespace, msg.param_value) @@ -253,13 +268,7 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - def send_heartbeat(self): - self.connection.mav.heartbeat_send( - mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_INVALID, - 0, 0, 0 - ) - + ''' def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ freqs = [0, 2, 4] @@ -275,6 +284,22 @@ class MAVLinkWorker(QThread): } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) + ''' + def set_sr_params(self, sysid): + """ 直接設置 MAVLink 訊息頻率 """ + freqs = [0, 1, 1] + params = { + "SR1_ADSB": freqs[0], + "SR1_EXT_STAT": freqs[0], + "SR1_EXTRA1": freqs[2], + "SR1_EXTRA2": freqs[2], + "SR1_EXTRA3": freqs[0], + "SR1_POSITION": freqs[1], + "SR1_RAW_SENS": freqs[1], + "SR1_RC_CHAN": freqs[0] + } + for param, value in params.items(): + self.set_param(f"UAV{sysid}", param, value) class DroneControlApp(QMainWindow): def __init__(self): @@ -296,6 +321,7 @@ class DroneControlApp(QMainWindow): self.workers.loss_signal.connect(self.update_loss) self.workers.frequency_signal.connect(self.update_frequency) self.workers.param_signal.connect(self.update_param) + self.workers.kbps_signal.connect(self.update_kbps) self.workers.start() def initUI(self): @@ -326,17 +352,18 @@ class DroneControlApp(QMainWindow): # 狀態顯示 self.uav_labels[namespace] = { "status": QLabel("狀態：等待數據..."), - "battery": QLabel("電量：等待數據..."), + "battery": QLabel("電壓：等待數據..."), + "local_position": QLabel("Local Position：等待數據..."), "gps": QLabel("GPS位置：等待數據...\n\n"), "fix": QLabel("Fix Type：等待數據..."), "satellites": QLabel("衛星數量：等待數據..."), - "local_position": QLabel("Local Position：等待數據..."), "groundspeed": QLabel("地面速度：等待數據..."), "pitch": QLabel("Pitch：等待數據..."), "heading": QLabel("Heading：等待數據..."), "ping": QLabel("Ping：等待數據..."), "loss": QLabel("丟包：等待數據..."), "frequency": QLabel("頻率：等待數據..."), + "kbps": QLabel("吞吐量：等待數據..."), "param": QLabel("SR1_EXTRA1：等待數據...") } @@ -370,25 +397,6 @@ class DroneControlApp(QMainWindow): mode_layout.addWidget(self.modeButton) main_layout.addLayout(mode_layout) - # XYZ 設置欄位 - xyz_layout = QHBoxLayout() - self.positionX = QLineEdit() - self.positionX.setPlaceholderText("X") - self.positionY = QLineEdit() - self.positionY.setPlaceholderText("Y") - self.positionZ = QLineEdit() - self.positionZ.setPlaceholderText("Z") - xyz_layout.addWidget(QLabel("輸入位置：")) - xyz_layout.addWidget(self.positionX) - xyz_layout.addWidget(self.positionY) - xyz_layout.addWidget(self.positionZ) - main_layout.addLayout(xyz_layout) - - # 設置 XYZ 的按鈕 - self.setPositionButton = QPushButton("設置位置") - self.setPositionButton.clicked.connect(self.set_local_position) - main_layout.addWidget(self.setPositionButton) - # 解鎖按鈕 fly_layout = QHBoxLayout() self.armButton = QPushButton("解鎖") @@ -406,6 +414,25 @@ class DroneControlApp(QMainWindow): fly_layout.addWidget(self.landButton) main_layout.addLayout(fly_layout) + # XYZ 設置欄位 + xyz_layout = QHBoxLayout() + self.positionX = QLineEdit() + self.positionX.setPlaceholderText("X") + self.positionY = QLineEdit() + self.positionY.setPlaceholderText("Y") + self.positionZ = QLineEdit() + self.positionZ.setPlaceholderText("Z") + self.setPositionButton = QPushButton("設置位置") + self.setPositionButton.clicked.connect(self.set_local_position) + xyz_layout.addWidget(QLabel("輸入位置：")) + xyz_layout.addWidget(self.positionX) + xyz_layout.addWidget(self.positionY) + xyz_layout.addWidget(self.positionZ) + xyz_layout.addWidget(self.setPositionButton) + main_layout.addLayout(xyz_layout) + + # 設置 XYZ 的按鈕 + #設置重啟按鈕 self.rebootButton = QPushButton("重啟") self.rebootButton.clicked.connect(self.reboot_drone) @@ -426,8 +453,8 @@ class DroneControlApp(QMainWindow): def update_state(self, namespace, mode): self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") - def update_battery(self, namespace, percentage): - self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%") + def update_battery(self, namespace, voltage): + self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V") def update_gps(self, namespace, latitude, longitude): self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°") @@ -465,6 +492,9 @@ class DroneControlApp(QMainWindow): def update_frequency(self, namespace, frequency): self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz") + def update_kbps(self, namespace, kbps): + self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s") + def update_param(self, namespace, value): self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") @@ -488,10 +518,12 @@ class DroneControlApp(QMainWindow): def takeoff_drone(self): try: z_text = self.positionZ.text().strip() - altitude = float(z_text) if z_text else 5.0 - for namespace in self.namespaces: + z = float(z_text) if z_text else 5.0 + h = 2 + for i, namespace in enumerate(self.namespaces): if self.uav_checkboxes[namespace].isChecked(): - self.workers.takeoff(namespace, altitude) + self.workers.takeoff(namespace, z + h * i) + except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") @@ -505,9 +537,11 @@ class DroneControlApp(QMainWindow): x = float(self.positionX.text().strip()) y = float(self.positionY.text().strip()) z = float(self.positionZ.text().strip()) - for namespace in self.namespaces: + h = 2 + for i, namespace in enumerate(self.namespaces): if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z) + self.workers.set_local_position(namespace, x, y, z + h * i) + except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") @@ -529,8 +563,7 @@ class DroneControlApp(QMainWindow): except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") -def main(args=None): - rclpy.init(args=args) +def main(): app = QtWidgets.QApplication(sys.argv) window = DroneControlApp() window.show() From 5535ab1daba701449c8405f4d6e248ebda986b01 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 9 Apr 2025 21:36:11 +0800 Subject: [PATCH 03/14] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/xbee.py | 620 ++++++++++++++++++++++++++++++++ 1 file changed, 620 insertions(+) create mode 100644 src/unitdev01/unitdev01/xbee.py diff --git a/src/unitdev01/unitdev01/xbee.py b/src/unitdev01/unitdev01/xbee.py new file mode 100644 index 0000000..db51d0f --- /dev/null +++ b/src/unitdev01/unitdev01/xbee.py @@ -0,0 +1,620 @@ +import sys +import time +import math +import serial +import struct +from PyQt5 import QtWidgets +from PyQt5.QtCore import QThread, pyqtSignal +from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox +from pymavlink import mavutil +from pymavlink.dialects.v20 import ardupilotmega as mavlink2 + +class PacketCapture: + def __init__(self): + self.data = bytearray() + + def write(self, b): + self.data.extend(b) + return len(b) + +class MAVLinkWorker(QThread): + state_signal = pyqtSignal(str, str) + battery_signal = pyqtSignal(str, float) + gps_signal = pyqtSignal(str, float, float) + gps_status_signal = pyqtSignal(str, int, int) + local_position_signal = pyqtSignal(str, float, float, float) + imu_signal = pyqtSignal(str, float) + hdg_signal = pyqtSignal(str, float) + groundspeed_signal = pyqtSignal(str, float) + ping_signal = pyqtSignal(str, float) + loss_signal = pyqtSignal(str, float) + frequency_signal = pyqtSignal(str, float) + param_signal = pyqtSignal(str, int) + kbps_signal = pyqtSignal(str, float) + + def __init__(self): + super().__init__() + self.namespaces = ['UAV1', 'UAV2', 'UAV3'] + self.connection = mavutil.mavlink_connection('/dev/ttyUSB0', baud=57600) + self.connection.wait_heartbeat() + for sysid in self.namespaces: + sysid = int(sysid.replace('UAV', '')) + self.set_sr_params(sysid) + self.running = True + + # 用於計算頻率丟包 + self.message_count = {} + self.frequency = {} + self.start_time = {} + self.seq_numbers = {} + self.packet_loss_count = {} + self.total_packet_count = {} + self.loss_percentage = {} + self.total_bytes_received = {} + self.throughput_KBps = {} + + for namespace in self.namespaces: + self.request_param(namespace, "SR1_EXTRA1") + self.connection.mav.timesync_send( + 0, #tc1 + int(time.time() * 1e9) #ts1 + ) + + def run(self): + while self.running: + try: + msg = self.connection.recv_msg() + current_time = time.time() + if not msg: + continue + sysid = msg.get_srcSystem() + if sysid == 0: + continue + namespace = f"UAV{sysid}" + msg_type = msg.get_type() + if msg_type =="BAD_DATA": + continue + + + if sysid not in self.total_bytes_received: + self.total_bytes_received[sysid] = 0 + self.throughput_KBps[sysid] = 0 + + # 計算訊息大小 + msg_bytes = msg.get_msgbuf() # 取得封包的 bytes + if msg_bytes: + self.total_bytes_received[sysid] += len(msg_bytes) + + # Packet loss calculation + if sysid not in self.seq_numbers: + self.seq_numbers[sysid] = msg.get_seq() # Initialize sequence number tracking + self.packet_loss_count[sysid] = 0 + self.total_packet_count[sysid] = 1 + else: + current_seq = msg.get_seq() + expected_seq = (self.seq_numbers[sysid] + 1) % 256 + self.total_packet_count[sysid] += 1 + + if current_seq != expected_seq: # Packet(s) lost + lost_packets = (current_seq - expected_seq) % 256 + self.packet_loss_count[sysid] += lost_packets + self.total_packet_count[sysid] += lost_packets + + self.seq_numbers[sysid] = current_seq + self.loss_percentage[sysid] = (self.packet_loss_count[sysid] / self.total_packet_count[sysid]) * 100 + self.loss_signal.emit(namespace, self.loss_percentage[sysid]) + + # Frequency calculation + if sysid not in self.message_count: + self.message_count[sysid] = 0 + self.start_time[sysid] = current_time + + self.message_count[sysid] += 1 + + # 每隔一段時間更新 + elapsed_time = current_time - self.start_time[sysid] + if elapsed_time >= 1: + # 每秒頻率 + self.frequency[sysid] = self.message_count[sysid] / elapsed_time + self.frequency_signal.emit(namespace, self.frequency[sysid]) + + # 發送 timesync request + self.connection.mav.timesync_send( + 0, #tc1 + int(current_time * 1e9) #ts1 + ) + #吞吐量 + self.throughput_KBps[sysid] = (self.total_bytes_received[sysid] / (1024)) / elapsed_time + self.kbps_signal.emit(namespace, self.throughput_KBps[sysid]) + + self.start_time[sysid] = current_time + self.message_count[sysid] = 0 + self.total_bytes_received[sysid] = 0 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + self.state_signal.emit(namespace, mode) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.battery_signal.emit(namespace, voltage) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.gps_signal.emit(namespace, latitude, longitude) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + satellites_visible = msg.satellites_visible + self.gps_status_signal.emit(namespace, fix_type, satellites_visible) + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.local_position_signal.emit(namespace, x, y, z) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.imu_signal.emit(namespace, pitch) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.hdg_signal.emit(namespace, heading) + self.groundspeed_signal.emit(namespace, groundspeed) + + elif msg_type == "TIMESYNC": + round_trip_time = (int(current_time * 1e9) - msg.ts1) / 1e6 + if(round_trip_time<1e6): + self.ping_signal.emit(namespace, round_trip_time) + + elif msg_type == 'PARAM_VALUE': + param_name = "SR1_EXTRA1" + if msg.param_id == param_name: + self.param_signal.emit(namespace, msg.param_value) + + except Exception as e: + print(f"Error reading message: {e}") + + def stop(self): + self.running = False + self.connection.close() + + def build_api_tx_frame(self, data: bytes, frame_id=0x01): + frame_type = 0x10 + dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # Broadcast + dest_addr16 = b'\xFF\xFE' + broadcast_radius = 0x00 + options = 0x00 + + frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) + frame += dest_addr64 + dest_addr16 + frame += struct.pack(">BB", broadcast_radius, options) + data + checksum = 0xFF - (sum(frame) & 0xFF) + return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) + + def send_command(self, msg): + # Create the packet and send via serial port + PORT = "/dev/ttyUSB0" + BAUD = 57600 + ser = serial.Serial(PORT, BAUD) + capture = PacketCapture() + mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) + mav.version = 2 + mav.send(msg) + api_frame = self.build_api_tx_frame(capture.data) + ser.write(api_frame) + print("RAW HEX:", capture.data.hex()) + + def set_mode(self, namespace, mode): + sysid = int(namespace.replace('UAV', '')) + if mode == 'STABILIZE': + mode = 0 + elif mode == 'AUTO': + mode = 3 + elif mode == 'GUIDED': + mode = 4 + elif mode == 'LOITER': + mode = 5 + + msg = self.connection.mav.command_long_encode( + target_system=sysid, + target_component=1, + command=mavutil.mavlink.MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=1, # Custom mode enabled + param2=mode, + param3=0, param4=0, param5=0, param6=0, param7=0 + ) + self.send_command(msg) + + def arm(self, namespace, arm): + sysid = int(namespace.replace('UAV', '')) + msg = self.connection.mav.command_long_encode( + target_system=sysid, + target_component=1, + command=mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + confirmation=0, + param1=1 if arm else 0, # 1 to arm, 0 to disarm + param2=0, param3=0, param4=0, param5=0, param6=0, param7=0 + ) + self.send_command(msg) + + def takeoff(self, namespace, altitude): + sysid = int(namespace.replace('UAV', '')) + msg = self.connection.mav.command_long_encode( + target_system=sysid, + target_component=1, + command=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + confirmation=0, + param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, + param7=altitude + ) + self.send_command(msg) + + def land(self, namespace): + sysid = int(namespace.replace('UAV', '')) + msg = self.connection.mav.command_long_encode( + target_system=sysid, + target_component=1, + command=mavutil.mavlink.MAV_CMD_NAV_LAND, + confirmation=0, + param1=0, param2=0, param3=0, param4=0, param5=0, param6=0, + param7=0 + ) + self.send_command(msg) + + def set_local_position(self, namespace, x, y, z): + sysid = int(namespace.replace('UAV', '')) + msg = self.connection.mav.set_position_target_local_ned_encode( + 0, sysid, 1, # time_boot_ms, sysid, compid + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame + 0b110111111000, # Mask + y, x, -z, # Position + 0, 0, 0, # Velocity + 0, 0, 0, # Acceleration + 0, 0 # Yaw, yaw_rate + ) + self.send_command(msg) + + def reboot_drone(self, namespace): + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # target_component (一般為1) + mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, # Reboot 命令 + 0, # confirmation + 1, # 第一個參數 (1 = reboot，0 = 無操作，2 = 關機) + 0, 0, 0, 0, 0, 0 # 其餘參數保留 + ) + + def set_param(self, namespace, param_name, value): + sysid = int(namespace.replace('UAV', '')) + try: + self.connection.mav.param_set_send( + sysid, + 1, + param_name.encode('utf-8'), + float(value), + mavutil.mavlink.MAV_PARAM_TYPE_REAL32 + ) + except Exception as e: + print(f"Failed to set parameter {param_name}: {e}") + + def request_param(self, namespace, param_name): + sysid = int(namespace.replace('UAV', '')) + try: + self.connection.mav.param_request_read_send( + sysid, # 系統 ID + 1, # 組件 ID + param_name.encode('utf-8'), # 參數名稱 + -1 # 參數索引，-1 表示根據名稱請求 + ) + except Exception as e: + print(f"Failed to set parameter {param_name}: {e}") + + ''' + def set_sr_params(self, sysid): + """ 直接設置 MAVLink 訊息頻率 """ + freqs = [0, 2, 4] + params = { + "SR1_ADSB": freqs[0], + "SR1_EXT_STAT": freqs[1], + "SR1_EXTRA1": freqs[2], + "SR1_EXTRA2": freqs[2], + "SR1_EXTRA3": freqs[1], + "SR1_POSITION": freqs[1], + "SR1_RAW_SENS": freqs[1], + "SR1_RC_CHAN": freqs[1] + } + for param, value in params.items(): + self.set_param(f"UAV{sysid}", param, value) + ''' + def set_sr_params(self, sysid): + """ 直接設置 MAVLink 訊息頻率 """ + freqs = [0, 1, 1] + params = { + "SR1_ADSB": freqs[0], + "SR1_EXT_STAT": freqs[0], + "SR1_EXTRA1": freqs[2], + "SR1_EXTRA2": freqs[2], + "SR1_EXTRA3": freqs[0], + "SR1_POSITION": freqs[1], + "SR1_RAW_SENS": freqs[1], + "SR1_RC_CHAN": freqs[0] + } + for param, value in params.items(): + self.set_param(f"UAV{sysid}", param, value) + +class DroneControlApp(QMainWindow): + def __init__(self): + super().__init__() + self.workers = MAVLinkWorker() + self.namespaces = self.workers.namespaces + self.initUI() + + # Connect signals to update the UI + self.workers.state_signal.connect(self.update_state) + self.workers.battery_signal.connect(self.update_battery) + self.workers.gps_signal.connect(self.update_gps) + self.workers.gps_status_signal.connect(self.update_gps_status) + self.workers.local_position_signal.connect(self.update_local_position) + self.workers.imu_signal.connect(self.update_imu) + self.workers.hdg_signal.connect(self.update_hdg) + self.workers.groundspeed_signal.connect(self.update_groundspeed) + self.workers.ping_signal.connect(self.update_ping) + self.workers.loss_signal.connect(self.update_loss) + self.workers.frequency_signal.connect(self.update_frequency) + self.workers.param_signal.connect(self.update_param) + self.workers.kbps_signal.connect(self.update_kbps) + self.workers.start() + + def initUI(self): + self.setWindowTitle("多無人機控制介面") + self.setGeometry(100, 100, 800, 600) + + # 主佈局 + main_layout = QVBoxLayout() + + # 無人機選擇區域 + uav_layout = QHBoxLayout() + self.uav_checkboxes = {} + for namespace in self.namespaces: + checkbox = QCheckBox(namespace) + checkbox.setChecked(True) # 預設勾選 + self.uav_checkboxes[namespace] = checkbox + uav_layout.addWidget(checkbox) + main_layout.addLayout(uav_layout) + + # 顯示所有無人機資訊，從左到右顯示 + uav_layout = QHBoxLayout() + + # 逐個顯示 UAV 的資訊 + self.uav_labels = {} + for namespace in self.namespaces: + uav_info_layout = QVBoxLayout() # 每台 UAV 的資訊垂直排列 + + # 狀態顯示 + self.uav_labels[namespace] = { + "status": QLabel("狀態：等待數據..."), + "battery": QLabel("電壓：等待數據..."), + "local_position": QLabel("Local Position：等待數據..."), + "gps": QLabel("GPS位置：等待數據...\n\n"), + "fix": QLabel("Fix Type：等待數據..."), + "satellites": QLabel("衛星數量：等待數據..."), + "groundspeed": QLabel("地面速度：等待數據..."), + "pitch": QLabel("Pitch：等待數據..."), + "heading": QLabel("Heading：等待數據..."), + "ping": QLabel("Ping：等待數據..."), + "loss": QLabel("丟包：等待數據..."), + "frequency": QLabel("頻率：等待數據..."), + "kbps": QLabel("吞吐量：等待數據..."), + "param": QLabel("SR1_EXTRA1：等待數據...") + } + + # 把每個資訊添加到該 UAV 的垂直佈局中 + for label in self.uav_labels[namespace].values(): + uav_info_layout.addWidget(label) + + # 將該 UAV 的資訊佈局加到主佈局中（從左到右排列） + uav_layout.addLayout(uav_info_layout) + + main_layout.addLayout(uav_layout) + + # SR1_EXTRA1參數設置 + param_layout = QHBoxLayout() + self.paramInput = QLineEdit() + self.paramInput.setPlaceholderText("輸入SR1_EXTRA1的新值") + self.setParamButton = QPushButton("設置SR1_EXTRA1") + self.setParamButton.clicked.connect(self.set_SR1_EXTRA1) + param_layout.addWidget(self.paramInput) + param_layout.addWidget(self.setParamButton) + main_layout.addLayout(param_layout) + + # 模式切換區域 + mode_layout = QHBoxLayout() + self.modeComboBox = QComboBox() + self.modeComboBox.addItems(["GUIDED", "STABILIZE", "AUTO", "LOITER"]) + self.modeButton = QPushButton("切換模式") + self.modeButton.clicked.connect(self.change_mode) + mode_layout.addWidget(QLabel("選擇模式：")) + mode_layout.addWidget(self.modeComboBox) + mode_layout.addWidget(self.modeButton) + main_layout.addLayout(mode_layout) + + # 解鎖按鈕 + fly_layout = QHBoxLayout() + self.armButton = QPushButton("解鎖") + self.armButton.clicked.connect(self.arm_drone) + + # 起飛按鈕 + self.takeoffButton = QPushButton("起飛") + self.takeoffButton.clicked.connect(self.takeoff_drone) + + # 降落按鈕 + self.landButton = QPushButton("降落") + self.landButton.clicked.connect(self.land_drone) + fly_layout.addWidget(self.armButton) + fly_layout.addWidget(self.takeoffButton) + fly_layout.addWidget(self.landButton) + main_layout.addLayout(fly_layout) + + # XYZ 設置欄位 + xyz_layout = QHBoxLayout() + self.positionX = QLineEdit() + self.positionX.setPlaceholderText("X") + self.positionY = QLineEdit() + self.positionY.setPlaceholderText("Y") + self.positionZ = QLineEdit() + self.positionZ.setPlaceholderText("Z") + self.setPositionButton = QPushButton("設置位置") + self.setPositionButton.clicked.connect(self.set_local_position) + xyz_layout.addWidget(QLabel("輸入位置：")) + xyz_layout.addWidget(self.positionX) + xyz_layout.addWidget(self.positionY) + xyz_layout.addWidget(self.positionZ) + xyz_layout.addWidget(self.setPositionButton) + main_layout.addLayout(xyz_layout) + + # 設置 XYZ 的按鈕 + + #設置重啟按鈕 + self.rebootButton = QPushButton("重啟") + self.rebootButton.clicked.connect(self.reboot_drone) + main_layout.addWidget(self.rebootButton) + + # 設置主佈局 + central_widget = QWidget(self) + central_widget.setLayout(main_layout) + self.setCentralWidget(central_widget) + self.show() + + def closeEvent(self, event): + try: + self.workers.stop() + finally: + event.accept() + + def update_state(self, namespace, mode): + self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") + + def update_battery(self, namespace, voltage): + self.uav_labels[namespace]["battery"].setText(f"電壓：{voltage:.2f} V") + + def update_gps(self, namespace, latitude, longitude): + self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°") + + def update_gps_status(self, namespace, fix_type, satellites_visible): + fix_type_str = { + 0: "No Fix", + 1: "No Fix", + 2: "2D Fix", + 3: "3D Fix", + 4: "RTK Float", + 5: "RTK Fix" + }.get(fix_type, "Unknown") + self.uav_labels[namespace]["fix"].setText(f"Fix Type：{fix_type_str}") + self.uav_labels[namespace]["satellites"].setText(f"衛星數量：{satellites_visible}") + + def update_local_position(self, namespace, x, y, z): + self.uav_labels[namespace]["local_position"].setText(f"Local：({x:.2f}, {y:.2f}, {z:.2f})") + + def update_imu(self, namespace, pitch): + self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°") + + def update_hdg(self, namespace, heading): + self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°") + + def update_groundspeed(self, namespace, groundspeed): + self.uav_labels[namespace]["groundspeed"].setText(f"地面速度：{groundspeed:.2f} m/s") + + def update_ping(self, namespace, ping): + self.uav_labels[namespace]["ping"].setText(f"Ping：{ping:.2f} ms") + + def update_loss(self, namespace, loss): + self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") + + def update_frequency(self, namespace, frequency): + self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz") + + def update_kbps(self, namespace, kbps): + self.uav_labels[namespace]["kbps"].setText(f"吞吐量：{kbps:.2f} KB/s") + + def update_param(self, namespace, value): + self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") + + def change_mode(self): + try: + selected_mode = self.modeComboBox.currentText() + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_mode(namespace, selected_mode) + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") + + def arm_drone(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.arm(namespace, True) + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"解鎖失敗：{e}") + + def takeoff_drone(self): + try: + z_text = self.positionZ.text().strip() + z = float(z_text) if z_text else 5.0 + h = 2 + for i, namespace in enumerate(self.namespaces): + if self.uav_checkboxes[namespace].isChecked(): + self.workers.takeoff(namespace, z + h * i) + + except ValueError: + QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + + def land_drone(self): + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.land(namespace) + + def set_local_position(self): + try: + x = float(self.positionX.text().strip()) + y = float(self.positionY.text().strip()) + z = float(self.positionZ.text().strip()) + h = 2 + for i, namespace in enumerate(self.namespaces): + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_local_position(namespace, x, y, z + h * i) + + except ValueError: + QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + + def reboot_drone(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.reboot_drone(namespace) + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"重啟失敗：{e}") + + def set_SR1_EXTRA1(self): + param_value = self.paramInput.text().strip() + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_param(namespace, "SR1_EXTRA1", param_value) + self.workers.request_param(namespace, "SR1_EXTRA1") + except ValueError: + QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + +def main(): + app = QtWidgets.QApplication(sys.argv) + window = DroneControlApp() + window.show() + sys.exit(app.exec_()) + +if __name__ == '__main__': + main() \ No newline at end of file From de694fe30197411870e658c3feb9cd271b0e3b12 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 10 Apr 2025 23:52:22 +0800 Subject: [PATCH 04/14] ros2 --- src/unitdev01/unitdev01/interface.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py index 5699e46..07b2773 100644 --- a/src/unitdev01/unitdev01/interface.py +++ b/src/unitdev01/unitdev01/interface.py @@ -144,7 +144,6 @@ class DroneControlApp(QMainWindow): self.workers.imu_signal.connect(self.update_imu) self.workers.hdg_signal.connect(self.update_hdg) self.workers.groundspeed_signal.connect(self.update_groundspeed) - self.workers.start() def initUI(self): From 84901cecee2c21c2d127c385e734861247cd5ab7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Fri, 11 Apr 2025 00:45:03 +0800 Subject: [PATCH 05/14] =?UTF-8?q?=E4=BA=8C=E9=80=B2=E5=88=B6=E5=B0=81?= =?UTF-8?q?=E5=8C=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev01/unitdev01/xbee.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/unitdev01/unitdev01/xbee.py b/src/unitdev01/unitdev01/xbee.py index db51d0f..b550e1e 100644 --- a/src/unitdev01/unitdev01/xbee.py +++ b/src/unitdev01/unitdev01/xbee.py @@ -75,7 +75,6 @@ class MAVLinkWorker(QThread): if msg_type =="BAD_DATA": continue - if sysid not in self.total_bytes_received: self.total_bytes_received[sysid] = 0 self.throughput_KBps[sysid] = 0 From c38f277db46a19d918f85fe68d24c2db05c7d3e8 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Apr 2025 22:57:49 +0800 Subject: [PATCH 06/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkObject.py | 469 ++++++++++++++---- 1 file changed, 361 insertions(+), 108 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index f84ce60..deaf82f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,16 +1,4 @@ -import threading -from pymavlink import mavutil -import queue - -import rclpy -from rclpy.node import Node -from std_msgs.msg import String - -import time - - ''' - # 不同的匯流排只有單一種通訊協定 # 匯流排接到訊息後透過 queue stack 傳送到分析器 # 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 @@ -18,43 +6,211 @@ import time # 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 # 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 -# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 -# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function - +# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 +# 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function ''' +# 基礎功能的 import +import threading +import queue +import time + +# mavlink 的 import +from pymavlink import mavutil + +# ROS2 的 import +from rclpy.node import Node + +# 自定義的 import +from mavlinkDevice import mavlink_device +from mavlinkPublish import mavlink_publisher +from theLogger import setup_logger + + +# ====================== 分割線 ===================== + +logger = setup_logger("mavlinkObject.py") + fixed_stream_analyzer_queue = queue.Queue() return_packet_processor_queue = queue.Queue() converte_format_queues = [] -class MavlinkObject(): +# 用來記錄每個 system 的資訊 +# 資料格式 { sysid : mavlink_device object } +mavlink_systems = {} + +# ====================== 分割線 ===================== + +class mavlink_analyzer(Node, mavlink_publisher): + ''' + 這個 class 就是 固定串流分析器 + 是用來接收 mavlink 訊息 並進行分析 + 這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的 + 記錄有 mavlink bus 上有那些 system id 和 component id + 為了每個 system id 都有一個對應的 device object + 並且看是否有重複 system id + + 整段代碼包含兩大區塊 thread 和 node + + thread 區塊內會對 fixed_stream_analyzer_queue 進行監聽 並且將收到的訊息進行處理 + 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object + + node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 + 藉由控制 ros2 的機制再把 device object 的資訊發送出去 + + ps. 我限制了這個 class 只能有一個 instance + + ''' + _instance = None + _lock = threading.Lock() # 確保多線程安全 + + def __new__(cls, *args, **kwargs): + with cls._lock: # 確保多線程環境下只有一個實例被創建 + if cls._instance is None: + cls._instance = super(mavlink_analyzer, cls).__new__(cls) + return cls._instance + + def __init__(self): + if not hasattr(self, "initialized"): # 防止重複初始化 + self.initialized = True + + # 關聯到全域變數 + global mavlink_systems + self.mavlink_systems = mavlink_systems + + # 當 object 建立時會直接運行 thread 直到消滅 + self.running = True + self.thread = threading.Thread(target=self._run_thread) + self.thread.start() + else: + logger.error('mavlink_analyzer instance already exists. Do not create another one.') + + def stop(self): + self.running = False + + # === Thread 區塊 === + def _run_thread(self): + # start_time = time.time() # debug + logger.info('Start of mavlink_analyzer._run_thread') + # 從 Queue fixed_stream_analyzer_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 + while self.running: + if fixed_stream_analyzer_queue.empty(): + continue + msg_pack = fixed_stream_analyzer_queue.get() + + msg = msg_pack[2] + sysid = msg.get_srcSystem() + msg_id = msg.get_msgId() + msg_type = msg.get_type() + + # 若這個 system id 還不存在 執行完整建立 device object 的流程 + if not sysid in self.mavlink_systems: + device_object = mavlink_device() # 創建一個新的 device object + self.mavlink_systems[sysid] = device_object + device_object.socket_id = msg_pack[0] + device_object.sysid = sysid + + this_component = device_object.mavlink_component() # 創建一個新的 component object + device_object.components[msg.get_srcComponent()] = this_component + this_component.mav_type = msg_type + + # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ + if msg_id == 0: # HEARTBEAT 處理 + this_component.emitParams['base_mode'] = msg.base_mode + this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg) + + elif msg_id == 30: # ATTITUDE 處理 + this_component.emitParams['attitude'] = msg + + elif msg_id == 32: # LOCAL_POSITION_NED 處理 + this_component.emitParams['local_position'] = msg + + elif msg_id == 33: # GLOBAL_POSITION_INT 處理 + this_component.emitParams['global_position'] = msg + + elif msg_id == 74: # VFR_HUD 處理 + this_component.emitParams['vfr_hud'] = msg + + elif msg_id == 147: # BATTERY_STATUS 處理 + this_component.emitParams['battery'] = msg + + # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑ + + # 若未定義的訊息類型則不處理 並跳出訊息 + else: + logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type())) + continue + + logger.info('End of mavlink_analyzer._run_thread') + + # === Node 區塊 === + def _init_node(self): + logger.info('Start of mavlink_analyzer._init_node') + super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒 + + def emit_info(self): + # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 + # 把 emitParams 的參數發送出去 + for sysid, device in self.mavlink_systems.items(): + for compid, component in device.components.items(): + for topic_name in component.publishers.keys(): + publisher = component.publishers[topic_name][0] + packEmit_func = component.publishers[topic_name][1] + packEmit_func(component.emitParams, publisher) + + def _del_node(self): + # TODO 這邊要刪除 node 的時候要做的事情 + # 先註銷所有 mavlink_systems 中 component 的 publisher + # 再註銷所有 mavlink_systems 中的 device object + # 再註銷 node + pass + +# ====================== 分割線 ===================== + +class mavlink_object(): + ''' + 每個 mavlink bus 都會有一個 mavlink_object + 其中主要是 thread 做統計封包與分流 + 分流表的控制在三個 list 分別為 + multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的 + multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 + multiplexingToConvert : 這個 list 是用來分流到轉格式的 + 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 + ''' def __init__(self, mavlink_socket, socket_id = None): self.socket_id = socket_id self.mavlink_socket = mavlink_socket self.msg_count = {} self.multiplexingList = [] + # 關聯到全域變數 + global mavlink_systems + self.mavlink_systems = mavlink_systems + + # 這三個 list 用來分配不同的訊息到不同的 queue self.multiplexingToAnalysis = [ - # 0, # HEARTBEAT + 0, # HEARTBEAT + 24, # GPS_RAW_INT 30, # ATTITUDE + 32, # LOCAL_POSITION_NED 33, # GLOBAL_POSITION_INT - # 147, # BATTERY_STATUS + 74, # VFR_HUD + 147, # BATTERY_STATUS ] self.multiplexingToReturn = [ 0, # HEARTBEAT - # 30, # ATTITUDE ] self.multiplexingToConvert = [ - [74, ] # VFR_HUD ] + def __del__(self): self.mavlink_socket.close() self.stop() def run(self): if not self.socket_id: - print('[mavlinkObject.py] Please set socket id before running') + logger.error('Please set socket id before running') return self.thread = threading.Thread(target=self._run_thread) self.running = self.updateMultiplexingList() @@ -64,134 +220,231 @@ class MavlinkObject(): self.running = False def _run_thread(self): + logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) start_time = time.time() while self.running: + timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 try: mavlinkMsg = self.mavlink_socket.recv_msg() except Exception as e: + logger.critical(f"Receiving data not mavlink format. Object Delete.") print(f"[mavlinkObject.py] Error receiving mavlink data: {e}") print(mavlinkMsg) self.running = False break - if mavlinkMsg: - self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息 - # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug + if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' + # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 + sysid = mavlinkMsg.get_srcSystem() + compid = mavlinkMsg.get_srcComponent() + if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息 + # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 + mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) + + # print(mavlinkMsg) # debug # break # Debug # 將訊息依照 multiplexing list 分發到不同的 queue for i in range(len(self.multiplexingList)): if mavlinkMsg.get_msgId() in self.multiplexingList[i]: if i == 0: - fixed_stream_analyzer_queue.put((self.socket_id,mavlinkMsg)) + fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg)) elif i == 1: - return_packet_processor_queue.put((self.socket_id,mavlinkMsg)) + return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) else: convert_queue = converte_format_queues[i-2] - convert_queue.put((self.socket_id,mavlinkMsg)) - - # 每秒中 統計一次 收到的訊息總量 - elapsed_time = time.time() - start_time - if elapsed_time > 1: - print('[mavlinkObject.py] Messages Type received (in about 1 sec) :') - print(self.msg_count) - start_time = time.time() - self.msg_count = {} - - # thread 結束 - print('[mavlinkObject.py] End of _run_thread') - - def count_mavlink_type(self, mavlinkMsg): - msg_type = mavlinkMsg.get_type() - if msg_type in self.msg_count: - self.msg_count[msg_type] += 1 - else: - self.msg_count[msg_type] = 1 + convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) - def publish_mavlink_data(self, mavlinkMsg): - msg = String() - msg.data = str(mavlinkMsg) - self.publisher_.publish(msg) + # thread 結束 + logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) def updateMultiplexingList(self): + ''' + 更新 multiplexing list 並做簡單的檢查 + ''' + # 更新 multiplexing list self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + # 檢查 multiplexing list 格式是否有錯誤 check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) - - # 若有錯誤則回傳 False if not check: - print('[mavlinkObject.py] Multiplexing List Error, Please check the list') + logger.error('Multiplexing List Format Error, Please check the list') + return False # 若有錯誤則回傳 False + + check = len(self.multiplexingToConvert) != len(converte_format_queues) + if check: + logger.error('Multiplexing Queue not fit List , Please check the list') return False return True -class mavlink_analyzer(Node): - ''' - 這個 class 是用來接收 mavlink 訊息 並進行分析 - 記錄有 mavlink bus 上有那些 system id 和 component id - 為了每個 system id 都有一個對應的 device object - 並且看是否有重複 system id +# ====================== 分割線 ===================== - 也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒? - ''' - def __init__(self): - super().__init__('mavlink_analyzer') +# 整合到 ros2 之後的程式進入點 +def main_node(): + pass - # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10) - # self.mavlink_object = None - # self.create_mavlink_object() +# ====================== 分割線 ===================== +# 測試時的程式進入點 +def main_develop(args=None): - def create_mavlink_object(self): - # connection_string="udp: - pass + test_item = 3 + print('test_item : ', test_item) -class mavlink_device(): - def __init__(self): - self.socket_id = None # 記錄來自於哪個 socket - self.sysid = None - self.component_count = 1 - self.compid_list = [] - + if test_item == 1: + # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 + # 只啟用了 mavlink_object 的功能 + print('Start of Program .Test 1') + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mavlink_object(mavlink_socket, 1) + mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE + mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT + mavlink_object1.multiplexingToConvert = [[74,]] # only VFR_HUD -def main(args=None): + # 做一個空的 queue list 驗證 multiplexingToConvert + converte_format_queues.append(queue.Queue()) + # 啟動連線的模組 + mavlink_object1.run() - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) + # 運行幾秒並印出 queue 的資料 + start_time = time.time() + while True: + while not fixed_stream_analyzer_queue.empty(): + print('fixed_stream_analyzer_queue:') + t = fixed_stream_analyzer_queue.get() + print('from {} : {}'.format(t[0],t[2])) + while not return_packet_processor_queue.empty(): + print('return_packet_processor_queue:') + t = return_packet_processor_queue.get() + # print('from {} : {}'.format(t[0],t[2])) + print(t[2].type) + + for q in converte_format_queues: + while not q.empty(): + print('converte_format_queues:') + t = q.get() + print('from {} : {}'.format(t[0],t[2])) + + # 結束程式 退出所有 thread + mavlink_object1.running = False + mavlink_object1.thread.join() + mavlink_socket.close() + print('End of Program .Test 1') + + elif test_item == 2: + # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 + # 啟用 mavlink_object 與 mavlink_analyzer的thread區塊 的功能 + print('Start of Program .Test 2') + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + + mavlink_object2 = mavlink_object(mavlink_socket, 1) + mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT + mavlink_object2.multiplexingToReturn = [] # only HEARTBEAT + mavlink_object2.multiplexingToConvert = [] # + + # 啟動 mavlink_analyzer + analyzer = mavlink_analyzer() + + # 啟動連線的模組 + mavlink_object2.run() - # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 - mavlink_object = MavlinkObject(mavlink_socket, 1) - mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE - mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT - mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD - - converte_format_queues.append(queue.Queue()) + start_time = time.time() + show_time = time.time() + compid = 1 + while time.time() - start_time < 5: + if (time.time() - show_time) >= 1: + show_time = time.time() + for sysid in analyzer.mavlink_systems: + print("目前收到的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].msg_count) + print("目前遺失的訊息數量 : ",analyzer.mavlink_systems[sysid].components[compid].lost_packet_count) + print("現在飛機的模式 : ",analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode']) + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + print("===================") + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + for sysid in mavlink_systems: + print(mavlink_systems[sysid]) + # 結束程式 退出所有 thread + mavlink_object2.stop() + mavlink_object2.thread.join() + analyzer.stop() + + analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 instance + analyzer.thread.join() + mavlink_socket.close() + print('End of Program .Test 2') + + elif test_item == 3: + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + print('Start of Program .Test 3') + rclpy.init() # 注意要初始化 rclpy 才能使用 node + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + + mavlink_object3 = mavlink_object(mavlink_socket, 1) + mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147] + mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT + mavlink_object3.multiplexingToConvert = [] # + + # 啟動 mavlink_analyzer + analyzer = mavlink_analyzer() + + # 關於 Node 的初始化 + show_time = time.time() + analyzer._init_node() # 初始化 node + print('初始化 node 完成 耗時 : ',time.time() - show_time) + + # 啟動連線的模組 + 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_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid]) + analyzer.create_global_vel(sysid, analyzer.mavlink_systems[sysid].components[compid]) + analyzer.create_global_hdg(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]) + end_time = time.time() + print(f"Execution time for create_flightMode: {end_time - start_time} seconds") + + print("start emit info") - mavlink_object.run() - start_time = time.time() - while time.time() - start_time < 2: - - while not fixed_stream_analyzer_queue.empty(): - print('fixed_stream_analyzer_queue:') - t = fixed_stream_analyzer_queue.get() - print('from {} : {}'.format(t[0],t[1])) - while not return_packet_processor_queue.empty(): - print('return_packet_processor_queue:') - t = return_packet_processor_queue.get() - print('from {} : {}'.format(t[0],t[1])) + start_time = time.time() + show_time = time.time() + while time.time() - start_time < 20: + try: + # rclpy.spin(analyzer) + analyzer.emit_info() # 這邊是測試 node 的運行 + time.sleep(0.5) + except KeyboardInterrupt: + break - for q in converte_format_queues: - while not q.empty(): - print('converte_format_queues:') - t = q.get() - print('from {} : {}'.format(t[0],t[1])) - - - mavlink_object.running = False - mavlink_object.thread.join() - print('End of Program') + analyzer.destroy_node() + rclpy.shutdown() + + # 結束程式 退出所有 thread + mavlink_object3.stop() + mavlink_object3.thread.join() + analyzer.stop() + analyzer.thread.join() + mavlink_socket.close() + print('End of Program .Test 3') if __name__ == '__main__': - main() \ No newline at end of file + import rclpy # 這邊是為了測試而加的 + main_develop() From 4dcff44bd3caa6849dfc0987fea1f5b0cc4b9499 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Apr 2025 22:58:01 +0800 Subject: [PATCH 07/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 249 ++++++++++++++++++ 1 file changed, 249 insertions(+) create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py new file mode 100644 index 0000000..0c2efb7 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -0,0 +1,249 @@ + +''' +這個檔案只有一個 class +是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生 + +主要概念是將 "離散的" mavlink 參數轉換成 ROS topic +包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit + +publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> +''' + +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +from theLogger import setup_logger +import math + + +logger = setup_logger("mavlinkPublish.py") + +class mavlink_publisher(): + + prefix_path = 'MavLinkBus' + + def create_flightMode(self, sysid, component_obj): + # target topic name # 請跟這個 method 的名稱保持一致 + target_topic = 'flightMode' + + # 這邊要檢查 flight_mode 是否存在 + try: + _ = component_obj.emitParams['flightMode_mode'] + except KeyError: + # 這個 component id 還不存在 + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + + # 若存在則 建立 publisher object 並回傳 true + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] + + return True + + def packEmit_flightMode(cls, emitParams, publisher): + msg_str = emitParams['flightMode_mode'] + msg = std_msgs.msg.String() + msg.data = msg_str + publisher.publish(msg) + pass + + # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同 ros2 topic 訊息 請放在這裡 ↓↓↓↓↓↓↓↓↓↓↓↓ + def euler_to_quaternion(cls,roll, pitch, yaw): + qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) + qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) + return [qx, qy, qz, qw] + + def create_attitude(self, sysid, component_obj): + # target topic name # 請跟這個 method 的名稱保持一致 + target_topic = 'attitude' + + # 這邊要檢查 attitude 是否存在 + try: + _ = component_obj.emitParams['attitude'] + except KeyError: + # 這個 component id 還不存在 + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + + # 若存在則 建立 publisher object 並回傳 true + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] + + def packEmit_attitude(self, emitParams, publisher): + mav_msg = emitParams['attitude'] + msg = sensor_msgs.msg.Imu() + x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) + msg.orientation.x = x + msg.orientation.y = y + msg.orientation.z = z + msg.orientation.w = w + msg.angular_velocity.x = mav_msg.rollspeed + msg.angular_velocity.y = mav_msg.pitchspeed + msg.angular_velocity.z = mav_msg.yawspeed + publisher.publish(msg) + pass + + def create_local_position_pose(self, sysid, component_obj): + target_topic = 'local_position/pose' + try: + _ = component_obj.emitParams['local_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] + + def packEmit_local_pose(cls, emitParams, publisher): + mav_msg = emitParams['local_position'] + msg = geometry_msgs.msg.Point() + msg.x = mav_msg.x + msg.y = mav_msg.y + msg.z = mav_msg.z + publisher.publish(msg) + pass + + def create_local_position_velocity(self, sysid, component_obj): + target_topic = 'local_position/velocity' + try: + _ = component_obj.emitParams['local_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] + + def packEmit_local_vel(cls, emitParams, publisher): + mav_msg = emitParams['local_position'] + msg = geometry_msgs.msg.Vector3() + msg.x = mav_msg.vx + msg.y = mav_msg.vy + msg.z = mav_msg.vz + publisher.publish(msg) + pass + + def create_global_global(self, sysid, component_obj): + target_topic = 'global_position/global' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] + + def packEmit_global_global(cls, emitParams, publisher): + mav_msg = emitParams['global_position'] + msg = sensor_msgs.msg.NavSatFix() + msg.latitude = mav_msg.lat/1e7 + msg.longitude = mav_msg.lon/1e7 + msg.altitude = mav_msg.alt/1e3 + publisher.publish(msg) + pass + + def create_global_rel(self, sysid, component_obj): + target_topic = 'global_position/rel_alt' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] + + def packEmit_global_rel(cls, emitParams, publisher): + mav_msg = emitParams['global_position'] + msg = std_msgs.msg.Float64() + msg.data = mav_msg.relative_alt/1e3 + publisher.publish(msg) + pass + + def create_global_vel(self, sysid, component_obj): + target_topic = 'global_position/gp_vel' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_vel] + + def packEmit_global_vel(cls, emitParams, publisher): + mav_msg = emitParams['global_position'] + msg = geometry_msgs.msg.Vector3() + msg.x = mav_msg.vx/1e2 + msg.y = mav_msg.vy/1e2 + msg.z = mav_msg.vz/1e2 + publisher.publish(msg) + pass + + def create_global_hdg(self, sysid, component_obj): + target_topic = 'global_position/compass_hdg' + try: + _ = component_obj.emitParams['global_position'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_hdg] + + def packEmit_global_hdg(cls, emitParams, publisher): + mav_msg = emitParams['global_position'] + msg = std_msgs.msg.Float64() + msg.data = mav_msg.hdg/1e2 + publisher.publish(msg) + pass + + def create_vfr_hud(self, sysid, component_obj): + target_topic = 'vfr_hud' + try: + _ = component_obj.emitParams['vfr_hud'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] + + def packEmit_vfr_hud(cls, emitParams, publisher): + mav_msg = emitParams['vfr_hud'] + msg = mavros_msgs.msg.VfrHud() + msg.airspeed = mav_msg.airspeed + msg.groundspeed = mav_msg.groundspeed + msg.heading = mav_msg.heading + msg.throttle = float(mav_msg.throttle) + msg.altitude = mav_msg.alt + msg.climb = mav_msg.climb + publisher.publish(msg) + pass + + def create_battery(self, sysid, component_obj): + target_topic = 'battery' + try: + _ = component_obj.emitParams['battery'] + except KeyError: + logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) + return False + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] + + def packEmit_battery(cls, emitParams, publisher): + mav_msg = emitParams['battery'] + msg = sensor_msgs.msg.BatteryState() + msg.voltage = mav_msg.voltages[0]/1e3 + publisher.publish(msg) + pass + + # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file From 396da0910041307533da33cab764d96d3d238738 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 17 Apr 2025 14:10:10 +0800 Subject: [PATCH 08/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 23 +------------------ 1 file changed, 1 insertion(+), 22 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index 0c2efb7..e16ba36 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -59,18 +59,14 @@ class mavlink_publisher(): return [qx, qy, qz, qw] def create_attitude(self, sysid, component_obj): - # target topic name # 請跟這個 method 的名稱保持一致 target_topic = 'attitude' - # 這邊要檢查 attitude 是否存在 try: _ = component_obj.emitParams['attitude'] except KeyError: - # 這個 component id 還不存在 logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) return False - # 若存在則 建立 publisher object 並回傳 true topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] @@ -187,24 +183,6 @@ class mavlink_publisher(): publisher.publish(msg) pass - def create_global_hdg(self, sysid, component_obj): - target_topic = 'global_position/compass_hdg' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_hdg] - - def packEmit_global_hdg(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = std_msgs.msg.Float64() - msg.data = mav_msg.hdg/1e2 - publisher.publish(msg) - pass - def create_vfr_hud(self, sysid, component_obj): target_topic = 'vfr_hud' try: @@ -245,5 +223,6 @@ class mavlink_publisher(): msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass + # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file From 53c062a2189ebe29a826dc4b01d5b67acfe548ce Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 17 Apr 2025 14:10:21 +0800 Subject: [PATCH 09/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- src/fc_network_adapter/fc_network_adapter/mavlinkObject.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index deaf82f..2c245c4 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -414,7 +414,6 @@ def main_develop(args=None): analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid]) analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid]) analyzer.create_global_vel(sysid, analyzer.mavlink_systems[sysid].components[compid]) - analyzer.create_global_hdg(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]) end_time = time.time() @@ -424,7 +423,7 @@ def main_develop(args=None): start_time = time.time() show_time = time.time() - while time.time() - start_time < 20: + while time.time() - start_time < 2000: try: # rclpy.spin(analyzer) analyzer.emit_info() # 這邊是測試 node 的運行 @@ -435,7 +434,6 @@ def main_develop(args=None): analyzer.destroy_node() rclpy.shutdown() - # 結束程式 退出所有 thread mavlink_object3.stop() mavlink_object3.thread.join() From d8ef37fdc0981d906d5c0abe7a0359020b6085a2 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 29 Apr 2025 10:49:50 +0800 Subject: [PATCH 10/14] PyQt6 GUI --- src/unitdev01/unitdev01/gui.py | 392 +++++++++++++++++++++++++++++++++ 1 file changed, 392 insertions(+) create mode 100644 src/unitdev01/unitdev01/gui.py diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py new file mode 100644 index 0000000..623a0d2 --- /dev/null +++ b/src/unitdev01/unitdev01/gui.py @@ -0,0 +1,392 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, + QWidget, QLabel, QSplitter, QScrollArea, + QSizePolicy) +from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal +from PyQt6.QtGui import QColor +import math +import re +from threading import Lock +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import String, Float64 +from mavros_msgs.msg import VfrHud + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + # 主题检测定时器 + self.create_timer(1.0, self.scan_topics) + + def scan_topics(self): + topics = self.get_topic_names_and_types() + drone_pattern = re.compile(r'/MavLinkBus/(s\d+)/(\w+)') + + found_drones = set() + for topic_name, _ in topics: + if match := drone_pattern.match(topic_name): + drone_id, topic_type = match.groups() + found_drones.add(drone_id) + with self.lock: + self.drone_topics.setdefault(drone_id, set()).add(topic_type) + + for drone_id in found_drones: + if not hasattr(self, f'drone_{drone_id}_subs'): + self.setup_drone_subs(drone_id) + + def setup_drone_subs(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + subs = { + 'attitude': self.create_subscription( + Imu, + f'{base_topic}/attitude', + lambda msg, did=drone_id: self.attitude_callback(did, msg), + 10 + ), + 'battery': self.create_subscription( + BatteryState, + f'{base_topic}/battery', + lambda msg, did=drone_id: self.battery_callback(did, msg), + 10 + ), + 'flightMode': self.create_subscription( + String, + f'{base_topic}/flightMode', + lambda msg, did=drone_id: self.mode_callback(did, msg), + 10 + ), + 'global': self.create_subscription( + NavSatFix, + f'{base_topic}/global_position/global', + lambda msg, did=drone_id: self.gps_callback(did, msg), + 10 + ), + 'rel_alt': self.create_subscription( + Float64, + f'{base_topic}/global_position/rel_alt', + lambda msg, did=drone_id: self.altitude_callback(did, msg), + 10 + ), + 'local_pose': self.create_subscription( + Point, + f'{base_topic}/local_position/pose', + lambda msg, did=drone_id: self.local_pose_callback(did, msg), + 10 + ), + 'local_vel': self.create_subscription( + Vector3, + f'{base_topic}/local_position/velocity', + lambda msg, did=drone_id: self.local_vel_callback(did, msg), + 10 + ), + 'vfr_hud': self.create_subscription( + VfrHud, + f'{base_topic}/vfr_hud', + lambda msg, did=drone_id: self.hud_callback(did, msg), + 10 + ) + } + + setattr(self, f'drone_{drone_id}_subs', subs) + self.signals.update_signal.emit('new_drone', drone_id, None) + + def quaternion_to_euler(self, q): + sinr_cosp = 2 * (q.w * q.x + q.y * q.z) + cosr_cosp = 1 - 2 * (q.x**2 + q.y**2) + roll = math.atan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (q.w * q.y - q.z * q.x) + pitch = math.asin(sinp) if abs(sinp) < 1 else math.copysign(math.pi/2, sinp) + + siny_cosp = 2 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1 - 2 * (q.y**2 + q.z**2) + yaw = math.atan2(siny_cosp, cosy_cosp) + + return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) + + # 回调函数 + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.signals.update_signal.emit('attitude', drone_id, { + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': (msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z) + }) + + def battery_callback(self, drone_id, msg): + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': msg.voltage + }) + + def mode_callback(self, drone_id, msg): + self.signals.update_signal.emit('mode', drone_id, msg.data) + + def gps_callback(self, drone_id, msg): + self.signals.update_signal.emit('gps', drone_id, { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + }) + + def local_vel_callback(self, drone_id, msg): + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + }) + + def altitude_callback(self, drone_id, msg): + self.signals.update_signal.emit('altitude', drone_id, msg.data) + + def local_pose_callback(self, drone_id, msg): + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + }) + + def hud_callback(self, drone_id, msg): + self.signals.update_signal.emit('hud', drone_id, { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + }) + +class ControlStationUI(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle('GCS') + self.resize(1400, 900) + self.drones = {} + + # 初始化ROS2 + rclpy.init() + self.monitor = DroneMonitor() + self.monitor.signals.update_signal.connect(self.update_ui) + + # ROS执行器配置 + self.executor = rclpy.executors.SingleThreadedExecutor() + self.executor.add_node(self.monitor) + + # 初始化UI + self.init_ui() + + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(50) + + def init_ui(self): + main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左侧信息面板 + left_panel = QWidget() + left_layout = QVBoxLayout(left_panel) + left_layout.setContentsMargins(5, 5, 5, 5) + + # 信息滚动区域 + scroll_area = QScrollArea() + scroll_area.setWidgetResizable(True) + self.info_container = QWidget() + self.info_layout = QVBoxLayout(self.info_container) + self.info_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + scroll_area.setWidget(self.info_container) + + left_layout.addWidget(scroll_area) + + # 右侧地图区域 + self.map_widget = QLabel() + self.map_widget.setAlignment(Qt.AlignmentFlag.AlignCenter) + self.map_widget.setStyleSheet(""" + QLabel { + background-color: #1A1A1A; + color: #AAAAAA; + font-size: 18px; + border: 2px solid #333; + border-radius: 10px; + padding: 20px; + } + """) + self.update_map_display("等待GPS數據...") + + main_splitter.addWidget(left_panel) + main_splitter.addWidget(self.map_widget) + main_splitter.setSizes([400, 1000]) + + self.setCentralWidget(main_splitter) + + def create_drone_panel(self, drone_id): + panel = QWidget() + panel.setObjectName(f"panel_{drone_id}") + panel.setStyleSheet(""" + QWidget#panel_%s { + background-color: #2A2A2A; + border-radius: 8px; + margin: 6px; + padding: 10px; + border: 1px solid #444; + } + QLabel { + color: #DDD; + font-size: 12px; + padding: 2px; + } + """ % drone_id) + + layout = QVBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + layout.setSpacing(4) + + # 顶部标题栏 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # ID显示 + id_label = QLabel(drone_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + # 状态指示灯 + status_light = QLabel("●") + status_light.setStyleSheet("color: #00FF00; font-size: 16px;") + + header_layout.addWidget(id_label) + header_layout.addWidget(status_light) + header_layout.addStretch() + + layout.addWidget(header) + + # 数据字段（带标签） + self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--") + self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "-- V") + self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "等待定位...") + self.create_data_row(layout, "Local:", f"{drone_id}_local", "X: -- Y: -- Z: --") + self.create_data_row(layout, "速度:", f"{drone_id}_velocity", "VX: -- VY: -- VZ: --") + self.create_data_row(layout, "姿態:", f"{drone_id}_attitude", "Roll: -- Pitch: -- Yaw: --") + + return panel + + def create_data_row(self, layout, title, object_name, default): + row = QWidget() + hbox = QHBoxLayout(row) + hbox.setContentsMargins(0, 0, 0, 0) + + # 标题标签 + title_label = QLabel(title) + title_label.setStyleSheet("color: #888; min-width: 80px;") + title_label.setSizePolicy(QSizePolicy.Policy.Fixed, QSizePolicy.Policy.Preferred) + + # 数据标签 + value_label = QLabel(default) + value_label.setObjectName(object_name) + value_label.setWordWrap(True) + value_label.setStyleSheet("margin-left: 10px;") + + hbox.addWidget(title_label) + hbox.addWidget(value_label) + layout.addWidget(row) + + def update_ui(self, msg_type, drone_id, data): + if msg_type == 'new_drone': + self.add_drone(drone_id) + return + + if not (panel := self.drones.get(drone_id)): + return + + if msg_type == 'mode': + self.update_field(panel, drone_id, 'mode', f"模式: {data}", + '#FF5555' if '返航' in data else '#55FF55') + + elif msg_type == 'battery': + voltage = data.get('voltage', 0) + self.update_field(panel, drone_id, 'battery', + f"{voltage:.1f} V", + '#FF6464' if voltage < 12 else '#FFFFFF') + + elif msg_type == 'gps': + text = (f"緯度: {data['lat']:.6f}\n" + f"經度: {data['lon']:.6f}\n" + f"高度: {data['alt']:.1f} m") + self.update_field(panel, drone_id, 'gps', text) + self.update_map_display(f"Drone {drone_id}\n{text}") + + elif msg_type == 'local_pose': + text = (f"X: {data['x']:.1f} m\n" + f"Y: {data['y']:.1f} m\n" + f"Z: {data['z']:.1f} m") + self.update_field(panel, drone_id, 'local', text) + + elif msg_type == 'velocity': + text = (f"VX: {data['vx']:.1f} m/s\n" + f"VY: {data['vy']:.1f} m/s\n" + f"VZ: {data['vz']:.1f} m/s") + self.update_field(panel, drone_id, 'velocity', text) + + elif msg_type == 'attitude': + text = (f"Roll: {data['roll']:.1f}°\n" + f"Pitch: {data['pitch']:.1f}°\n" + f"Yaw: {data['yaw']:.1f}°") + self.update_field(panel, drone_id, 'attitude', text) + + elif msg_type == 'hud': + text = (f"空速: {data['airspeed']:.1f} m/s\n" + f"地速: {data['groundspeed']:.1f} m/s\n" + f"航向: {data['heading']:.1f}°") + self.update_field(panel, drone_id, 'hud', text) + + def update_field(self, panel, drone_id, field, text, color=None): + if label := panel.findChild(QLabel, f"{drone_id}_{field}"): + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def add_drone(self, drone_id): + if drone_id not in self.drones: + panel = self.create_drone_panel(drone_id) + self.info_layout.addWidget(panel) + self.drones[drone_id] = panel + + def update_map_display(self, text): + self.map_widget.setText(text) + self.map_widget.repaint() + + def spin_ros(self): + try: + self.executor.spin_once(timeout_sec=0) + except Exception as e: + print(f"ROS spin error: {e}") + + def closeEvent(self, event): + self.monitor.destroy_node() + self.executor.shutdown() + rclpy.shutdown() + event.accept() + +if __name__ == '__main__': + app = QApplication([]) + station = ControlStationUI() + station.show() + app.exec() \ No newline at end of file From 30239b87f82333d8b8f3086f48ec027482736ea2 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 6 May 2025 11:14:44 +0800 Subject: [PATCH 11/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- src/fc_network_adapter/fc_network_adapter/mavlinkObject.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 2c245c4..d3f2214 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -412,10 +412,9 @@ def main_develop(args=None): 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_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid]) - analyzer.create_global_vel(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_flightMode: {end_time - start_time} seconds") @@ -423,7 +422,7 @@ def main_develop(args=None): start_time = time.time() show_time = time.time() - while time.time() - start_time < 2000: + while time.time() - start_time < 200000: try: # rclpy.spin(analyzer) analyzer.emit_info() # 這邊是測試 node 的運行 From 72a59b1a3a9cec5e4fa8e0bc80c8ce74e19503fb Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 6 May 2025 11:14:56 +0800 Subject: [PATCH 12/14] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 22 +------------------ 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index e16ba36..028e3ee 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -159,27 +159,7 @@ class mavlink_publisher(): def packEmit_global_rel(cls, emitParams, publisher): mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() - msg.data = mav_msg.relative_alt/1e3 - publisher.publish(msg) - pass - - def create_global_vel(self, sysid, component_obj): - target_topic = 'global_position/gp_vel' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_vel] - - def packEmit_global_vel(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = geometry_msgs.msg.Vector3() - msg.x = mav_msg.vx/1e2 - msg.y = mav_msg.vy/1e2 - msg.z = mav_msg.vz/1e2 + msg.data = float(mav_msg.relative_alt/1e3) publisher.publish(msg) pass From 945d79a6fa2866c05d75ff99062750db22a300ad Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 6 May 2025 11:15:28 +0800 Subject: [PATCH 13/14] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 110 +++++++++++++++++++-------------- 1 file changed, 65 insertions(+), 45 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 623a0d2..d9e7a0f 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -3,11 +3,14 @@ import rclpy from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, - QSizePolicy) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal + QSizePolicy, QApplication) +from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal, QUrl from PyQt6.QtGui import QColor +from PyQt6.QtWebEngineWidgets import QWebEngineView import math import re +import os +import sys from threading import Lock from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu @@ -138,8 +141,8 @@ class DroneMonitor(Node): def gps_callback(self, drone_id, msg): self.signals.update_signal.emit('gps', drone_id, { 'lat': msg.latitude, - 'lon': msg.longitude, - 'alt': msg.altitude + 'lon': msg.longitude + ,'alt': msg.altitude }) def local_vel_callback(self, drone_id, msg): @@ -150,7 +153,9 @@ class DroneMonitor(Node): }) def altitude_callback(self, drone_id, msg): - self.signals.update_signal.emit('altitude', drone_id, msg.data) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': msg.data + }) def local_pose_callback(self, drone_id, msg): self.signals.update_signal.emit('local_pose', drone_id, { @@ -212,26 +217,43 @@ class ControlStationUI(QMainWindow): left_layout.addWidget(scroll_area) # 右侧地图区域 - self.map_widget = QLabel() - self.map_widget.setAlignment(Qt.AlignmentFlag.AlignCenter) - self.map_widget.setStyleSheet(""" - QLabel { - background-color: #1A1A1A; - color: #AAAAAA; - font-size: 18px; - border: 2px solid #333; - border-radius: 10px; - padding: 20px; + self.map_view = QWebEngineView() + + inline_html = ''' + + + + + + + +
+ + + ''' + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self.on_map_loaded) + main_splitter.addWidget(left_panel) - main_splitter.addWidget(self.map_widget) + main_splitter.addWidget(self.map_view) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) + def on_map_loaded(self, ok: bool): + if ok: + self.map_loaded = True + else: + print("⚠️ 地图页加载失败") + def create_drone_panel(self, drone_id): panel = QWidget() panel.setObjectName(f"panel_{drone_id}") @@ -280,11 +302,11 @@ class ControlStationUI(QMainWindow): # 数据字段（带标签） self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--") - self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "-- V") - self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "等待定位...") - self.create_data_row(layout, "Local:", f"{drone_id}_local", "X: -- Y: -- Z: --") - self.create_data_row(layout, "速度:", f"{drone_id}_velocity", "VX: -- VY: -- VZ: --") - self.create_data_row(layout, "姿態:", f"{drone_id}_attitude", "Roll: -- Pitch: -- Yaw: --") + self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "--") + self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "--") + self.create_data_row(layout, "高度:", f"{drone_id}_altitude", "--") + self.create_data_row(layout, "Local:", f"{drone_id}_local", "--") + self.create_data_row(layout, "HUD:", f"{drone_id}_hud", "--") return panel @@ -317,7 +339,7 @@ class ControlStationUI(QMainWindow): return if msg_type == 'mode': - self.update_field(panel, drone_id, 'mode', f"模式: {data}", + self.update_field(panel, drone_id, 'mode', f"{data}", '#FF5555' if '返航' in data else '#55FF55') elif msg_type == 'battery': @@ -327,18 +349,26 @@ class ControlStationUI(QMainWindow): '#FF6464' if voltage < 12 else '#FFFFFF') elif msg_type == 'gps': - text = (f"緯度: {data['lat']:.6f}\n" - f"經度: {data['lon']:.6f}\n" - f"高度: {data['alt']:.1f} m") + text = (f"緯度: {data['lat']:.6f}°\n" + f"經度: {data['lon']:.6f}°") self.update_field(panel, drone_id, 'gps', text) - self.update_map_display(f"Drone {drone_id}\n{text}") + if self.on_map_loaded: + js = f"updateDrone({data['lat']}, {data['lon']:.6f}, {drone_id})" + self.map_view.page().runJavaScript(js) + + elif msg_type == 'altitude': + text = (f"{data['altitude']:.1f} m") + self.update_field(panel, drone_id, 'altitude', text) elif msg_type == 'local_pose': - text = (f"X: {data['x']:.1f} m\n" - f"Y: {data['y']:.1f} m\n" - f"Z: {data['z']:.1f} m") + text = (f"({data['x']:.1f}, {data['y']:.1f}, {data['z']:.1f})") self.update_field(panel, drone_id, 'local', text) + elif msg_type == 'hud': + text = (f"地速: {data['groundspeed']:.1f} m/s\n" + f"航向: {data['heading']:.1f}°") + self.update_field(panel, drone_id, 'hud', text) + ''' elif msg_type == 'velocity': text = (f"VX: {data['vx']:.1f} m/s\n" f"VY: {data['vy']:.1f} m/s\n" @@ -350,13 +380,7 @@ class ControlStationUI(QMainWindow): f"Pitch: {data['pitch']:.1f}°\n" f"Yaw: {data['yaw']:.1f}°") self.update_field(panel, drone_id, 'attitude', text) - - elif msg_type == 'hud': - text = (f"空速: {data['airspeed']:.1f} m/s\n" - f"地速: {data['groundspeed']:.1f} m/s\n" - f"航向: {data['heading']:.1f}°") - self.update_field(panel, drone_id, 'hud', text) - + ''' def update_field(self, panel, drone_id, field, text, color=None): if label := panel.findChild(QLabel, f"{drone_id}_{field}"): label.setText(text) @@ -369,10 +393,6 @@ class ControlStationUI(QMainWindow): self.info_layout.addWidget(panel) self.drones[drone_id] = panel - def update_map_display(self, text): - self.map_widget.setText(text) - self.map_widget.repaint() - def spin_ros(self): try: self.executor.spin_once(timeout_sec=0) @@ -386,7 +406,7 @@ class ControlStationUI(QMainWindow): event.accept() if __name__ == '__main__': - app = QApplication([]) + app = QApplication(sys.argv) station = ControlStationUI() station.show() - app.exec() \ No newline at end of file + app.exec(app.exec()) \ No newline at end of file From 4d694f28b41451c4919fe1a51555e27e0c9685af Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 7 May 2025 23:45:14 +0800 Subject: [PATCH 14/14] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 80 ++++++++++++++++++++++++---------- 1 file changed, 56 insertions(+), 24 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index d9e7a0f..a81dbb2 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -191,6 +191,8 @@ class ControlStationUI(QMainWindow): self.executor.add_node(self.monitor) # 初始化UI + self.drone_positions = {} + self.map_loaded = False self.init_ui() # 定时处理ROS事件 @@ -220,24 +222,49 @@ class ControlStationUI(QMainWindow): self.map_view = QWebEngineView() inline_html = ''' - - - - - - - -
- - + + + + + + + + + + +
+ + + ''' self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) @@ -349,12 +376,11 @@ class ControlStationUI(QMainWindow): '#FF6464' if voltage < 12 else '#FFFFFF') elif msg_type == 'gps': - text = (f"緯度: {data['lat']:.6f}°\n" - f"經度: {data['lon']:.6f}°") + lat, lon = data['lat'], data['lon'] + self.drone_positions[drone_id] = (lat, lon) + text = (f"緯度: {lat:.6f}°\n" + f"經度: {lon:.6f}°") self.update_field(panel, drone_id, 'gps', text) - if self.on_map_loaded: - js = f"updateDrone({data['lat']}, {data['lon']:.6f}, {drone_id})" - self.map_view.page().runJavaScript(js) elif msg_type == 'altitude': text = (f"{data['altitude']:.1f} m") @@ -365,9 +391,15 @@ class ControlStationUI(QMainWindow): self.update_field(panel, drone_id, 'local', text) elif msg_type == 'hud': + heading = data['heading'] text = (f"地速: {data['groundspeed']:.1f} m/s\n" - f"航向: {data['heading']:.1f}°") + f"航向: {heading:.1f}°") self.update_field(panel, drone_id, 'hud', text) + + if self.map_loaded and drone_id in self.drone_positions: + lat, lon = self.drone_positions[drone_id] + js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});" + self.map_view.page().runJavaScript(js) ''' elif msg_type == 'velocity': text = (f"VX: {data['vx']:.1f} m/s\n"