From c5011898405c5b742198a0b793b48ea93890dc2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=96=87=E6=98=8C=E5=B8=9D=E9=88=9E?= Date: Tue, 18 Feb 2025 17:38:54 +0800 Subject: [PATCH 001/146] =?UTF-8?q?=E6=96=B0=E5=A2=9Epymavlink=5F01.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- pymavlink_01.py | 232 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 232 insertions(+) create mode 100644 pymavlink_01.py diff --git a/pymavlink_01.py b/pymavlink_01.py new file mode 100644 index 0000000..dc8ebb1 --- /dev/null +++ b/pymavlink_01.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python3 +from pymavlink import mavutil +import time +import math +import threading + +class DroneController: + def __init__(self, connection_string="udp:127.0.0.1:14550"): + # 建立 MAVLink 連接 + self.master = mavutil.mavlink_connection(connection_string) + + # 等待心跳包 + self.master.wait_heartbeat() + print("已建立連接!") + + def set_mode(self, mode): + """設置飛行模式""" + mode_id = self.master.mode_mapping()[mode] + self.master.mav.set_mode_send( + self.master.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + mode_id + ) + + for _ in range(5): + msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1) + if msg and msg.custom_mode == mode_id: + return True + return False + + def arm(self, arm_state=True): + """解鎖/上鎖無人機""" + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 1 if arm_state else 0, 0, 0, 0, 0, 0, 0 + ) + + for _ in range(5): + msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1) + if msg: + armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + if armed == arm_state: + return True + return False + + def takeoff(self, altitude): + """起飛到指定高度""" + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, + 0, 0, 0, 0, 0, 0, + altitude + ) + + while True: + msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + if msg: + current_alt = msg.relative_alt / 1000.0 + if abs(current_alt - altitude) < 0.5: + return True + time.sleep(0.1) + + def goto_position_local(self, x, y, z, vx=2, vy=2, vz=0, yaw=0, yaw_rate=0): + """ + 飛到指定的本地座標位置 + x, y, z: 目標位置(公尺) + """ + # NED系統中，向上飛需要給負值 + z = -z # 將輸入的z值轉換為NED系統 + + # 重要：type_mask 設為 0 表示使用該維度 + # 我們只想使用位置控制 (x,y,z)，所以其他都設為 ignore + type_mask = ( + # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VX_IGNORE | # 8 + # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VY_IGNORE | # 16 + # mavutil.mavlink.POSITION_TARGET_TYPEMASK_VZ_IGNORE | # 32 + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | # 64 + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | # 128 + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | # 256 + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | # 1024 + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE # 2048 + ) + + # 或直接使用二進制: type_mask = 0b0000111111111000 + + print(f"發送位置指令: x={x}, y={y}, z={-z}") + + self.master.mav.set_position_target_local_ned_send( + 0, # timestamp + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + x, y, z, # 位置 + 0, 0, 0, # 速度 (設為 0) + 5, 5, 0, # 加速度 (設為 0) + 0, 0 # yaw, yaw_rate (設為 0) + ) + + # 監控位置變化 + print("開始監控位置變化...") + start_time = time.time() + while time.time() - start_time < 20: # 20秒超時 + msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1) + if msg: + dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2) + print(f"當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m") + if dist < 0.5: + print("到達目標位置") + return True + time.sleep(0.1) + + print("移動超時") + return False + + def get_current_state(self): + """獲取當前狀態""" + msg = self.master.recv_match(type='HEARTBEAT', blocking=True) + if msg: + mode = list(self.master.mode_mapping().keys())[list(self.master.mode_mapping().values()).index(msg.custom_mode)] + armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + return { + 'mode': mode, + 'armed': armed + } + return None + +def print_menu(): + """列印功能表""" + menu = [ + "\n=== 無人機控制界面 ===", + "1. 切換至 GUIDED 模式", + "2. 切換至 LAND 模式", + "3. 解鎖", + "4. 上鎖", + "5. 起飛", + "6. 飛到指定位置", + "7. 切換至 RTL 模式(返航)", + "8. 查看當前狀態", + "9. 退出", + "0. 顯示此清單", + "====================" + ] + for item in menu: + print(item) + +def main(): + drone = DroneController() + print_menu() + + while True: + try: + choice = input("\n請輸入指令編號: ") + + if choice == '0': + print_menu() + + elif choice == '1': + if drone.set_mode("GUIDED"): + print("已切換至 GUIDED 模式") + else: + print("切換模式失敗") + + elif choice == '2': + if drone.set_mode("LAND"): + print("已切換至 LAND 模式") + else: + print("切換模式失敗") + + elif choice == '3': + if drone.arm(True): + print("無人機已解鎖") + else: + print("解鎖失敗") + + elif choice == '4': + if drone.arm(False): + print("無人機已上鎖") + else: + print("上鎖失敗") + + elif choice == '5': + altitude = float(input("請輸入起飛高度(公尺): ")) + if drone.takeoff(altitude): + print(f"已到達目標高度: {altitude}公尺") + else: + print("起飛失敗") + + elif choice == '6': + x = float(input("請輸入X座標(公尺): ")) + y = float(input("請輸入Y座標(公尺): ")) + z = float(input("請輸入Z座標(公尺,負值代表向下): ")) + if drone.goto_position_local(x, y, z): + print("已到達目標位置") + else: + print("移動失敗") + + elif choice == '7': + if drone.set_mode("RTL"): + print("已切換至 RTL 模式，無人機正在返航") + else: + print("切換至 RTL 模式失敗") + + elif choice == '8': + state = drone.get_current_state() + if state: + print("\n=== 無人機狀態 ===") + print(f"模式: {state['mode']}") + print(f"解鎖狀態: {'已解鎖' if state['armed'] else '已上鎖'}") + print("==================") + else: + print("無法獲取狀態") + + elif choice == '9': + print("程式結束") + break + + else: + print("無效的指令，請重新輸入") + + except ValueError as e: + print(f"輸入錯誤: {e}") + except Exception as e: + print(f"發生錯誤: {e}") + +if __name__ == "__main__": + main() \ No newline at end of file From 5c5cadc9da381fbe616e3e1b4596db87a62e47a2 Mon Sep 17 00:00:00 2001 From: picard Date: Tue, 18 Feb 2025 17:42:01 +0800 Subject: [PATCH 002/146] mavlink to gazebo devtest 02 --- src/unitdev01/unitdev01/mavlink.py | 26 ++++----- src/unitdev02/setup.py | 3 +- src/unitdev02/unitdev02/test01.py | 86 ++++++++++++++++++++++-------- 3 files changed, 80 insertions(+), 35 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index e17aa74..430864e 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -94,23 +94,23 @@ class MAVLinkWorker(QThread): def arm(self, namespace, arm): sysid = int(namespace[-1]) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # Confirmation - 1 if arm else 0, - 0, 0, 0, 0, 0, 0 - ) # self.connection.mav.command_long_send( # sysid, # 1, # Component ID - # mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - # 0, - # 33, - # -1, - # 0, 0, 0, 0, 0 + # mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + # 0, # Confirmation + # 1 if arm else 0, + # 0, 0, 0, 0, 0, 0 # ) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + 0, + 33, + 1000000, + 0, 0, 0, 0, 0 + ) def takeoff(self, namespace, altitude): sysid = int(namespace[-1]) diff --git a/src/unitdev02/setup.py b/src/unitdev02/setup.py index 62d265b..f5d6e64 100644 --- a/src/unitdev02/setup.py +++ b/src/unitdev02/setup.py @@ -21,7 +21,8 @@ setup( entry_points={ 'console_scripts': [ 'showmavsimple = unitdev02.test01:mavlink_analyzer_simple', - 'fdm_switch_one = unitdev02.test01:fdm_swicth_example_one', + 'fdm_switch_one = unitdev02.test01:fdm_switch_example_one', + 'fdm_switch_two = unitdev02.test01:fdm_switch_example_two', ], }, ) diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index c3cb82c..7e1f96c 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -6,6 +6,10 @@ import time import socket import struct +# used at fdm_switch_example_two +import threading +import queue + def mavlink_analyzer_simple(count = 500): # rclpy.init() # node = rclpy.create_node('mavlink_analyzer_simple') @@ -32,6 +36,7 @@ def mavlink_analyzer_simple(count = 500): if msg.get_type() == 'SERVO_OUTPUT_RAW': print(msg) + pass else: print('No message yet, 1 second for data to fill') @@ -113,34 +118,73 @@ def fdm_switch_example_two(): ''' 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 有轉換數據格式 + + time_usec + servo1_raw + servo16_raw ''' # read info from SITL via MAVLink - connection_string="udp:127.0.0.1:14550" + connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB connection = mavutil.mavlink_connection(connection_string) # set a target link pass JSON to Gazebo, as a client sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) server_address = ('127.0.0.1', 19002) + data_queue = queue.Queue() + servo_out = [0] * 16 + data_queue.put(servo_out) + + def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1): + interval = 1.0 / frame_rate_fdm + while True: + start_time = time.time() + try: + data = q.get(timeout=0.1) + if data is None: + break + last_data = data + except queue.Empty: + data = last_data + data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out) + sock.sendto(data, server_address) + frame_count_fdm += 1 + fdm_parser_example(data) # debug + + elapsed_time = time.time() - start_time + sleep_time = interval - elapsed_time + if sleep_time > 0: + sleep(sleep_time) + + Running = True + thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue)) + thread.start() + + while Running: + msgb1 = None + msg = connection.recv_msg() + while msg : + if msg.get_type() == 'SERVO_OUTPUT_RAW': + msgb1 = msg + # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值 + msg = connection.recv_msg() + + if msgb1: + for i in range(16): + servo_out[i] = getattr(msgb1, f'servo{i+1}_raw') + data_queue.put(servo_out) + msgb1 = None + # Running = False # debug + else: + # print('No message yet, 10 ms for data to fill') + sleep(0.01) + + # Example of putting data into the queue + # data_queue.put(data) + # To stop the thread, you can put None into the queue + # data_queue.put(None) + print('Start') -mavlink_analyzer_simple() - -# connection_string="udp:127.0.0.1:14550" -# connection = mavutil.mavlink_connection(connection_string) - -# connection.mav.command_long_send( -# 1, -# 1, -# mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, -# 0, 33, 1000000, 0, 0, 0, 0, 0 -# ) - -# connection.mav.command_long_send( -# 1, -# 1, # Component ID -# mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, -# 0, # Confirmation -# 1, # 0: disarm, 1: arm -# 0, 0, 0, 0, 0, 0 -# ) \ No newline at end of file +# fdm_switch_example_two() +# fdm_parser_example() \ No newline at end of file From a8968e25b3afc44889732f36bcd58e275be807b3 Mon Sep 17 00:00:00 2001 From: wenchun Date: Tue, 18 Feb 2025 18:08:40 +0800 Subject: [PATCH 003/146] =?UTF-8?q?mavlink=E5=96=AE=E6=A9=9F=E4=BD=8D?= =?UTF-8?q?=E7=BD=AE=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- pymavlink_01.py => src/unitdev03/pymavlink_01.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename pymavlink_01.py => src/unitdev03/pymavlink_01.py (100%) diff --git a/pymavlink_01.py b/src/unitdev03/pymavlink_01.py similarity index 100% rename from pymavlink_01.py rename to src/unitdev03/pymavlink_01.py From a8b36c391f0b90daaa7a97882067b44dab13abf7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:01:35 +0800 Subject: [PATCH 004/146] Update 'src/unitdev01/unitdev01/interface.py' --- src/unitdev01/unitdev01/interface.py | 499 ++++++++++++++++++--------- 1 file changed, 345 insertions(+), 154 deletions(-) diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py index 5699e46..9078880 100644 --- a/src/unitdev01/unitdev01/interface.py +++ b/src/unitdev01/unitdev01/interface.py @@ -1,141 +1,289 @@ import sys -import numpy as np +import time +import math import rclpy -from rclpy.executors import SingleThreadedExecutor from PyQt5 import QtWidgets from PyQt5.QtCore import QThread, pyqtSignal from PyQt5.QtWidgets import QVBoxLayout, QHBoxLayout, QLabel, QLineEdit, QPushButton, QCheckBox, QWidget, QMainWindow, QComboBox -from std_msgs.msg import Float64 -from mavros_msgs.srv import SetMode, CommandBool, CommandTOL -from mavros_msgs.msg import State, GPSRAW, VfrHud -from sensor_msgs.msg import NavSatFix, BatteryState, Imu -from geometry_msgs.msg import PoseStamped -from mavros.base import SENSOR_QOS - -class ROS2Worker(QThread): - state_signal = pyqtSignal(str, str) # 新增 namespace 參數 - battery_signal = pyqtSignal(str, float) # 新增 namespace 參數 - gps_signal = pyqtSignal(str, float, float) # 新增 namespace 參數 +from pymavlink import mavutil + +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) # 新增 namespace 參數 - imu_signal = pyqtSignal(str, float, float, float, float) + local_position_signal = pyqtSignal(str, float, float, float) + imu_signal = pyqtSignal(str, float) hdg_signal = pyqtSignal(str, float) - groundspeed_signal = pyqtSignal(str, float) # 新增 namespace 與 groundspeed 值 + 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) - def __init__(self, namespaces): + def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): super().__init__() - namespaces = ['uav1', 'uav2', 'uav3'] - self.node = rclpy.create_node('multi_uav_control_app') - self.executor = SingleThreadedExecutor() # 創建單執行器 - self.executor.add_node(self.node) # 添加節點到執行器 + self.namespaces = ['UAV1', 'UAV4', 'UAV5'] + self.connection = mavutil.mavlink_connection(usb, baud=57600) + self.connection.wait_heartbeat() + for sysid in self.namespaces: + sysid = int(sysid.replace('UAV', '')) + self.set_sr_params(sysid) self.running = True - for namespace in namespaces: - self.node.create_subscription(State, f'/{namespace}/state', - lambda msg, ns=namespace: self.state_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(BatteryState, f'/{namespace}/battery', - lambda msg, ns=namespace: self.battery_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(NavSatFix, f'/{namespace}/global_position/global', - lambda msg, ns=namespace: self.gps_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(GPSRAW, f'/{namespace}/gpsstatus/gps1/raw', - lambda msg, ns=namespace: self.gps_status_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(PoseStamped, f'/{namespace}/local_position/pose', - lambda msg, ns=namespace: self.local_position_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(Imu, f'/{namespace}/imu/data', - lambda msg, ns=namespace: self.imu_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(Float64, f'/{namespace}/global_position/compass_hdg', - lambda msg, ns=namespace: self.heading_callback(msg, ns), qos_profile=SENSOR_QOS) - self.node.create_subscription(VfrHud, f'/{namespace}/vfr_hud', - lambda msg, ns=namespace: self.vfr_hud_callback(msg, ns), qos_profile=SENSOR_QOS) - - self.clients = {namespace: { - "set_mode": self.node.create_client(SetMode, f'/{namespace}/set_mode'), - "arming": self.node.create_client(CommandBool, f'/{namespace}/cmd/arming'), - "takeoff": self.node.create_client(CommandTOL, f'/{namespace}/cmd/takeoff'), - "land": self.node.create_client(CommandTOL, f'/{namespace}/cmd/land'), - "local_position": self.node.create_publisher(PoseStamped, f'/{namespace}/setpoint_position/local', 10) - } for namespace in namespaces} + # 設置需要監控的訊息類型 + 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 = {} + + for namespace in self.namespaces: + self.request_param(namespace, "SR1_EXTRA1") def run(self): - while self.running and rclpy.ok(): - self.executor.spin_once(timeout_sec=0.1) + while self.running: + try: + current_time = time.time() + msg = self.connection.recv_msg() + 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) + + # 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 # MAVLink sequence numbers are modulo 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 + + # 每隔一段時間更新 + if current_time - self.start_time[sysid] >= 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_signal.emit(namespace, self.frequency[sysid]) + + # 發送 timesync request + self.connection.mav.timesync_send( + 0, #tc1 + int(time.time() * 1e9) #ts1 + ) + #self.send_heartbeat + + 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) + + 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(time.time() * 1e9) - msg.ts1) / 1e6 + if(round_trip_time<1e6): + self.ping_signal.emit(namespace, round_trip_time) + + elif msg.get_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.executor.shutdown() - self.node.destroy_node() - - def state_callback(self, msg, namespace): - self.state_signal.emit(namespace, msg.mode) - - def battery_callback(self, msg, namespace): - self.battery_signal.emit(namespace, msg.percentage) - - def gps_callback(self, msg, namespace): - self.gps_signal.emit(namespace, msg.latitude, msg.longitude) - - def gps_status_callback(self, msg, namespace): - self.gps_status_signal.emit(namespace, msg.fix_type, msg.satellites_visible) + self.connection.close() - def local_position_callback(self, msg, namespace): - self.local_position_signal.emit(namespace, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) - - def imu_callback(self, msg, namespace): - self.imu_signal.emit(namespace, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) - - def heading_callback(self, msg, namespace): - self.hdg_signal.emit(namespace, msg.data) - - def vfr_hud_callback(self, msg, namespace): - self.groundspeed_signal.emit(namespace, msg.groundspeed) - def set_mode(self, namespace, mode): - req = SetMode.Request() - req.custom_mode = mode - self.clients[namespace]["set_mode"].call_async(req) - self.node.get_logger().info(f'{namespace}: 模式切換: {mode}') - - def arm(self, namespace, arm: bool): - req = CommandBool.Request() - req.value = arm - self.clients[namespace]["arming"].call_async(req) - self.node.get_logger().info(f'{namespace}: {"解鎖" if arm else "上鎖"}') + 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 + self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP] + sysid, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + mode + ) + + def arm(self, namespace, arm): + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400) + 0, # Confirmation + 1 if arm else 0, + 0, 0, 0, 0, 0, 0 + ) def takeoff(self, namespace, altitude): - req = CommandTOL.Request() - req.altitude = altitude - req.latitude = 0.0 - req.longitude = 0.0 - req.yaw = 0.0 - req.min_pitch = 0.0 - self.clients[namespace]["takeoff"].call_async(req) - self.node.get_logger().info(f'{namespace}: 起飛到高度 {altitude}') + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, #MAV_CMD_NAV_TAKEOFF (22) + 0, # Confirmation (0 = first transmission) + 0, 0, 0, 0, # Parameters 1-4 (Unused in takeoff command) + 0, 0, altitude # Latitude, Longitude, Altitude (target_altitude in meters) + ) def land(self, namespace): - req = CommandTOL.Request() - req.altitude = 0.0 - req.latitude = 0.0 - req.longitude = 0.0 - req.yaw = 0.0 - req.min_pitch = 0.0 - self.clients[namespace]["land"].call_async(req) - self.node.get_logger().info(f'{namespace}: 降落') + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.command_long_send( + sysid, + 1, # Component ID + mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21) + 0, + 0, 0, 0, 0, + 0, 0, 0 + ) def set_local_position(self, namespace, x, y, z): - pose_msg = PoseStamped() - pose_msg.pose.position.x = x - pose_msg.pose.position.y = y - pose_msg.pose.position.z = z - self.clients[namespace]["local_position"].publish(pose_msg) - self.node.get_logger().info(f'{namespace}: Published local position: X={x}, Y={y}, Z={z}') + sysid = int(namespace.replace('UAV', '')) + self.connection.mav.set_position_target_local_ned_send( + 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 + ) + + 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 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] + 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) class DroneControlApp(QMainWindow): def __init__(self): super().__init__() - self.namespaces = ['uav1', 'uav2', 'uav3'] - self.workers = ROS2Worker(self.namespaces) + 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) @@ -144,7 +292,10 @@ 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.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.start() def initUI(self): @@ -174,15 +325,19 @@ class DroneControlApp(QMainWindow): # 狀態顯示 self.uav_labels[namespace] = { - "status": QLabel(f"{namespace} 狀態：等待數據..."), - "battery": QLabel(f"{namespace} 電量：等待數據..."), - "gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"), - "fix": QLabel(f"{namespace} Fix Type：等待數據..."), - "satellites": QLabel(f"{namespace} 衛星數量：等待數據..."), - "local_position": QLabel(f"{namespace} Local Position：等待數據..."), - "groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."), - "pitch": QLabel(f"{namespace} Pitch：等待數據..."), - "heading": QLabel(f"{namespace} Heading：等待數據..."), + "status": QLabel("狀態：等待數據..."), + "battery": QLabel("電量：等待數據..."), + "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("頻率：等待數據..."), + "param": QLabel("SR1_EXTRA1：等待數據...") } # 把每個資訊添加到該 UAV 的垂直佈局中 @@ -194,6 +349,16 @@ class DroneControlApp(QMainWindow): 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() @@ -225,19 +390,26 @@ class DroneControlApp(QMainWindow): main_layout.addWidget(self.setPositionButton) # 解鎖按鈕 + fly_layout = QHBoxLayout() self.armButton = QPushButton("解鎖") self.armButton.clicked.connect(self.arm_drone) - main_layout.addWidget(self.armButton) # 起飛按鈕 self.takeoffButton = QPushButton("起飛") self.takeoffButton.clicked.connect(self.takeoff_drone) - main_layout.addWidget(self.takeoffButton) # 降落按鈕 self.landButton = QPushButton("降落") self.landButton.clicked.connect(self.land_drone) - main_layout.addWidget(self.landButton) + fly_layout.addWidget(self.armButton) + fly_layout.addWidget(self.takeoffButton) + fly_layout.addWidget(self.landButton) + main_layout.addLayout(fly_layout) + + #設置重啟按鈕 + self.rebootButton = QPushButton("重啟") + self.rebootButton.clicked.connect(self.reboot_drone) + main_layout.addWidget(self.rebootButton) # 設置主佈局 central_widget = QWidget(self) @@ -252,13 +424,13 @@ class DroneControlApp(QMainWindow): event.accept() def update_state(self, namespace, mode): - self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}") + self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") def update_battery(self, namespace, percentage): - self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%") + self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%") def update_gps(self, namespace, latitude, longitude): - self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°") + 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 = { @@ -269,22 +441,32 @@ class DroneControlApp(QMainWindow): 4: "RTK Float", 5: "RTK Fix" }.get(fix_type, "Unknown") - self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}") - self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}") + 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"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})") + 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"{namespace} 地面速度：{groundspeed:.2f} m/s") + 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_imu(self, namespace, x, y, z, w): - q = np.array([x, y, z, w]) - pitch = self.quaternion_to_euler(q) - self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°") + def update_loss(self, namespace, loss): + self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") - def update_hdg(self, namespace, data): - self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{data:.2f}°") + def update_frequency(self, namespace, frequency): + self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz") + + def update_param(self, namespace, value): + self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") def change_mode(self): try: @@ -295,17 +477,6 @@ class DroneControlApp(QMainWindow): except Exception as e: QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") - def set_local_position(self): - try: - x = float(self.positionX.text().strip()) - y = float(self.positionY.text().strip()) - z = float(self.positionZ.text().strip()) - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z) - except ValueError: - QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") - def arm_drone(self): try: for namespace in self.namespaces: @@ -329,15 +500,35 @@ class DroneControlApp(QMainWindow): if self.uav_checkboxes[namespace].isChecked(): self.workers.land(namespace) - def quaternion_to_euler(self, q): - # Convert quaternion to Euler angles (roll, pitch, yaw) - x, y, z, w = q + def set_local_position(self): + try: + x = float(self.positionX.text().strip()) + y = float(self.positionY.text().strip()) + z = float(self.positionZ.text().strip()) + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_local_position(namespace, x, y, z) + 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, "錯誤", "請輸入有效的數值！") - # Pitch (y-axis rotation) - pitch_y = np.arcsin(2.0 * (w * y - z * x)) - - return pitch_y - def main(args=None): rclpy.init(args=args) app = QtWidgets.QApplication(sys.argv) From b8965c0bd6d070791d51b0ac721c99eb38091593 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:02:50 +0800 Subject: [PATCH 005/146] Update 'src/unitdev01/unitdev01/interface.py' --- src/unitdev01/unitdev01/interface.py | 499 +++++++++------------------ 1 file changed, 154 insertions(+), 345 deletions(-) diff --git a/src/unitdev01/unitdev01/interface.py b/src/unitdev01/unitdev01/interface.py index 9078880..5699e46 100644 --- a/src/unitdev01/unitdev01/interface.py +++ b/src/unitdev01/unitdev01/interface.py @@ -1,289 +1,141 @@ import sys -import time -import math +import numpy as np import rclpy +from rclpy.executors import SingleThreadedExecutor 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 - -class MAVLinkWorker(QThread): - state_signal = pyqtSignal(str, str) - battery_signal = pyqtSignal(str, float) - gps_signal = pyqtSignal(str, float, float) +from std_msgs.msg import Float64 +from mavros_msgs.srv import SetMode, CommandBool, CommandTOL +from mavros_msgs.msg import State, GPSRAW, VfrHud +from sensor_msgs.msg import NavSatFix, BatteryState, Imu +from geometry_msgs.msg import PoseStamped +from mavros.base import SENSOR_QOS + +class ROS2Worker(QThread): + state_signal = pyqtSignal(str, str) # 新增 namespace 參數 + battery_signal = pyqtSignal(str, float) # 新增 namespace 參數 + gps_signal = pyqtSignal(str, float, float) # 新增 namespace 參數 gps_status_signal = pyqtSignal(str, int, int) - local_position_signal = pyqtSignal(str, float, float, float) - imu_signal = pyqtSignal(str, float) + local_position_signal = pyqtSignal(str, float, float, float) # 新增 namespace 參數 + imu_signal = pyqtSignal(str, float, float, float, 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) + groundspeed_signal = pyqtSignal(str, float) # 新增 namespace 與 groundspeed 值 - def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): + def __init__(self, namespaces): super().__init__() - self.namespaces = ['UAV1', 'UAV4', 'UAV5'] - self.connection = mavutil.mavlink_connection(usb, baud=57600) - self.connection.wait_heartbeat() - for sysid in self.namespaces: - sysid = int(sysid.replace('UAV', '')) - self.set_sr_params(sysid) + namespaces = ['uav1', 'uav2', 'uav3'] + self.node = rclpy.create_node('multi_uav_control_app') + self.executor = SingleThreadedExecutor() # 創建單執行器 + self.executor.add_node(self.node) # 添加節點到執行器 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 = {} - - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") + for namespace in namespaces: + self.node.create_subscription(State, f'/{namespace}/state', + lambda msg, ns=namespace: self.state_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(BatteryState, f'/{namespace}/battery', + lambda msg, ns=namespace: self.battery_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(NavSatFix, f'/{namespace}/global_position/global', + lambda msg, ns=namespace: self.gps_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(GPSRAW, f'/{namespace}/gpsstatus/gps1/raw', + lambda msg, ns=namespace: self.gps_status_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(PoseStamped, f'/{namespace}/local_position/pose', + lambda msg, ns=namespace: self.local_position_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(Imu, f'/{namespace}/imu/data', + lambda msg, ns=namespace: self.imu_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(Float64, f'/{namespace}/global_position/compass_hdg', + lambda msg, ns=namespace: self.heading_callback(msg, ns), qos_profile=SENSOR_QOS) + self.node.create_subscription(VfrHud, f'/{namespace}/vfr_hud', + lambda msg, ns=namespace: self.vfr_hud_callback(msg, ns), qos_profile=SENSOR_QOS) + + self.clients = {namespace: { + "set_mode": self.node.create_client(SetMode, f'/{namespace}/set_mode'), + "arming": self.node.create_client(CommandBool, f'/{namespace}/cmd/arming'), + "takeoff": self.node.create_client(CommandTOL, f'/{namespace}/cmd/takeoff'), + "land": self.node.create_client(CommandTOL, f'/{namespace}/cmd/land'), + "local_position": self.node.create_publisher(PoseStamped, f'/{namespace}/setpoint_position/local', 10) + } for namespace in namespaces} def run(self): - while self.running: - try: - current_time = time.time() - msg = self.connection.recv_msg() - 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) - - # 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 # MAVLink sequence numbers are modulo 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 - - # 每隔一段時間更新 - if current_time - self.start_time[sysid] >= 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_signal.emit(namespace, self.frequency[sysid]) - - # 發送 timesync request - self.connection.mav.timesync_send( - 0, #tc1 - int(time.time() * 1e9) #ts1 - ) - #self.send_heartbeat - - 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) - - 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(time.time() * 1e9) - msg.ts1) / 1e6 - if(round_trip_time<1e6): - self.ping_signal.emit(namespace, round_trip_time) - - elif msg.get_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}") + while self.running and rclpy.ok(): + self.executor.spin_once(timeout_sec=0.1) def stop(self): self.running = False - self.connection.close() + self.executor.shutdown() + self.node.destroy_node() + + def state_callback(self, msg, namespace): + self.state_signal.emit(namespace, msg.mode) + + def battery_callback(self, msg, namespace): + self.battery_signal.emit(namespace, msg.percentage) + + def gps_callback(self, msg, namespace): + self.gps_signal.emit(namespace, msg.latitude, msg.longitude) + + def gps_status_callback(self, msg, namespace): + self.gps_status_signal.emit(namespace, msg.fix_type, msg.satellites_visible) + def local_position_callback(self, msg, namespace): + self.local_position_signal.emit(namespace, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) + + def imu_callback(self, msg, namespace): + self.imu_signal.emit(namespace, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) + + def heading_callback(self, msg, namespace): + self.hdg_signal.emit(namespace, msg.data) + + def vfr_hud_callback(self, msg, namespace): + self.groundspeed_signal.emit(namespace, msg.groundspeed) + 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 - self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP] - sysid, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - mode - ) - - def arm(self, namespace, arm): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400) - 0, # Confirmation - 1 if arm else 0, - 0, 0, 0, 0, 0, 0 - ) + req = SetMode.Request() + req.custom_mode = mode + self.clients[namespace]["set_mode"].call_async(req) + self.node.get_logger().info(f'{namespace}: 模式切換: {mode}') + + def arm(self, namespace, arm: bool): + req = CommandBool.Request() + req.value = arm + self.clients[namespace]["arming"].call_async(req) + self.node.get_logger().info(f'{namespace}: {"解鎖" if arm else "上鎖"}') def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, #MAV_CMD_NAV_TAKEOFF (22) - 0, # Confirmation (0 = first transmission) - 0, 0, 0, 0, # Parameters 1-4 (Unused in takeoff command) - 0, 0, altitude # Latitude, Longitude, Altitude (target_altitude in meters) - ) + req = CommandTOL.Request() + req.altitude = altitude + req.latitude = 0.0 + req.longitude = 0.0 + req.yaw = 0.0 + req.min_pitch = 0.0 + self.clients[namespace]["takeoff"].call_async(req) + self.node.get_logger().info(f'{namespace}: 起飛到高度 {altitude}') def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.command_long_send( - sysid, - 1, # Component ID - mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21) - 0, - 0, 0, 0, 0, - 0, 0, 0 - ) + req = CommandTOL.Request() + req.altitude = 0.0 + req.latitude = 0.0 + req.longitude = 0.0 + req.yaw = 0.0 + req.min_pitch = 0.0 + self.clients[namespace]["land"].call_async(req) + self.node.get_logger().info(f'{namespace}: 降落') def set_local_position(self, namespace, x, y, z): - sysid = int(namespace.replace('UAV', '')) - self.connection.mav.set_position_target_local_ned_send( - 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 - ) - - 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 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] - 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) + pose_msg = PoseStamped() + pose_msg.pose.position.x = x + pose_msg.pose.position.y = y + pose_msg.pose.position.z = z + self.clients[namespace]["local_position"].publish(pose_msg) + self.node.get_logger().info(f'{namespace}: Published local position: X={x}, Y={y}, Z={z}') class DroneControlApp(QMainWindow): def __init__(self): super().__init__() - self.workers = MAVLinkWorker() - self.namespaces = self.workers.namespaces + self.namespaces = ['uav1', 'uav2', 'uav3'] + self.workers = ROS2Worker(self.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) @@ -292,10 +144,7 @@ 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.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.start() def initUI(self): @@ -325,19 +174,15 @@ class DroneControlApp(QMainWindow): # 狀態顯示 self.uav_labels[namespace] = { - "status": QLabel("狀態：等待數據..."), - "battery": QLabel("電量：等待數據..."), - "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("頻率：等待數據..."), - "param": QLabel("SR1_EXTRA1：等待數據...") + "status": QLabel(f"{namespace} 狀態：等待數據..."), + "battery": QLabel(f"{namespace} 電量：等待數據..."), + "gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"), + "fix": QLabel(f"{namespace} Fix Type：等待數據..."), + "satellites": QLabel(f"{namespace} 衛星數量：等待數據..."), + "local_position": QLabel(f"{namespace} Local Position：等待數據..."), + "groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."), + "pitch": QLabel(f"{namespace} Pitch：等待數據..."), + "heading": QLabel(f"{namespace} Heading：等待數據..."), } # 把每個資訊添加到該 UAV 的垂直佈局中 @@ -349,16 +194,6 @@ class DroneControlApp(QMainWindow): 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() @@ -390,26 +225,19 @@ class DroneControlApp(QMainWindow): main_layout.addWidget(self.setPositionButton) # 解鎖按鈕 - fly_layout = QHBoxLayout() self.armButton = QPushButton("解鎖") self.armButton.clicked.connect(self.arm_drone) + main_layout.addWidget(self.armButton) # 起飛按鈕 self.takeoffButton = QPushButton("起飛") self.takeoffButton.clicked.connect(self.takeoff_drone) + main_layout.addWidget(self.takeoffButton) # 降落按鈕 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) - - #設置重啟按鈕 - self.rebootButton = QPushButton("重啟") - self.rebootButton.clicked.connect(self.reboot_drone) - main_layout.addWidget(self.rebootButton) + main_layout.addWidget(self.landButton) # 設置主佈局 central_widget = QWidget(self) @@ -424,13 +252,13 @@ class DroneControlApp(QMainWindow): event.accept() def update_state(self, namespace, mode): - self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") + self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}") def update_battery(self, namespace, percentage): - self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%") + self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%") def update_gps(self, namespace, latitude, longitude): - self.uav_labels[namespace]["gps"].setText(f"GPS位置： \n Lat:{latitude:.6f}° \n Lon:{longitude:.6f}°") + self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°") def update_gps_status(self, namespace, fix_type, satellites_visible): fix_type_str = { @@ -441,32 +269,22 @@ class DroneControlApp(QMainWindow): 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}") + self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}") + self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{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}°") + self.uav_labels[namespace]["local_position"].setText(f"{namespace} Local：({x:.2f}, {y:.2f}, {z:.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") + self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s") - def update_loss(self, namespace, loss): - self.uav_labels[namespace]["loss"].setText(f"丟包：{loss:.2f}%") + def update_imu(self, namespace, x, y, z, w): + q = np.array([x, y, z, w]) + pitch = self.quaternion_to_euler(q) + self.uav_labels[namespace]["pitch"].setText(f"{namespace} Pitch：{pitch:.2f}°") - def update_frequency(self, namespace, frequency): - self.uav_labels[namespace]["frequency"].setText(f"頻率：{frequency:.2f} Hz") - - def update_param(self, namespace, value): - self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") + def update_hdg(self, namespace, data): + self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{data:.2f}°") def change_mode(self): try: @@ -477,6 +295,17 @@ class DroneControlApp(QMainWindow): except Exception as e: QtWidgets.QMessageBox.warning(self, "錯誤", f"模式切換失敗：{str(e)}") + def set_local_position(self): + try: + x = float(self.positionX.text().strip()) + y = float(self.positionY.text().strip()) + z = float(self.positionZ.text().strip()) + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.set_local_position(namespace, x, y, z) + except ValueError: + QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + def arm_drone(self): try: for namespace in self.namespaces: @@ -500,35 +329,15 @@ class DroneControlApp(QMainWindow): 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()) - for namespace in self.namespaces: - if self.uav_checkboxes[namespace].isChecked(): - self.workers.set_local_position(namespace, x, y, z) - 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 quaternion_to_euler(self, q): + # Convert quaternion to Euler angles (roll, pitch, yaw) + x, y, z, w = q + # Pitch (y-axis rotation) + pitch_y = np.arcsin(2.0 * (w * y - z * x)) + + return pitch_y + def main(args=None): rclpy.init(args=args) app = QtWidgets.QApplication(sys.argv) From ee4cf1d07cd75d1214e7a15e81b13cda9885f94c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 22 Feb 2025 00:03:11 +0800 Subject: [PATCH 006/146] Update 'src/unitdev01/unitdev01/mavlink.py' --- src/unitdev01/unitdev01/mavlink.py | 291 +++++++++++++++++++++++------ 1 file changed, 237 insertions(+), 54 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index 430864e..9078880 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -1,4 +1,6 @@ import sys +import time +import math import rclpy from PyQt5 import QtWidgets from PyQt5.QtCore import QThread, pyqtSignal @@ -14,26 +16,94 @@ class MAVLinkWorker(QThread): 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) - def __init__(self, connection_string="udp:127.0.0.1:14550"): + def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): super().__init__() - self.connection = mavutil.mavlink_connection(connection_string) + self.namespaces = ['UAV1', 'UAV4', 'UAV5'] + self.connection = mavutil.mavlink_connection(usb, 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.uav_data = {} + + # 設置需要監控的訊息類型 + 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 = {} + + for namespace in self.namespaces: + self.request_param(namespace, "SR1_EXTRA1") def run(self): while self.running: try: - msg = self.connection.recv_match(blocking=True) + current_time = time.time() + msg = self.connection.recv_msg() if not msg: continue - - msg_type = msg.get_type() sysid = msg.get_srcSystem() - namespace = f"uav{sysid}" + if sysid == 0: + continue + namespace = f"UAV{sysid}" + print(sysid) + msg_type = msg.get_type() + if msg_type =="BAD_DATA": + continue - if sysid not in self.uav_data: - self.uav_data[sysid] = {"sysid": sysid} + print(msg_type) + + # 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 # MAVLink sequence numbers are modulo 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 + + # 每隔一段時間更新 + if current_time - self.start_time[sysid] >= 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_signal.emit(namespace, self.frequency[sysid]) + + # 發送 timesync request + self.connection.mav.timesync_send( + 0, #tc1 + int(time.time() * 1e9) #ts1 + ) + #self.send_heartbeat if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) @@ -60,7 +130,7 @@ class MAVLinkWorker(QThread): self.local_position_signal.emit(namespace, x, y, z) elif msg_type == "ATTITUDE": - pitch = msg.pitch + pitch = math.degrees(msg.pitch) self.imu_signal.emit(namespace, pitch) elif msg_type == "VFR_HUD": @@ -69,6 +139,16 @@ class MAVLinkWorker(QThread): self.hdg_signal.emit(namespace, heading) self.groundspeed_signal.emit(namespace, groundspeed) + elif msg_type == "TIMESYNC": + round_trip_time = (int(time.time() * 1e9) - msg.ts1) / 1e6 + if(round_trip_time<1e6): + self.ping_signal.emit(namespace, round_trip_time) + + elif msg.get_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}") @@ -77,7 +157,7 @@ class MAVLinkWorker(QThread): self.connection.close() def set_mode(self, namespace, mode): - sysid = int(namespace[-1]) + sysid = int(namespace.replace('UAV', '')) if mode == 'STABILIZE': mode = 0 elif mode == 'AUTO': @@ -86,56 +166,47 @@ class MAVLinkWorker(QThread): mode = 4 elif mode == 'LOITER': mode = 5 - self.connection.mav.set_mode_send( + self.connection.mav.set_mode_send( #SET_MODE (11) — [DEP] sysid, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode ) def arm(self, namespace, arm): - sysid = int(namespace[-1]) - # self.connection.mav.command_long_send( - # sysid, - # 1, # Component ID - # mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - # 0, # Confirmation - # 1 if arm else 0, - # 0, 0, 0, 0, 0, 0 - # ) + sysid = int(namespace.replace('UAV', '')) self.connection.mav.command_long_send( sysid, 1, # Component ID - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, - 0, - 33, - 1000000, - 0, 0, 0, 0, 0 + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, #MAV_CMD_COMPONENT_ARM_DISARM (400) + 0, # Confirmation + 1 if arm else 0, + 0, 0, 0, 0, 0, 0 ) def takeoff(self, namespace, altitude): - sysid = int(namespace[-1]) + sysid = int(namespace.replace('UAV', '')) self.connection.mav.command_long_send( sysid, 1, # Component ID - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, # Command to execute + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, #MAV_CMD_NAV_TAKEOFF (22) 0, # Confirmation (0 = first transmission) 0, 0, 0, 0, # Parameters 1-4 (Unused in takeoff command) 0, 0, altitude # Latitude, Longitude, Altitude (target_altitude in meters) ) def land(self, namespace): - sysid = int(namespace[-1]) + sysid = int(namespace.replace('UAV', '')) self.connection.mav.command_long_send( sysid, 1, # Component ID - mavutil.mavlink.MAV_CMD_NAV_LAND, + mavutil.mavlink.MAV_CMD_NAV_LAND, #MAV_CMD_NAV_LAND (21) 0, 0, 0, 0, 0, 0, 0, 0 ) def set_local_position(self, namespace, x, y, z): - sysid = int(namespace[-1]) + sysid = int(namespace.replace('UAV', '')) self.connection.mav.set_position_target_local_ned_send( 0, sysid, 1, # time_boot_ms, sysid, compid mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame @@ -146,12 +217,70 @@ class MAVLinkWorker(QThread): 0, 0 # Yaw, yaw_rate ) + 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 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] + 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) class DroneControlApp(QMainWindow): def __init__(self): super().__init__() - self.namespaces = ['uav1', 'uav2', 'uav3'] self.workers = MAVLinkWorker() + self.namespaces = self.workers.namespaces self.initUI() # Connect signals to update the UI @@ -163,7 +292,10 @@ 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.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.start() def initUI(self): @@ -193,15 +325,19 @@ class DroneControlApp(QMainWindow): # 狀態顯示 self.uav_labels[namespace] = { - "status": QLabel(f"{namespace} 狀態：等待數據..."), - "battery": QLabel(f"{namespace} 電量：等待數據..."), - "gps": QLabel(f"{namespace} GPS位置：等待數據...\n\n"), - "fix": QLabel(f"{namespace} Fix Type：等待數據..."), - "satellites": QLabel(f"{namespace} 衛星數量：等待數據..."), - "local_position": QLabel(f"{namespace} Local Position：等待數據..."), - "groundspeed": QLabel(f"{namespace} 地面速度：等待數據..."), - "pitch": QLabel(f"{namespace} Pitch：等待數據..."), - "heading": QLabel(f"{namespace} Heading：等待數據..."), + "status": QLabel("狀態：等待數據..."), + "battery": QLabel("電量：等待數據..."), + "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("頻率：等待數據..."), + "param": QLabel("SR1_EXTRA1：等待數據...") } # 把每個資訊添加到該 UAV 的垂直佈局中 @@ -213,6 +349,16 @@ class DroneControlApp(QMainWindow): 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() @@ -244,19 +390,26 @@ class DroneControlApp(QMainWindow): main_layout.addWidget(self.setPositionButton) # 解鎖按鈕 + fly_layout = QHBoxLayout() self.armButton = QPushButton("解鎖") self.armButton.clicked.connect(self.arm_drone) - main_layout.addWidget(self.armButton) # 起飛按鈕 self.takeoffButton = QPushButton("起飛") self.takeoffButton.clicked.connect(self.takeoff_drone) - main_layout.addWidget(self.takeoffButton) # 降落按鈕 self.landButton = QPushButton("降落") self.landButton.clicked.connect(self.land_drone) - main_layout.addWidget(self.landButton) + fly_layout.addWidget(self.armButton) + fly_layout.addWidget(self.takeoffButton) + fly_layout.addWidget(self.landButton) + main_layout.addLayout(fly_layout) + + #設置重啟按鈕 + self.rebootButton = QPushButton("重啟") + self.rebootButton.clicked.connect(self.reboot_drone) + main_layout.addWidget(self.rebootButton) # 設置主佈局 central_widget = QWidget(self) @@ -271,13 +424,13 @@ class DroneControlApp(QMainWindow): event.accept() def update_state(self, namespace, mode): - self.uav_labels[namespace]["status"].setText(f"{namespace} 狀態：{mode}") + self.uav_labels[namespace]["status"].setText(f"狀態：{mode}") def update_battery(self, namespace, percentage): - self.uav_labels[namespace]["battery"].setText(f"{namespace} 電量：{percentage * 100:.2f}%") + self.uav_labels[namespace]["battery"].setText(f"電量：{percentage * 100:.2f}%") def update_gps(self, namespace, latitude, longitude): - self.uav_labels[namespace]["gps"].setText(f"{namespace} GPS位置： \nLat:{latitude:.6f}° \nLon:{longitude:.6f}°") + 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 = { @@ -288,20 +441,32 @@ class DroneControlApp(QMainWindow): 4: "RTK Float", 5: "RTK Fix" }.get(fix_type, "Unknown") - self.uav_labels[namespace]["fix"].setText(f"{namespace} Fix Type：{fix_type_str}") - self.uav_labels[namespace]["satellites"].setText(f"{namespace} 衛星數量：{satellites_visible}") + 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"{namespace} Local：({x:.2f}, {y:.2f}, {z:.2f})") + 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"{namespace} Pitch：{pitch:.2f}°") + self.uav_labels[namespace]["pitch"].setText(f"Pitch：{pitch:.2f}°") def update_hdg(self, namespace, heading): - self.uav_labels[namespace]["heading"].setText(f"{namespace} Heading：{heading:.2f}°") + self.uav_labels[namespace]["heading"].setText(f"Heading：{heading:.2f}°") def update_groundspeed(self, namespace, groundspeed): - self.uav_labels[namespace]["groundspeed"].setText(f"{namespace} 地面速度：{groundspeed:.2f} m/s") + 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_param(self, namespace, value): + self.uav_labels[namespace]["param"].setText(f"SR1_EXTRA1:{value}") def change_mode(self): try: @@ -346,6 +511,24 @@ class DroneControlApp(QMainWindow): 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(args=None): rclpy.init(args=args) app = QtWidgets.QApplication(sys.argv) From 4f9e6fa51eed47ea5d3033a496a35fe8c91d7e01 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 26 Feb 2025 11:13:15 +0800 Subject: [PATCH 007/146] =?UTF-8?q?Gazebo=E7=89=A9=E4=BB=B6=E7=94=9F?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/gazebo_native_manager.py | 174 +++++++++++++++++++++++++ 1 file changed, 174 insertions(+) create mode 100644 src/unitdev03/gazebo_native_manager.py diff --git a/src/unitdev03/gazebo_native_manager.py b/src/unitdev03/gazebo_native_manager.py new file mode 100644 index 0000000..8699188 --- /dev/null +++ b/src/unitdev03/gazebo_native_manager.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +import subprocess +import argparse +import time +import psutil +import h5py +import numpy as np + +class GazeboModelManager: + def __init__(self): + self.base_cmd = "gz service -s /world/map" + self.timeout = 5000 + + def add_iris(self, number, x_pos, y_pos, yaw_angle): + cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "model://iris{number}{x_pos} {y_pos} 0.195 0 0 {yaw_angle}"' ''' + + try: + print(f"正在新增 iris{number}...") + print(f"位置: x={x_pos}, y={y_pos}, yaw={yaw_angle}度") + subprocess.run(cmd, shell=True, check=True) + print(f"成功新增 iris{number}") + + sitl_instance = number - 1 + port = 14550 + sitl_instance + + sitl_cmd = f"sim_vehicle.py -v ArduCopter -f gazebo-iris{sitl_instance} -L NCHU --model JSON --map --console -I{sitl_instance} --out=127.0.0.1:{port}" + terminal_cmd = f'gnome-terminal -- bash -c "{sitl_cmd}; exec bash"' + print(f"正在啟動 SITL...") + subprocess.Popen(terminal_cmd, shell=True) + + return True + except subprocess.CalledProcessError as e: + print(f"新增失敗: {e}") + return False + + def cleanup_sitl(self, number): + instance_number = number - 1 + print(f"正在清理 SITL 進程 {number}...") + + cleanup_commands = [ + f"pkill -f 'sim_vehicle.py.*-I{instance_number}'", + f"pkill -f 'arducopter.*-I{instance_number}'", + "pkill -f 'mavproxy.py.*map'", + "pkill -f 'mavproxy.py.*console'", + f"pkill -f 'mavproxy.*{instance_number}'", + "pkill -f 'python.*map.py'", + "pkill -f 'MAVProxy.py.*map'" + ] + + for cmd in cleanup_commands: + try: + subprocess.run(cmd, shell=True) + except subprocess.CalledProcessError: + pass + + time.sleep(2) + print(f"SITL 進程清理完成") + + def remove_iris(self, number): + cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "iris{number}"\'' + + try: + print(f"正在移除 iris{number}...") + subprocess.run(cmd, shell=True, check=True) + print(f"成功移除 iris{number}") + return True + except subprocess.CalledProcessError as e: + print(f"移除失敗: {e}") + return False + + def add_obstacle(self, number, x_pos, y_pos, radius=0.5, height=2.0): + cmd = f'''gz service -s /world/map/create --reqtype gz.msgs.EntityFactory --reptype gz.msgs.Boolean --timeout 5000 --req 'sdf: "true{x_pos} {y_pos} {height/2} 0 0 0{radius}{height}{radius}{height}0.3 0.3 0.3 10.7 0.7 0.7 1"' ''' + + try: + print(f"正在新增障礙物 {number}...") + print(f"位置: x={x_pos}, y={y_pos}, radius={radius}, height={height}") + subprocess.run(cmd, shell=True, check=True) + return True + except subprocess.CalledProcessError as e: + print(f"新增失敗: {e}") + return False + + def remove_obstacle(self, number): + cmd = f'gz service -s /world/map/remove --reqtype gz.msgs.Entity --reptype gz.msgs.Boolean --timeout 1000 --req \'name: "obstacle_{number}"\'' + + try: + print(f"正在移除障礙物 {number}...") + subprocess.run(cmd, shell=True, check=True) + print(f"成功移除障礙物 {number}") + return True + except subprocess.CalledProcessError as e: + print(f"移除失敗: {e}") + return False + + def add_obstacles_from_mat(self, mat_file): + """從 .mat 檔案逐一生成障礙物""" + try: + with h5py.File(mat_file, 'r') as f: + x_data = f['#refs#/d'][()] + y_data = f['#refs#/e'][()] + z_data = f['#refs#/f'][()] + radius_data = f['#refs#/g'][()] + height_data = f['#refs#/h'][()] + + data_matrix = np.vstack((x_data, y_data, z_data, radius_data, height_data)).T + + for i, obstacle in enumerate(data_matrix): + x, y, _, radius, height = obstacle + success = self.add_obstacle(i+1, float(x), float(y), float(radius), float(height)) + if not success: + print(f"警告：障礙物 {i+1} 新增失敗") + time.sleep(0.1) + + print("所有障礙物新增完成") + return True + + except Exception as e: + print(f"錯誤：讀取或處理 .mat 檔案時發生問題: {e}") + return False + +def main(): + parser = argparse.ArgumentParser(description='Gazebo Iris and Obstacle Manager') + parser.add_argument('action', choices=['add', 'remove', 'cleanup', 'add_from_mat'], + help='Action to perform (add, remove, cleanup, or add_from_mat)') + parser.add_argument('type', choices=['iris', 'obstacle'], + help='Type of object to manage (iris or obstacle)') + parser.add_argument('number', type=int, nargs='?', + help='Object number (e.g., 4 for iris4 or obstacle_4)') + parser.add_argument('--x-pos', type=float, default=15.0, + help='X position for new object') + parser.add_argument('--y-pos', type=float, default=0.0, + help='Y position for new object') + parser.add_argument('--yaw', type=float, default=90.0, + help='Yaw angle in degrees for iris') + parser.add_argument('--radius', type=float, default=0.5, + help='Radius for obstacle cylinder') + parser.add_argument('--height', type=float, default=2.0, + help='Height for obstacle cylinder') + parser.add_argument('--mat-file', type=str, + help='Path to .mat file containing obstacles data') + + args = parser.parse_args() + manager = GazeboModelManager() + + if args.action == 'add_from_mat': + if args.type == 'obstacle' and args.mat_file: + manager.add_obstacles_from_mat(args.mat_file) + else: + print("錯誤：使用 add_from_mat 時需指定 --mat-file 參數") + elif args.action == 'add': + if args.number is None: + print("錯誤：使用 add 時需指定 number 參數") + return + if args.type == 'iris': + manager.add_iris(args.number, args.x_pos, args.y_pos, args.yaw) + else: # obstacle + manager.add_obstacle(args.number, args.x_pos, args.y_pos, args.radius, args.height) + elif args.action == 'cleanup': + if args.number is None: + print("錯誤：使用 cleanup 時需指定 number 參數") + return + if args.type == 'iris': + manager.cleanup_sitl(args.number) + else: # remove + if args.number is None: + print("錯誤：使用 remove 時需指定 number 參數") + return + if args.type == 'iris': + manager.remove_iris(args.number) + else: + manager.remove_obstacle(args.number) + +if __name__ == "__main__": + main() \ No newline at end of file From cd9d055409cc03646b0537f777476e7fcaf702d1 Mon Sep 17 00:00:00 2001 From: picard Date: Fri, 7 Mar 2025 16:41:05 +0800 Subject: [PATCH 008/146] temp commit for share code --- README.md | 6 ++ .../fc_network_adapter/__init__.py | 0 .../fc_network_adapter/mavlinkObject.py | 97 +++++++++++++++++++ .../fc_network_adapter/socketManager.py | 14 +++ src/fc_network_adapter/package.xml | 18 ++++ .../resource/fc_network_adapter | 0 src/fc_network_adapter/setup.cfg | 4 + src/fc_network_adapter/setup.py | 25 +++++ src/unitdev02/unitdev02/test01.md | 34 ++++++- src/unitdev02/unitdev02/test01.py | 57 +++++++++-- 10 files changed, 245 insertions(+), 10 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/__init__.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkObject.py create mode 100644 src/fc_network_adapter/fc_network_adapter/socketManager.py create mode 100644 src/fc_network_adapter/package.xml create mode 100644 src/fc_network_adapter/resource/fc_network_adapter create mode 100644 src/fc_network_adapter/setup.cfg create mode 100644 src/fc_network_adapter/setup.py diff --git a/README.md b/README.md index c93bd79..b92299c 100644 --- a/README.md +++ b/README.md @@ -16,5 +16,11 @@ === Package 簡述 +1. unitdev 為各自協作者做開發時的測試區 +2. fc_network_adapter + 建立、維護與飛控韌體的連接 + 構築 mavlink 封包 + 同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 + 處理無線模組的通訊格式 (XBee) diff --git a/src/fc_network_adapter/fc_network_adapter/__init__.py b/src/fc_network_adapter/fc_network_adapter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py new file mode 100644 index 0000000..093e8ac --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -0,0 +1,97 @@ +import threading +from pymavlink import mavutil + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +import time + +''' + +# 不同的匯流排只有單一種通訊協定 +# 匯流排接到訊息後透過 queue stack 傳送到分析器 +# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 +# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue +# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 + +# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 +# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 +# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function + +''' + +class MavlinkObject(): + def __init__(self, mavlink_socket): + self.mavlink_socket = mavlink_socket + self.msg_count = {} + # self.multiplexingDict = {} + self.multiplexingToAnalysis = [] + self.multiplexingToReturn = [] + self.multiplexingToConvert = [] + def __del__(self): + self.mavlink_socket.close() + self.stop() + + def run(self): + self.thread = threading.Thread(target=self._run_thread) + self.running = True + self.thread.start() + def stop(self): + self.running = False + + def _run_thread(self): + start_time = time.time() + while self.running: + try: + mavlinkMsg = self.mavlink_socket.recv_msg() + if mavlinkMsg: + self.count_mavlink_type(mavlinkMsg) + # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug + # break # Debug + + except Exception as e: + print(f"Error receiving mavlink data: {e}") + print(mavlinkMsg) + self.running = False + break + + # 每秒中 統計一次 收到的訊息總量 + elapsed_time = time.time() - start_time + if elapsed_time > 1: + print('Messages Type received (in about 1 sec) :') + print(self.msg_count) + start_time = time.time() + self.msg_count = {} + + 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 + + def publish_mavlink_data(self, mavlinkMsg): + msg = String() + msg.data = str(mavlinkMsg) + self.publisher_.publish(msg) + + + +def main(args=None): + + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + + mavlink_object = MavlinkObject(mavlink_socket) + mavlink_object.run() + + time.sleep(5) + mavlink_object.running = False + mavlink_object.thread.join() + print('End of Program') + + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/socketManager.py b/src/fc_network_adapter/fc_network_adapter/socketManager.py new file mode 100644 index 0000000..52e8681 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/socketManager.py @@ -0,0 +1,14 @@ + +''' + +透過某個地方 得到 udp 或 uart 接口 +對於每個接口 視為一個獨立的物件 + +物件對於不同的接口 是為不同類型的物件 + +每個類型的物件 創建一個獨立的執行緒 來處理資料 +關於執行緒的實作 是寫在另一個模組 + +物件之間 也可以做資料轉換/轉拋 + +''' \ No newline at end of file diff --git a/src/fc_network_adapter/package.xml b/src/fc_network_adapter/package.xml new file mode 100644 index 0000000..7f65623 --- /dev/null +++ b/src/fc_network_adapter/package.xml @@ -0,0 +1,18 @@ + + + + fc_network_adapter + 0.0.0 + TODO: Package description + picars + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/fc_network_adapter/resource/fc_network_adapter b/src/fc_network_adapter/resource/fc_network_adapter new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/setup.cfg b/src/fc_network_adapter/setup.cfg new file mode 100644 index 0000000..d4fbae2 --- /dev/null +++ b/src/fc_network_adapter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fc_network_adapter +[install] +install_scripts=$base/lib/fc_network_adapter diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py new file mode 100644 index 0000000..33414cb --- /dev/null +++ b/src/fc_network_adapter/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'fc_network_adapter' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='picars', + maintainer_email='chiyu1468@hotmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/unitdev02/unitdev02/test01.md b/src/unitdev02/unitdev02/test01.md index d265f70..053d1ac 100644 --- a/src/unitdev02/unitdev02/test01.md +++ b/src/unitdev02/unitdev02/test01.md @@ -46,4 +46,36 @@ RC_CHANNELS：遙控器 (RC) 的通道數據，代表從操控者 (或模擬器) 'MISSION_CURRENT': 73, 'SIMSTATE': 72, 7. 任務與導航 MISSION_CURRENT：目前正在執行的飛行任務 (Mission) 的編號。 -SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。 \ No newline at end of file +SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。 + + + +================================ + + +['__annotations__', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__getitem__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', + +'_crc', '_fieldnames', '_header', '_instance_field', '_instance_offset', '_instances', '_link_id', '_msgbuf', '_pack', '_payload', '_posted', '_signed', '_timestamp', , + +, 'crc_extra', 'fieldunits_by_name', 'format_attr', 'get_crc', , 'get_header', 'get_link_id', , 'get_msgbuf', 'get_payload', 'get_seq', 'get_signed', 'instance_field', 'instance_offset', 'lengths', 'name', 'native_format', 'ordered_fieldnames', 'orders', 'pack', 'pitch', 'pitchspeed', 'roll', 'rollspeed', 'sign_packet', 'time_boot_ms', 'to_dict', 'to_json', 'unpacker', 'yaw', 'yawspeed'] + +mavlink 封包 種類名稱 +'get_type()' '_type' 'msgname' +'get_msgId()' 'id' +'fieldenums_by_name' + + +發送端的 sysid get_srcComponent() +發送端的 compid get_srcComponent() + +這邊是一組的 mavlink 封包 內容的 +資料長度 'array_lengths' +資料名稱 'fieldnames' 'get_fieldnames()' +資料格式 'fieldtypes' + +===================================== + + +['_MAVLink__callbacks', '_MAVLink__parse_char_legacy', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'actuator_control_target_encode', 'actuator_control_target_send', 'actuator_output_status_encode', 'actuator_output_status_send', 'adsb_vehicle_encode', 'adsb_vehicle_send', 'ais_vessel_encode', 'ais_vessel_send', 'altitude_encode', 'altitude_send', 'att_pos_mocap_encode', 'att_pos_mocap_send', 'attitude_encode', 'attitude_quaternion_cov_encode', 'attitude_quaternion_cov_send', 'attitude_quaternion_encode', 'attitude_quaternion_send', 'attitude_send', 'attitude_target_encode', 'attitude_target_send', 'auth_key_encode', 'auth_key_send', 'autopilot_state_for_gimbal_device_encode', 'autopilot_state_for_gimbal_device_send', 'autopilot_version_encode', 'autopilot_version_send', 'battery_status_encode', 'battery_status_send', 'buf', 'buf_index', 'buf_len', 'button_change_encode', 'button_change_send', 'bytes_needed', 'callback', 'callback_args', 'callback_kwargs', 'camera_capture_status_encode', 'camera_capture_status_send', 'camera_fov_status_encode', 'camera_fov_status_send', 'camera_image_captured_encode', 'camera_image_captured_send', 'camera_information_encode', 'camera_information_send', 'camera_settings_encode', 'camera_settings_send', 'camera_thermal_range_encode', 'camera_thermal_range_send', 'camera_tracking_geo_status_encode', 'camera_tracking_geo_status_send', 'camera_tracking_image_status_encode', 'camera_tracking_image_status_send', 'camera_trigger_encode', 'camera_trigger_send', 'can_filter_modify_encode', 'can_filter_modify_send', 'can_frame_encode', 'can_frame_send', 'canfd_frame_encode', 'canfd_frame_send', 'change_operator_control_ack_encode', 'change_operator_control_ack_send', 'change_operator_control_encode', 'change_operator_control_send', 'check_signature', 'collision_encode', 'collision_send', 'command_ack_encode', 'command_ack_send', 'command_int_encode', 'command_int_send', 'command_long_encode', 'command_long_send', 'control_system_state_encode', 'control_system_state_send', 'crc_extra', 'data_stream_encode', 'data_stream_send', 'data_transmission_handshake_encode', 'data_transmission_handshake_send', 'debug_encode', 'debug_float_array_encode', 'debug_float_array_send', 'debug_send', 'debug_vect_encode', 'debug_vect_send', 'decode', 'distance_sensor_encode', 'distance_sensor_send', 'efi_status_encode', 'efi_status_send', 'encapsulated_data_encode', 'encapsulated_data_send', 'estimator_status_encode', 'estimator_status_send', 'expected_length', 'extended_sys_state_encode', 'extended_sys_state_send', 'fence_status_encode', 'fence_status_send', 'file', 'file_transfer_protocol_encode', 'file_transfer_protocol_send', 'flight_information_encode', 'flight_information_send', 'follow_target_encode', 'follow_target_send', 'generator_status_encode', 'generator_status_send', 'gimbal_device_attitude_status_encode', 'gimbal_device_attitude_status_send', 'gimbal_device_information_encode', 'gimbal_device_information_send', 'gimbal_device_set_attitude_encode', 'gimbal_device_set_attitude_send', 'gimbal_manager_information_encode', 'gimbal_manager_information_send', 'gimbal_manager_set_attitude_encode', 'gimbal_manager_set_attitude_send', 'gimbal_manager_set_manual_control_encode', 'gimbal_manager_set_manual_control_send', 'gimbal_manager_set_pitchyaw_encode', 'gimbal_manager_set_pitchyaw_send', 'gimbal_manager_status_encode', 'gimbal_manager_status_send', 'global_position_int_cov_encode', 'global_position_int_cov_send', 'global_position_int_encode', 'global_position_int_send', 'global_vision_position_estimate_encode', 'global_vision_position_estimate_send', 'gps2_raw_encode', 'gps2_raw_send', 'gps2_rtk_encode', 'gps2_rtk_send', 'gps_global_origin_encode', 'gps_global_origin_send', 'gps_inject_data_encode', 'gps_inject_data_send', 'gps_input_encode', 'gps_input_send', 'gps_raw_int_encode', 'gps_raw_int_send', 'gps_rtcm_data_encode', 'gps_rtcm_data_send', 'gps_rtk_encode', 'gps_rtk_send', 'gps_status_encode', 'gps_status_send', 'have_prefix_error', 'heartbeat_encode', 'heartbeat_send', 'high_latency2_encode', 'high_latency2_send', 'high_latency_encode', 'high_latency_send', 'highres_imu_encode', 'highres_imu_send', 'hil_actuator_controls_encode', 'hil_actuator_controls_send', 'hil_controls_encode', 'hil_controls_send', 'hil_gps_encode', 'hil_gps_send', 'hil_optical_flow_encode', 'hil_optical_flow_send', 'hil_rc_inputs_raw_encode', 'hil_rc_inputs_raw_send', 'hil_sensor_encode', 'hil_sensor_send', 'hil_state_encode', 'hil_state_quaternion_encode', 'hil_state_quaternion_send', 'hil_state_send', 'home_position_encode', 'home_position_send', 'hygrometer_sensor_encode', 'hygrometer_sensor_send', 'isbd_link_status_encode', 'isbd_link_status_send', 'landing_target_encode', 'landing_target_send', 'little_endian', 'local_position_ned_cov_encode', 'local_position_ned_cov_send', 'local_position_ned_encode', 'local_position_ned_send', 'local_position_ned_system_global_offset_encode', 'local_position_ned_system_global_offset_send', 'log_data_encode', 'log_data_send', 'log_entry_encode', 'log_entry_send', 'log_erase_encode', 'log_erase_send', 'log_request_data_encode', 'log_request_data_send', 'log_request_end_encode', 'log_request_end_send', 'log_request_list_encode', 'log_request_list_send', 'logging_ack_encode', 'logging_ack_send', 'logging_data_acked_encode', 'logging_data_acked_send', 'logging_data_encode', 'logging_data_send', 'mag_cal_report_encode', 'mag_cal_report_send', 'manual_control_encode', 'manual_control_send', 'manual_setpoint_encode', 'manual_setpoint_send', 'mav10_unpacker', 'mav20_h3_unpacker', 'mav20_unpacker', 'mav_csum_unpacker', 'mav_sign_unpacker', 'memory_vect_encode', 'memory_vect_send', 'message_interval_encode', 'message_interval_send', 'mission_ack_encode', 'mission_ack_send', 'mission_clear_all_encode', 'mission_clear_all_send', 'mission_count_encode', 'mission_count_send', 'mission_current_encode', 'mission_current_send', 'mission_item_encode', 'mission_item_int_encode', 'mission_item_int_send', 'mission_item_reached_encode', 'mission_item_reached_send', 'mission_item_send', 'mission_request_encode', 'mission_request_int_encode', 'mission_request_int_send', 'mission_request_list_encode', 'mission_request_list_send', 'mission_request_partial_list_encode', 'mission_request_partial_list_send', 'mission_request_send', 'mission_set_current_encode', 'mission_set_current_send', 'mission_write_partial_list_encode', 'mission_write_partial_list_send', 'mount_orientation_encode', 'mount_orientation_send', 'named_value_float_encode', 'named_value_float_send', 'named_value_int_encode', 'named_value_int_send', 'nav_controller_output_encode', 'nav_controller_output_send', 'obstacle_distance_encode', 'obstacle_distance_send', 'odometry_encode', 'odometry_send', 'open_drone_id_arm_status_encode', 'open_drone_id_arm_status_send', 'open_drone_id_authentication_encode', 'open_drone_id_authentication_send', 'open_drone_id_basic_id_encode', 'open_drone_id_basic_id_send', 'open_drone_id_location_encode', 'open_drone_id_location_send', 'open_drone_id_message_pack_encode', 'open_drone_id_message_pack_send', 'open_drone_id_operator_id_encode', 'open_drone_id_operator_id_send', 'open_drone_id_self_id_encode', 'open_drone_id_self_id_send', 'open_drone_id_system_encode', 'open_drone_id_system_send', 'open_drone_id_system_update_encode', 'open_drone_id_system_update_send', 'optical_flow_encode', 'optical_flow_rad_encode', 'optical_flow_rad_send', 'optical_flow_send', 'param_ext_ack_encode', 'param_ext_ack_send', 'param_ext_request_list_encode', 'param_ext_request_list_send', 'param_ext_request_read_encode', 'param_ext_request_read_send', 'param_ext_set_encode', 'param_ext_set_send', 'param_ext_value_encode', 'param_ext_value_send', 'param_map_rc_encode', 'param_map_rc_send', 'param_request_list_encode', 'param_request_list_send', 'param_request_read_encode', 'param_request_read_send', 'param_set_encode', 'param_set_send', 'param_value_encode', 'param_value_send', 'parse_buffer', 'parse_char', 'ping_encode', 'ping_send', 'play_tune_encode', 'play_tune_send', 'position_target_global_int_encode', 'position_target_global_int_send', 'position_target_local_ned_encode', 'position_target_local_ned_send', 'power_status_encode', 'power_status_send', 'protocol_marker', 'radio_status_encode', 'radio_status_send', 'raw_imu_encode', 'raw_imu_send', 'raw_pressure_encode', 'raw_pressure_send', 'raw_rpm_encode', 'raw_rpm_send', 'rc_channels_encode', 'rc_channels_override_encode', 'rc_channels_override_send', 'rc_channels_raw_encode', 'rc_channels_raw_send', 'rc_channels_scaled_encode', 'rc_channels_scaled_send', 'rc_channels_send', 'relay_status_encode', 'relay_status_send', 'request_data_stream_encode', 'request_data_stream_send', 'resource_request_encode', 'resource_request_send', 'robust_parsing', 'safety_allowed_area_encode', 'safety_allowed_area_send', 'safety_set_allowed_area_encode', 'safety_set_allowed_area_send', 'scaled_imu2_encode', 'scaled_imu2_send', 'scaled_imu3_encode', 'scaled_imu3_send', 'scaled_imu_encode', 'scaled_imu_send', 'scaled_pressure2_encode', 'scaled_pressure2_send', 'scaled_pressure3_encode', 'scaled_pressure3_send', 'scaled_pressure_encode', 'scaled_pressure_send', 'send', 'send_callback', 'send_callback_args', 'send_callback_kwargs', 'seq', 'serial_control_encode', 'serial_control_send', 'servo_output_raw_encode', 'servo_output_raw_send', 'set_actuator_control_target_encode', 'set_actuator_control_target_send', 'set_attitude_target_encode', 'set_attitude_target_send', 'set_callback', 'set_gps_global_origin_encode', 'set_gps_global_origin_send', 'set_home_position_encode', 'set_home_position_send', 'set_mode_encode', 'set_mode_send', 'set_position_target_global_int_encode', 'set_position_target_global_int_send', 'set_position_target_local_ned_encode', 'set_position_target_local_ned_send', 'set_send_callback', 'setup_signing_encode', 'setup_signing_send', 'signing', 'sim_state_encode', 'sim_state_send', 'smart_battery_info_encode', 'smart_battery_info_send', 'sort_fields', 'srcComponent', 'srcSystem', 'startup_time', 'statustext_encode', 'statustext_send', 'storage_information_encode', 'storage_information_send', 'sys_status_encode', 'sys_status_send', 'system_time_encode', 'system_time_send', 'terrain_check_encode', 'terrain_check_send', 'terrain_data_encode', 'terrain_data_send', 'terrain_report_encode', 'terrain_report_send', 'terrain_request_encode', 'terrain_request_send', 'timesync_encode', 'timesync_send', 'total_bytes_received', 'total_bytes_sent', 'total_packets_received', 'total_packets_sent', 'total_receive_errors', 'trajectory_representation_bezier_encode', 'trajectory_representation_bezier_send', 'trajectory_representation_waypoints_encode', 'trajectory_representation_waypoints_send', 'tunnel_encode', 'tunnel_send', 'uavcan_node_info_encode', 'uavcan_node_info_send', 'uavcan_node_status_encode', 'uavcan_node_status_send', 'utm_global_position_encode', 'utm_global_position_send', 'v2_extension_encode', 'v2_extension_send', 'vfr_hud_encode', 'vfr_hud_send', 'vibration_encode', 'vibration_send', 'vicon_position_estimate_encode', 'vicon_position_estimate_send', 'video_stream_information_encode', 'video_stream_information_send', 'video_stream_status_encode', 'video_stream_status_send', 'vision_position_estimate_encode', 'vision_position_estimate_send', 'vision_speed_estimate_encode', 'vision_speed_estimate_send', 'wheel_distance_encode', 'wheel_distance_send', 'wifi_config_ap_encode', 'wifi_config_ap_send', 'winch_status_encode', 'winch_status_send', 'wind_cov_encode', 'wind_cov_send'] + + diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index 7e1f96c..bf634e2 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -42,6 +42,11 @@ def mavlink_analyzer_simple(count = 500): print('No message yet, 1 second for data to fill') sleep(1) + print("markA ------") + print(msg.get_type()) + print(msg.ordered_fieldnames) + print("markA ------ END") + print('Messages Type received:') print(msg_count) @@ -108,7 +113,8 @@ def fdm_switch_example_one(): elapsed_time = current_time - start_time if elapsed_time >= 1.0: - print(f"Packets received in the last second: {packet_count}") + # print(f"Packets received in the last second: {packet_count}") + print(f'JSON Pack : {data_g2s}') packet_count = 0 start_time = current_time fdm_parser_example(data_s2g) @@ -132,24 +138,24 @@ def fdm_switch_example_two(): server_address = ('127.0.0.1', 19002) data_queue = queue.Queue() - servo_out = [0] * 16 + servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....] data_queue.put(servo_out) - def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1): + def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1): interval = 1.0 / frame_rate_fdm while True: start_time = time.time() try: - data = q.get(timeout=0.1) + data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....] if data is None: break last_data = data except queue.Empty: data = last_data - data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out) - sock.sendto(data, server_address) - frame_count_fdm += 1 - fdm_parser_example(data) # debug + data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ] + sock.sendto(data_fdm, server_address) + frame_count_fdm += 3 + # fdm_parser_example(data_fdm) # debug elapsed_time = time.time() - start_time sleep_time = interval - elapsed_time @@ -159,6 +165,7 @@ def fdm_switch_example_two(): Running = True thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue)) thread.start() + print('Start sending data to Gazebo') while Running: msgb1 = None @@ -184,7 +191,39 @@ def fdm_switch_example_two(): # To stop the thread, you can put None into the queue # data_queue.put(None) +from pymavlink.dialects.v20 import common # 使用 MAVLink 2.0 + +def mavlink_btye_generator_test(): + + # 創建一個 MAVLink 對象 + mav = common.MAVLink(None) # 不透過 connection，直接使用 None + + # 創建一個 HEARTBEAT 訊息 + msg = mav.heartbeat_encode( + type=mavutil.mavlink.MAV_TYPE_QUADROTOR, + autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC, + base_mode=0, + custom_mode=0, + system_status=mavutil.mavlink.MAV_STATE_ACTIVE + ) + + # 取得 MAVLink 訊息的 bytes + mavlink_bytes = msg.pack(mav) + + # 回傳 bytes + print(mavlink_bytes) + print(type(mavlink_bytes)) + + mav2 = common.MAVLink(None) + # my_msg = mav2.decode(mavlink_bytes) + print("========") + print(dir(mav2)) + # print(my_msg) + + print('Start') # fdm_switch_example_two() -# fdm_parser_example() \ No newline at end of file +# fdm_parser_example() +# mavlink_analyzer_simple(8) +mavlink_btye_generator_test() From ac272a6c3da8d49b97bd42ec29806069ee23786a Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 10 Mar 2025 17:25:27 +0800 Subject: [PATCH 009/146] Enhance mavlinkObject with multiplexing capabilities and add error handling; update README with team member assignments --- README.md | 4 + .../fc_network_adapter/mavlinkObject.py | 130 ++++++++++++++++-- src/unitdev02/unitdev02/test01.py | 5 +- 3 files changed, 122 insertions(+), 17 deletions(-) diff --git a/README.md b/README.md index b92299c..d03f8a7 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,10 @@ === Package 簡述 1. unitdev 為各自協作者做開發時的測試區 + 01 -> 晉凱(ken) + 02 -> 其宇(chiyu) + 03 -> 文鈞 + 04 -> 倫渝 2. fc_network_adapter 建立、維護與飛控韌體的連接 構築 mavlink 封包 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 093e8ac..f84ce60 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,5 +1,6 @@ import threading from pymavlink import mavutil +import queue import rclpy from rclpy.node import Node @@ -7,6 +8,7 @@ from std_msgs.msg import String import time + ''' # 不同的匯流排只有單一種通訊協定 @@ -19,24 +21,45 @@ import time # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 # 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function + ''' +fixed_stream_analyzer_queue = queue.Queue() +return_packet_processor_queue = queue.Queue() +converte_format_queues = [] + class MavlinkObject(): - def __init__(self, mavlink_socket): + def __init__(self, mavlink_socket, socket_id = None): + self.socket_id = socket_id self.mavlink_socket = mavlink_socket self.msg_count = {} - # self.multiplexingDict = {} - self.multiplexingToAnalysis = [] - self.multiplexingToReturn = [] - self.multiplexingToConvert = [] + self.multiplexingList = [] + + self.multiplexingToAnalysis = [ + # 0, # HEARTBEAT + 30, # ATTITUDE + 33, # GLOBAL_POSITION_INT + # 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') + return self.thread = threading.Thread(target=self._run_thread) - self.running = True + self.running = self.updateMultiplexingList() self.thread.start() + def stop(self): self.running = False @@ -45,24 +68,38 @@ class MavlinkObject(): while self.running: try: mavlinkMsg = self.mavlink_socket.recv_msg() - if mavlinkMsg: - self.count_mavlink_type(mavlinkMsg) - # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug - # break # Debug - except Exception as e: - print(f"Error receiving mavlink data: {e}") + 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 + # 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)) + elif i == 1: + return_packet_processor_queue.put((self.socket_id,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('Messages Type received (in about 1 sec) :') + 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() @@ -76,6 +113,45 @@ class MavlinkObject(): msg.data = str(mavlinkMsg) self.publisher_.publish(msg) + def updateMultiplexingList(self): + # 更新 multiplexing list + self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + + 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') + 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') + + # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10) + # self.mavlink_object = None + # self.create_mavlink_object() + + def create_mavlink_object(self): + # connection_string="udp: + pass + +class mavlink_device(): + def __init__(self): + self.socket_id = None # 記錄來自於哪個 socket + self.sysid = None + self.component_count = 1 + self.compid_list = [] + def main(args=None): @@ -83,10 +159,34 @@ def main(args=None): connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object = MavlinkObject(mavlink_socket) + # 這邊是測試代碼 運行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()) + 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])) + + for q in converte_format_queues: + while not q.empty(): + print('converte_format_queues:') + t = q.get() + print('from {} : {}'.format(t[0],t[1])) + - time.sleep(5) mavlink_object.running = False mavlink_object.thread.join() print('End of Program') diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index bf634e2..c1b5276 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -62,7 +62,6 @@ def fdm_parser_example(data=None): data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000') # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000 - parse_format = 'HHI16H' if len(data) != struct.calcsize(parse_format): @@ -226,4 +225,6 @@ print('Start') # fdm_switch_example_two() # fdm_parser_example() # mavlink_analyzer_simple(8) -mavlink_btye_generator_test() +# mavlink_btye_generator_test() + + From 8bc7bfee86be15446d85b01990425baa2ae441f8 Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:04:57 +0800 Subject: [PATCH 010/146] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=B3=87=E6=96=99?= =?UTF-8?q?=E5=A4=BE=E4=BD=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- obstacles_data.mat | Bin 0 -> 2246 bytes .../{ => 0228}/gazebo_native_manager.py | 0 src/unitdev03/{ => 0228}/pymavlink_01.py | 0 uav_simulation_data.mat | Bin 0 -> 36048 bytes 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 obstacles_data.mat rename src/unitdev03/{ => 0228}/gazebo_native_manager.py (100%) rename src/unitdev03/{ => 0228}/pymavlink_01.py (100%) create mode 100644 uav_simulation_data.mat diff --git a/obstacles_data.mat b/obstacles_data.mat new file mode 100644 index 0000000000000000000000000000000000000000..f2badb7d510797c2295555246726b8c0557ebfcc GIT binary patch literal 2246 zcmV;%2s!snK~zjZLLfCRFd$7qR4ry{Y-KDUP;6mzW^ZzBIv__(PFO)UG%O%Pa%Ew3 zWn>_4ZaN@MZ*Cw>VR9fbFd#8EIxsdmF*YDFFfuhDARr(hARr(hARr(hARr(hARr(h zARr(hARr(hARr(hARr(hARr*$00000000000ZB~{000120001Zoa19)V3+{JY(UHb z#DWYEzzD?rK%AdcT#}falUf{Kl9-f}3KVBy@OAbN2FbGmF+`Msfe(m1kj&wQn!^g^ zvq8;dgo=as5)kHHCL}o+1^|!*3&9Tn007?y004NL?OA;^T-O%AJ~E<3NCcyjc=0(Q zh$4iV&5SUZF<)AmFVh%ui5Zz0l1h{&qDWpU5v5X6q7o7L;8pZh(u9O2Wu>h}lc>@( zSs|LHq_maCygSUqH6e<=mw)WF&fRyPy?^KIefK_Rt#hspf*>c15rly8`B>9LMrod0 zR>9g1<54anRk@6)(C9K*2WvNpV|djG@QOCtuRfwvk=xKIK^(<6nYeNE&%XGQLj(c{Uz}tS0BOK6PB5B2ThD0y35}#aM*r_@jg4m$WC(i(*WCUe2nH*%OrP znM!fU@|KL*^Ikf#{hXsM>i4o*h5)4^>H&pN94Oe@bb`K|Tz>Z;O*mZ{q#CVaj zdjuDDN1ZiKeJKzusiAiNlrDfPD1+Xk&xYcT(i;}-95{48=R~av9SG;oMSMhJ!p23O znxv{Au%-Q;<@P7Q7pF+I6&it1v-ph?eyts__6fOLtQb&3%sEL7*M>@i((=dNuJ9ip ziX>srBKXXt^UyZdawuhW)pupE;Ev~(WsRrlP}iSA?kaKzweo9Ug*$P9YI{-IY_R~6 zozNw`hu#pj_f7YdRDkxVqJ_~MCM-zIEgJ4)LTH)4?d}Q($V!h4>(ldrZvUVSy)G1R zGVWnoe-Z%g$FrRc0~oNfbaaPVl6@Ofs83lzrLw`pDSfpbTr^xpoS4!s-J_r2542D8YB!1q`o|;oj~uzMwjkx6(+_T7q*F*Stj6+k(_m*^ zF%z~I9(?5!EidRGUR0;NN&`b`_TazxOQB+Y$X!Og0sQGqvL5)r7p&9~r$|Q@+{(A2 zAIjvyh3<9NGxDr~Th*Tv?MZ{OCHq2`_fWyR#_mXlrW;7oc) zdR8h6R#m)P%vNyk21b>dq)_+mi!QrQ`Qcw|$Vc&~FckTWh@H$M)9 zo*TuV8}4Mlu5Kyi)4fdadzk0wPNagTdno-vwkb3*lF<4dConot!;4?S1--ge4XJx< zq3K7Ksig~+&$f^?PagO}W6J&Tga^T}Z$;T#$xh3_$S^eEptG5Xd4RClCXMn7H+bwBf?eq!5z zk+rBovz!D?gDdr1tvOhIb(`dzr-2`}&D%5D5As@HZfSS50lfk7830 zaPz58F`q8>7BWLz=nQdhPVikO!V$*dMrAfH9E}st9Ph_RBezIK@n6rI-hGj=T>cn) z#EJQ@^F*lZx`ZLSKM4|{k*I*Tbwoo&WB39wDin-YQR2iBVGMKRqGdcOhI-Z_o-dLO z#NoyB!`F(0F=Ln*R6r5N#PTF427G<0R#fsfT^#vzo=Hce^2M7)XsqPv zAkX-w6s8O_E=)XTk`I7Uu$V8R%I>6|A;mm6PsHOj!2(nwo)AM33M8l?4hvSHQLal( z)#v9|RsJ>`9KIg^NFqPRv)mVdB~P^vC$9f0l6h-al1dmHm*}r{U};`acwcvsabN U`2RzGv+x`Le@I6E1)kN4>nmS6OaK4? literal 0 HcmV?d00001 diff --git a/src/unitdev03/gazebo_native_manager.py b/src/unitdev03/0228/gazebo_native_manager.py similarity index 100% rename from src/unitdev03/gazebo_native_manager.py rename to src/unitdev03/0228/gazebo_native_manager.py diff --git a/src/unitdev03/pymavlink_01.py b/src/unitdev03/0228/pymavlink_01.py similarity index 100% rename from src/unitdev03/pymavlink_01.py rename to src/unitdev03/0228/pymavlink_01.py diff --git a/uav_simulation_data.mat b/uav_simulation_data.mat new file mode 100644 index 0000000000000000000000000000000000000000..3c3083a0e15a4416ea17fc8487e1b368c7fa6a8e GIT binary patch literal 36048 zcma%>Q*dpr*DrQ{l4G&+WUWvr8(tCCNc&1eY~ZH-*a0Zw+@gc7pynnLWXw1gr~W=1Y%ri1`{ZbCVLJ)xYD z6Co2LArl8TBRe+}J0UY8GaKRmGJgI40FjbI`0p(l_3M{M*5%3@huTjsq=WFdET^>D zlxDC=%)?)rt(uEP1F`~|AmQy_Q#_|*sgt%4q=Mg&x!&?p zfWP2pM-epM)XUh~(tL4Hcl!s2FYJn#Z+7+BsXpG;nNnkqU2Wuezd>&f riL2H3Q zD0|NZjg2VYl90LJMM(i2_o>%Ur4dkAFO4rxu1aoM$hz1q8ears;*sEJ5XUuc0AlpO zj#Q(FsrTkD{WT6{dr!lH*bL(6*B(ZPVv;X!$A}6B4l)%1wYO`1Ik9`%tQ0~G2$bre zJ)iB1mcRqGE9CHo|pnojQxvmby1?I(aehxG6hxFJq6frAfj!E2-0Uu3H_s-_u?zj(H!G8TPcB(<>5Tl% zbFdj$kDKo7X?yF-ro-PUM9xK{up+T3&~%C{qy5;xuZ;fVI9vMHBURg&z}bTU zp3*5Je%lSdsiog^i&8bj166ozdmH?BFzyOo{u6pjC*i?cWym0ceywDPZcc+C&TsrPZr_*!cJ;fl&1;uaVP= zF7M53hhn8~A}Je!`=3*(ngE&V;} z;VT(}HRAa4Pu}Ehco}gWSx~x5#kdSz^AuCaj*|MF1Z^JH6?oK`h>byi@8^hBo6EaQ z;M>Ns^D}40C>6${6@u1gL8HBNrQ=Iw?a+i>rID+p5w-iCwU1)iF@QiPIYGSiC+`E^z%LG(02}-+MOq^D?A$k*w3WYop{M(Z&AMYU~ zXB{p8Hqa^9Bxm%ccZZ9Zq(CyA`8bRoM+oQvMNvj~-^pvZ8$55(lCHzSnV&M-;xCtl zGF1w`er@13@D<>-CpMdvx06GU@x2U{3bdcI-!n7ZL*bGEWrK*>x^kR#F#St%GO`k` z2YV^EiPrv%3hEo`8oSjy+E)?=Ji%yYR9t26zkW*$$X4?q1233qGk5J}RT#zU#atGk zTj3m>I4D&24rW>K`Dl^IauzR)w?3^eov}=l|D%E9@DDNcf(Hf)BP;G4!Jg4k$T)gB z`}`M%DSm|~&r1``A%cNa1f4qw1KC62D+dNx?E$o%NE5{X1e(AKTT_mt%#6-O06 zea@EF9z=!R6kIC^2krj3rykuIe2YX(5G#$l%MBe;Pva&YTSkMn^!jz%7vDVDMKaHX z{L>#_0#}K9K86^eivQ<9??vx6#n)Zzj}FGOTAE94k*5lyM0Bs5kBZ=46IUFtlikS)AVPmTk?N{X9h&8W zl$R}`7xGPk%vFv=-6A79qZVeayUoOaq0>^liQzS5J|g5Pwc}-!#TSNWQrNjA zHMXB;Zd<+h$|fd5$R7Ayh2TjQDxt4a+RewsWhsE<>`oD)qqv3|IO+>4zx^6(gtfOY zvr!lw3v!foJN=jUTN-1y-iC~FU$t%Jgwf87Qa{^fb*?gyJS2PmyMIzCOR4@^a0I#t zD(u`sKz(qhjc(tJ?_3lAj_A^RK$>=C=LUnm9Hb}U$bO51?e;@E=*>z+&IgDpmkd}M z8Xb;s3i;M9v+^N{qN0<;x7qLT%Z=E1aL1`lR=j&fyAi>IM%&k`FBvSFeNNrTFy1Hy zUu-yz)ru|ugA}Ra?-vXWREu;i6>CCrne1eflfeNoABJ;sQ{}Zo)^XO7#PP3fqVyvb z9!{`R3+&BRLBgLUV_&ER9|ozuPI94cp%(XQ(NvuGx$)JM;71dIL?GDyl7k8OBqB2j z&^}G&q@w073IP<1pXVN1?fn53`T=3-g1NUh&Rm3GM#N+>#QvezY@9*DbZLnV7h53~vi_hpjbOAI0P1Wu3jNrkGSkZe||X5pbOD%Y_O% z%jDtrcy>R&_2^wHo)qcfs|u(k<+iPo&jU>GZFZmnignq z=6TkZOU2wX?{0)3SA>jK=?W$S?nxx-K#zj*C{T)Fo@S^^(MU7j% zO}eu{y?Yi~AUmtJxVs@RnWM96R@kN}bZcs9!#H|J1mF0wwaO@M-23qfU_%O|+?#1= z5V{}%a{eT;D|*vB>y%DIFzF?GbEa+!BDL=r zX5)$GFK{x9!s}ql!NU!@QQq^v?ibYa9fv5N@gQIA`L!JcC>8S6Sl}=2CF@0p1CF06Gj>R15!LHCJ!r66`XpA$1@nk!9<*nooZKcAejF z!JHx=cG%Nm($7u-E1f-eB<&j5YJd2sS&x#T{-OHxc?HHpFnWkC4>3^)n`NVlnU|Ab z@EG^FxOxw^0r#(??S)z55lLkL=a4!rvS9m}_tZi?JQC6k;-CI9s?~sqD~iHGU0)Y)|iT#B^Mkv-|ry z+1m^Jx_$R7;yL}@9>al#x_g7-I^sS?Z_gOL262pKp0{Nb4IO?}Sl_gb*h@3dMZTRT zLjEe9>GLweuRHdUk5`sb@8W{Dawr(XjQqPC93>qxLmW!)A1D5?d8rhUk4B5klcCzTPW3XW~RbG85@ zJtwR}#+m-RkaZ6&!~)W3#In6=iy?!HhC}{iXhLpbK(n1{Xw{Z3X}#0K?#zUv2^P(0 zw2`_!KYqOvucY~rNBzI#gP`9uqoGLZF$70dS-+GuxrDYbGxR8lXdPC2A9zLo7$M9u zClCSPX`zkpg);C^)TD3vi<~XaUlgaI1yW#9iji7nV{=)RhLIKbsUF|+?S1$ z^m&s}s+(w2h1Dg^My33>etrr>8Z29|6&ezZcu*4(!PW6G=Wr~mCr1z6ygb5>D70(o zy4v?((wU)hq$MnLP=ZF)dMfIRd-|)mtH|q32F>1hoKNWT+~feAKy6+t+V8m%t>lpD zDuT*enZ8Z=+Lw|0yWA+XStlHv+Wg!ij6AcMGZztPKEaNF%&`h{!f=+i3rXPJ1$X!N zEl-D1iT8c+esU#^^7qr9%8QPn43)@r!L6rrcD*P}lX~wo`N!0bm%P-&&|_gkC@h_Hj(g>B zqa_(0b|izAnLS#f?+HENmQ<79j|s?+1@wOPGQ$Pp7oJa0ci3d_+8qT77ieO;Oc()V zC09qd`}6Hgn2SmruMvw7iPtD;3Vw{-g zj9`LzZdFJo`EJjFy7GV~LOL;@POZHH#0G zHY(#f-Swa%8&`Ko#Wp)hZOFL>VcFtqz){N!NC93+bV<7{S<`th^0SJjLd|{l zL;BpRm0m7(t&#w=d#XS^)PyOpTHBBpz0dR}xJ9r!|agQJd8Xx3T{M@BD` z@o>glZ+L`Pvi?o0E5dKXj*uyAK(T!Cz!{h(b5LRJ4Qg@j%Tmji?Y_t)zTT z;Tm~8>rCqAHfj@$go1~TZid1Z@c^ffoLMxD4~Jp`&#_1bNvVhl?NN?{7UV9v}oA>;u8=8up@p^Rg+qmDgFu5HRMtcRcgk7{B#R z6J_4hLWc7L5gcWD(;_K+y+ro|OSEze_WI9$Utls-YS>6cejwY>skfMvK$zX?lV=L7 z%5F~gM;%u^`xUu6;{3TLV8F`QbD+U9Hq47D2eKCtMwq!hSe#N3+Ip*2qN`3UZ84H^ z&j`am&0~tpNS-Kuv&Pr}hAD-Ac;qgqivhx`zG4V3qn~AF2>}= zTSXp2#49B=`zPC<4W!jol+2=a_`s!qT02Mm}(67N_Ws0&QUvA_8LZe z!v)iw9O>q<)oMNecO>`}QtE`#V1pR_Ule8a3I(erude zTcRr@GGsD0l%Bu|ZLo%7fw&WQNE1(K@Hc|D<3(kKZ;LM5D<^+(l|DzGnKvbidwpI` zpO1fxxs7Q3=aZ|V%#>{q^+WA2hTQM&?EsL}(;MRjTsWolGw4kA@IYU;ZL z==xc9vV?q9W@pkvhZjHR8-f$s%Es%4YrE}>k68=8*_($Ro8DPK_s#p2!YGZE>o;l` z7P-xqnbK}xbV<7oITkDrb0+y(LO$)-v!QG#^5xvAsy=2FSVDTgI~caKL;S7eE#F>z zUf?aN5yc)=AnP90fZ2e`FOmV3zZVg!6lbgBB3}t!Ust4_iuWo`u{zX{lL@KW81HgH z+s&Oaq{^m5A-5&Mczbf-$a~{`$^(fI0t1PGrUQw=+5?GTVgs4KW&Qw|KZQLK3Bzwm zchUBw{{G&R15+8u{M8d28jxJl-DI)CIPqu1J;er&Nc|-0BvKesDbH?gKT;30jO(FQ zCc*7%Ebx{;zcY9>QuR+QBpzv({gSAM9;N-do$~Y!{Nc({?oXvZO-3ixVD;MuxV}3F|52(A{UJP~?ik;m%hE5#BIEXD#Yn-HMnZcf)k2qgEC0wI@T$oqvCQsu` zP}mbR17Y`oPsFh$@_lHAdneF}X16#aiqwPha$CGOub{V-{x$8B^g8e?F8Ih69Bsi1 zRLppqHkX>w2neWSb^Z`hJ;e#>^*8fnZTS&Hn2n^f&|o($<##Nq09*-K<1``sC5G{k zuOZlh(T;QLx4;=eMANP)U~~;Y?WtrQAnXi>rx7V^%O1&y%Wu0~3yO8Iq-G^AzMuv@ zD+D~AT%fis6?@4Sguya%92Z}8%>2z;T6neBJ5P(RG%P0r?qp@3OD^p94*lX;fAY>c zrg0HidmA3)Q!Pu;JUQF{Kx$7Lf}f}@9H>om&)edX%hI`Jd>!^=f8D%bVDhdBmkEVR zTw@jE?e-3uotT8JyF7O3+sV1aqoaX>gZ{B6Ys@G%-oRZw8HHQ4))OuDf7jEM?Dur& zO9a)Ua&IaV@tBLc`U7iZ!)WI%-zv$z^wUISLs5_^3jU$w7Yp&;PA^d#Y|t!*M<%Ci ztT`{Spi2Kl5O}grIqfglI3;^FTV_*X%|Bvy92n%Uo3}C4cqW0~6~OgtPIA?~%6s}9 z(}9`np2RgSSx(Fb%XSGWl(w%1j+OEdJ*@7ZZxLMe!V!nWo`66Mdj*@u;koDS&*V5$6xBI@ z0=K3@mzXRb4M@>zizMu-MA~k%(tk%|xD-9V zGJ?tV-@Oo5^$Q^&j&oHmoG1=31Q4>3)5-$!=iJ*XtEs`vn%Rz_)*N`?>%cjcXR*>qyq&6_S96@kdr(3)?B9~`7Lrk#8U74*)=yHZ+mzuPI@!vtT5~TD1f%uyV@E z=Y}p?*+_5Js())SqNbA<$4n;o%WaF2PdUEO8g|8vT*+rVWn@tHozuM6HwJ2XeCJIm ztm0Gcp^JSE$7_uJrS*tDhrn(l=xv?3tG1jmazls-#UL^IJ6x5{Q{=E~pWQ(!1cmy_ zVD_Yv^cSB+6Q@!JR9>#^U2&>aBnWWm9)P>7*Ne&0)U)naUv^{2b)91#FCK*Ap7&jycr zr7x!>@qC%tZ_{ZFrJw+hMZnnH%+Uuq(jeEiVdevf_W@c`70>la$>TFgI>CqVr07Q) zr67T~k^6t=@lV=K`!b+(gk2nAboPxq#99OXvF}ZI1I_~M`QpMh;seBv+Dhb(z8_=W zb6hh_1s)UMhi;-#dYx!=ljXAeBEXh2dsnEWBydGs(7m3m&xFVFXDWVetnAOG+bD{T`$4l`{#9@Hx6C3x zi5ZZwUUQ4Rnos#4J%aJPnCFp8Ez}0RQ1ZEv3m4&fKDAa__QT8Rw(KQcBvr#3iFrEj z=Yp?U$2?gxbg6sw4BRHLcM=;o6q!5 z%&Io~Bw}WGSaX(N%k;9@ijwuKZ0_~UJF;%tOa3$=dF8w}6&=T5;@P+1lMHe^7pcP* z*&*(>-+2386P_DrxmDy$3dB6P;=|atizRk8dvkQ>AdX^raA|BgXJ7pT#WUGjm* z0w|S49$Tk45_)x8&hTS66^IU*-i+R$Be~&so6r&S5GgK5Zvix^7|2Jb&z3oVKzYy3 zjHq(FjlBk9_rKOO$}Q<@5-UM}2*zy~{&1ko=G(0e`CPvKUxN7Gn6v)hm;?Ne9tMju z=Sq_8V`XK|&T62tgTG39>3#A!n4SDpMvvh+=0Yl}nL`5fLRYl`Fp4DSWxF z>Rr@$-)Q#&39g!Fys9m3T77dKd}atfUT^cfm$qC%hDj)rYAqCGJIR)rjWQ+$Z0ZWkT4rZi;=W>Ro-rjt6>@BvxP<-tKx-#omud3|@VxnlsKp zJ4An8e)DJS_beri8%qhG`jqcu%yNyo_?FW`WgRp=lYO+Nk&80C-b6p==(5?5|7b@$ zw|7+an=OTt7~_N%rz%Z)(@$}$97){0;%)P5IgbBqx%=4nqN{p zKgnzXQyeI|r4iV&Ro_lJGAc!4G5n7W95V|^>DSp;f3yIH8(%rQ=L_xVHCtHN#;55;JXQ<5k`s^8^=-qxGT9(Ox! zhauh;8qMek2_RSuyHJPuapQQ%2 z`-a3Gy4&yPr;4%NMgRsMUuYjJ2N(kvAh0Uq=|6k-ot`VHpMmaA@9|#N`~4))pQ`~o zdpZ3oM&_r?tCm6(_IG0+j4NpTG-_()oGv}${5}DFl#&V6RrGStJy~@0pkF&66gJ7% zHLYDu6~d}<;j`Ck+_-UjcQ|e-z0I-=v)L~j*A!oX%azND{h2nJ`pZQR-vIKYi3v!( z>sF=*IGMw3@`(A!Y1T%17)}n{d8gjH9AfkRNlo8(6oyyJ`1@!pfpQ|8K*!t84P)Qj0+J(=H#sDi~9gVde6u+Lu zDfD0nAl=k@=jTUqHkFHcCYt)UeCZ9M3xl9jzS}YExNlR-Ia{c7F+QuaI79Bd%HnF- za5ZWpPjsb*50kw-=uzv9M*iZz4&GS0T`qJ-?N;Pd z*b7c9YpbCpj6Za`c!MVFA)4{GqbDqLG0;4usiPCUeJ5ja_F-t!J@4AYwUH>KaOGo? zD7BON?y*AG^fNS#pHPPB^0lMXOtu4=xxAgE5zxu=a_wpT8%}2X@b1Clrnx^;=M^(d zB7>;~;mInaNyI>ZandrZuIRn)W`E2@V>(7%XaAyU^sg5f7aPEq;H;31w__e%Zg7DY ztD$8QyWS)Os)TcUeO$CcUnjJbDAHnNVaM3#u|e@Y4$o#lgFjLJ*A~X$+cSjC!!{y5ZmA;CXb347!bOq!z@Qpy3=h{OxhZYH+ zl$Z=Z8wyWvrq65nkC;sC~4wymg zMr!wV?qNaI-`1>o8pQH45HCYer>{d}OXfqm2?T#=@pG?s6^bKyA^V}#u!Y)f*Y27? z3$H)EBht`yJidHhh8C$Qv$0OONiZ?e`U4cgu|enmiX)KInI8DZIq5IHujPT=ZLu68 zLyHTt1@Gn&$Ab|x=%t2n+Z|w(=#J%DSIm(E{;A_49GNf zZDdRqV3n85iXQL22drQ<zR&$aPZ)U-1>?NaR7OzYCsUCl< zhr_Kmw+}N3+x}^=>#-%dXg06a`}egT_TfO1?imFa#M>HtEnzEK#euKK-zOel<73&~ zgwq2jZ)@HU2O;)#IgxB1wx;R6%DMw5CSJ@9{$}dSNh8vnQw8KE^!zu9hor-PZOpi> zWP1EUx3i816t@F?a3P)SDAszulU}i4ZVVRdXNbo&<-k zh4VvU_!Lc3JC=DzKx-HDV*tE+0jf6fQGX@yJ@~7|nA@j8C$1VVrH8m)Nna>^DL6Sd z+jx4J(t?^!|AU9IPH{Fq3~$zCkO~*-daLgvLPimCYVR%zS&vFSbWjSmsYf?!-2*5f za5uehS9ifO)@QG~x(cW7B}~}4p2FhxIEhcoD7@2=aMu$aMw5gz*y{S2IhL-yhXf3w zQ>=(AeCHty8x{N%WZx}&+q@J*mhp{XIJ>tju)*vopbL;DDc@qV zd0r6JL;Ldggq}KWqcdzgH$Q4-LMm!>p<#<5&P4`Z~^D6R7w`vR@ z_YhbJ-;Nd9tLi#6s8g9rH?YW>f7wiQXI4&riB>A5)@eoLuzNf{flfD+{7#)@KQ}Tw zocmPCnRdeL-1yuf_U!GVW3Gx@HO0IZ(ygJ#M7kTFUdsTPOT7|d z4~|aXtr9M7mB0FEzYN5@8UrsvigoDQ0HxGxnt5POpMSVLHM&mOX(%m~c_gw4MBT2L ziLuqjYz7(S5WU$x=9(!W%50EmX65a5|MM=?XUFIC=Igbt$Bti_mpD}HKZ}IEuu6++ z6t(3j^5&X_Nkn3EwUG~Dx=!P^5583lRW+TO2D+27iHbHkmIL$=+G)70^RH&5QO~z^ z-f{8tU*197i0@DxUY}_s_O(ajRZr)n(HXkV9UFM5^07d9P?*zV52k?w6mMY=~uu{okdzEOrs;Jr+MTR6!?ubD=nfk%^*M3BD9SpwR8-b z>O2kH8OtooXw47r1F@|tB^8dRPR|H7R8S1pDt|z9-CosddIdWrkVEsyIvb9(R0ma| z%L?=-E7&&Cv8B!Y{`gfOrUlVsXv_V*j*c^uU6jw;Byy^TC*a_C&B&oKE>^z|uPOwc zyN0@M@O4IPYgOPfj-?`#&=!DjCpXTpf>%Ep8vqId1-P0=$9-Y(jsdOMG)72<X49``cZ`_{?e}QPtUd93>*xlo zd<%TU!a19<;`HwDIS++fSxP&!7#|*C7s}#_1E~x>`!OXZY5xAIrN5S#nOR3K6tJpUgzNU zcIARUbn|NCA=_Ts!gJXH`u^!hQQGMI;mVa?)@(dS#A%GcNgD{UJ)Dv;FF9_Ex5oWu zZcm90V$Wd;7qLC{s`d2MSb)<}eZ7JAH!}IT`%;cA-kPIw*;AK-q zscCQ&aSJ)oJ2T{XoxR0v%R2ris^$9CN6mqW6VWK|-flEKI6oOMa6P8Ez@}!fnElz~ z+x>5K8gHQfNCCgvJ@lxFH`+w%dMzz6qpfQUuif0S`{8`+?prJP;vlkfJdB56dh1M;wHlq@C5`|m?Xs|BDF^7+61x+i=1 z_oMPWAqyMQ0A3&E+G)RUNYb}Fj|0+x&W$prYESSX%;2+Y8u!-D)k7nGsvp-seF4W4 z7fcN|zEgj?KioUOHh>=f{vwi~RK}CI{itjS-&p}H*7mi17>I(tZ(n`!;$@tXF*3M( zxV34*L^hk6i+=l=H}O#m(?2A7j7rehf|%i-5pJ*6sLNq>}$HhQmwyVd1#G+o+r# zue4_bXLO(?WICGSp>@+|*{LhPeOJ}^X_&whehmHppwRZ|mIf+HKUm#RW&WHWR-%P*lN zg1V1=lZp*CRo*zhR0rqQqTv*gLQC(wp%Ih|&OJ-LX8ntuPPg1AGG^eE0F$zpb2JX>VE|(aBFBkb= zWG?cEjxP4RU1wWhlnd=pud*lPfifrLMgrNHQQ$82&`GCTsIYTfL|ggJ17W4kD)E~F z&v~MR*LJFkX!L<&35(3waKx6x3LN9gs4I^BS0w8NWBJa;h{evvVR_ERmU+(mbw$qm zVjJwkR{WRro1_i;k8H@;Hu2UHu zGJ2RG?$2`nB;MZjCvq(CT^=YWjZ zcgnzolZ)@jdxyb;A9U6rVOXLLHzR`5SE-`d5#7{MKFvk7(Nyy?rlqm+*(yUbk$BF8 zNE`Q@%6teFgiH;)y{SV)h}L2^g!wfd0Ivuu@ZXtuOJr^SnxZK=^cL=?F*}AnjVzTj zB484!BS33S+n5k?i_!bYN~u)SnEX2eONsHc*Cd;aIjz9;wTGL3+@tP47-Hap19=IP ztsni|9xCF9rduGu#7tdxU;Wp{9(;4@Wtp0U z>Fd{l0-pBM6-|CkC+loQO0SRSWupuUij#t`$VBMZ&3AX|v@f2H_X!y`xF@i(Lu~Jd zw6Y}hSv-G1`WEjK^98~&74AcjnNBp6y}~a=HIyOgnSU`PJp2)A6DA+c^&7@j8N+rX)SOSgso5!mLCBf3m^r)|Dc)Siopm~M1_DF@4aPwPlg zZ+t&-q7l~U4lBco$bmrNA$MuKyR038%2)6_TF@#Je&;9Oh#ZDjyF=G4)Z_v;`6?(| zowi25GTj$vRRJyb*j9pvE84}|dcUBwUj&=`?JfFq=F9Cn#1rcFZ}ORu(Ko@R9*>Ea z+}#xPZr$Cvk!7eozj;13ymJ%u1`Y|xfQ6=bPWDvEXL};fdt8Ovn#`%_c5w$@FzM4W zMq}*em$})-5AnKOkiZ(&V-aHTf%vd6Z1#+Jq6$Y(O9-)JnB^tWuqoVwyje(jp93qyYiX?#vw63g$$XU6-*8<6mEfA1H5ssaV)BNjA40b^fcf_9!Ic3vlM)-OX* zH?EO@aNb)YHzX;U|q_<8nG&toEHcVC}6__&`@5AdODOAulGd&8Hkxi-cYs?lju zN$VUtYM6|^!ir{+-HV&&Zk%dIzr+9c39#j+Y?201IQ`RR5ip%2(|sMf-L}0$j|Bt6 z;I`B6#oKU7Kav4g>el+7D=P0Y8NIt|#`S_}LZYiH9%9eCJG~u-=hMo~hEAh)n5q%X zmu{m@{3xp%O-evx)3&T{o&=#zaV=nI6kAqry^wSmWIJkQrIei9%QQs8Igq}ppG_^MzUl9I zqm^8WQqUk%HcIDm-laq^GzlW%FFILWGia9yLmfbKelosn=*ZhiN={c!SRx0tv zuG8P?%9E<9+TLM22Gb8M_UQxZj8ONW1f%gVLCFl4^H|948f?0+h&4sv^Hz?UHNZGFOH`C~yLb_U-3)$<=zwn4%$w zL25}P1})>`&YeLJMIM;(5OEcW|U@1g@3xYqUNqtZ=>PsXiXIR z?`+K0oi)FC-;Xy73U9w+66Oi}gYGnf9kHlzf zT)l8xOj!sPS?*`N3v#uw_cBWpYoNc6Wx-k|a$z|TIxjfHP058;n%ZMOCU8;B5j!gz zDjbs%1x`r`GNq%0;xkh~WSGjsUXEo^;u1O&D~cXdDjK|y_uY)}nA&#jjgxdsxx7Da zo0&5mNa`Bf%fIj9G;Lo{h+&;D4UsvMSc0VEh>x3Ivmui?|A^+8+9PI5;~+PM{)sDX zmpUHY4o$_Jq#EF*1TEUib&qp@jG=?ulsc$Y)@bBrtqr09t+ueLX5<45ajQbv? ze7zzo4MX8&Mbv0+;kqE&P<)zWl&uC?)~x{ComGnf(@VdPIA_9Nr#$_@p<1?CMc6Ma(Ly8j6+VHbrN9u7-(`4nbbky zAM{WFa0phO;La7zv-Usg3Nnxu?;&X=zY+_9t3{ z#o7*Dwnx&LAaa(*kh}&i3GegIK&uFgX`J5iSo~8zzA9+cAWxmAf2sjCSf|PEsqg*w z7MD8AA91{xmkR=&)FSw($Yj(`HOl#OGZh^9RuB#>$1a~P0o+Iaf5@|~a}Q3Oq}fNS z4YbjerOCxVA8+d`WJHFRdwbpd8E4#-UYxe`e~W3&a{9`5fceXHma7t>QAV&QJ}eJ| z1nPJBvJ=TO^1AzwuSkOn_bv=XjQi}XYn$Cmze+}#R{qtPij`>-oOWJU zY6XuvM%O|P-n}&1d5_4zxf-wN%m2usLCnQ&y42&JB14-TwS0$xcv+k{{;n*Wo8emp z98|!OVewyA=fmc0*+|DFseibrVo})*^|Ig}pktNGYYQTUH&j6-K^liOh-Gr1yq=$} zm;l)N4$4#xA}X;3)6#RpE((J-?;#i-B~jA6c}Ue@*aE^huJrA%r5dkkLc0Xv)%)s+QM3+8{@dJ7HK<<&Ip;(39|a$k$ox~8`H%@I7@jO=xa{P0%iwwS+J~r@1VcS4Mph2S!UkA# z)fXkD&bfAQvVYyRq?d7MK3l;&ib-)S$&BU-{&`Uo!0O21TiSbQjiuSKahr-hmswz zWh*VWtnfs?46!mNT;Sj+@|$B(qp_hLwP@}4lVxmp5?t}-RbSkjEXQY`-vSDO&z!MB~f!Yf9`O(^mYvN zFQL0_N4nN|k{HSjDGSuo$e!0r3;F00^fwdW1?c)i+HmdkQqF!9;LCBmfhafZ&{f=K zB6IOCp^B{;vJ!EVsD|98+0VD>VEh1`OYHm{I@S)0@Zu-5-^8!RRDhz0vqtxrwWLgoq=((a+wRK5y;XIf!tr$ ztrYeF`jth!eeJ<>B!4r9zceJL%&A}x1vI23*81fLEJG?RQz&UP!#{5rOiT27fl3KT zKj@MAzI-{t8OJ~Ee!WSQ(P@$a8>-*FUQwDpS5YkS(=P;J_pGWdBvYo`^OZ_$KCAOF z$`YfV2@1#%RPG0!2zl-Uy9F~_{#5ZYhZ%C#4jz%I0C9sFxT^*L6^ly7+_7B87JFgQA8u!0I|+0y=aK$i zOigz(imA2$TO<3rjtOh$b`-M;qq1Y!mMp)( zfP>qG&RE~i%RF{W`?H;!t=m*rV8f)QD?OwN&%@t-wPZ&>Q~(&&N}I1Z`tXjaLf=3D zaPRZ-@860h)2?RML#iRjtasS9?n01C9$&xU$NB;PA9$z-guE?`V>ah5bCHy!Qh|?u zV4KY{;T%n3ozs(6R*gW0vs7fo#3l<*P?qHWH-VbJl}t<=j2eRM#h~%^(Bt1T+qmLf z`_#1Bz;E+YQ|tTO-XidPU)YpWEoI!fy9=cmgf+C$#;?dCME%lhR2SB<$S_R1cKv3dQndK?(P~*bQ~@gTMQ1 zz&q+Idb?nVL7vso!YLVc#FPVM-8g<|F!n?M>J-=>$X8fUJT3O`eo|BRc#EBfd+qOj zs!wW|`oD@5` z-rQD)YCp6#Zqu4&582Z-COJr zIR((E%v2!N=c>Y2SLA-zt4;R4ucO`Oi(G=j9TMwu7oS{Ne{{!@K{m(Un#TQ$G+=rQ zB|Px}t<~Y)(Tb!sh^<|aPa{V61&SlwNB@n1M)(qY1Ql8!|JPA!V$jXYR^o|9^ohGK z_Z^A(r?+tY(0`7(g5piYaPcNZ@ov|ah3&jBby)^Pmw$E2unn12{?d>ZCj$%NyfO!5 z#%H!fu!mLL+K26Nb2~KVyS~rDf*=w@%TAo{Y))uk?6}1XI2DnnUeoWZQ=j1GkRmiQ zN|WM~4e*>;Rl}{Zj_+zUGrmxh#foqy5wc-+X)~*X{W%LE`-ldg6F+q+E;N`T$rH}m=qcD1!wd3`190dpV*#-rp zwtT8eC=W3l3-L96y@!>>_#Luahe>!(b4m{X^-EamGh_x6N4( z+b&r3e2E@>zZTg1W$kNO0I2D-pBCv||1Y}C0aOE9wqQ(+JHv%wBH1BoFAY(K(H6DL zH0T6zj~n@QbPE%((DKTgP;pG57iSe^iyS0v4tTN)e`aBw%b*(1NUSxzH#3_>JP6WF zAO`BfUpTv`cP2lJOM?7Y0RO3KP= z#!m=XLwwW%^hvg1H>nryRR*UNpI+Pa71OaSV+Zb;wsDEfE}lg3rtgY|*m}A8aU+ID zytL$w*X{)Q1d_}HHa;4>R{YQvsShVh&9w-d4FB$M83uH89e3$=kR{!yFOUq<*7Ll` z<5x~0!?n^cSS?oPWjK@80{fx&)_KmvpwvD5wmBGD#-UdlU%KT}TSux`x-KBUs^?Q& z_p1N_Y)+FKJz!GnhNDeIK6C(_OKvJutpgco=f?7i&9iB{{>tvrPv4owxS-?2eL zrc+f+Gmd|fdP9KkXA>*43>14k;US#n6BM%!0^A~MD}J+8;Vth0aG0_5YD6*3olf&$JO|6QvdU2~%Vr`}i!Zf1jf#MN8iD{= zWC76#I>-jFh$4HvBlO(<`DVV{UyjZwN_$Y=-1cJ$&@NFDfa}fOerf7l8e^E6!X;PK z>(;p!pBzn8)9;3*UjAGp!JrE$>vL51f&1{l>*#r21#(yi!Aw!;I&`L!tO+@b$?7Tr z`BgNkn6ou1cTb?Z_e9o*Lv@KV<_TJv7tykF=!H<&YABK0Pkf#mX^^Fl>DiJD2mLqw z&Q@jC8I{iP*3U-%*`hooPtmjU6ZCj9+|c42MXgg^8ne&9i3h`Vn~-o;Px1uBg7R>9 zTUI(TD%`dR2f;xWiZQFS+V1D^Gj@rv#spP9aN}>c4fp5tRxe)RaQz>?&LLPB9ca_{ z*tTukwr$(CZQHhO+qP}n=gc?%OwDqt(u*vzN+oH${X}2P4DS-_V4t64Wc7**%&n9Q zf)C?lzsKcLDTo1<*r*qG4*RUXdUD%jRSq^fEO-VJJcMP94d(A(f2M)o4jC`g@B5fv zU=1&~qYE1uI`A!>6i59$^EZC?ZcZ|9z{MLbWlp$)qZKm;0(?0v0rur)y$ z`)NOSIk8z>d!AAt_6n((lrwKOVG1#=>g?FOMr@`~6Jso%2lM4%9pnrBjpZB(lzhj7 z;iDxR%e!ZUtu|f*Jh!VsXuulkLA5~L{Hxiedi7I_UX&Yz=(LycWd){uj%)Wks}#^J z%5xvC5E8=zP2y-wCVBIQo?^Q$luuw3pfg!BeOajh?O8A@?3mWgOIBS_9)Vg+9s0Tc z#4-;7Z*Y;K>RQK_%e;oNQ>#EqIoI^RShx}>VQDa#O=#2I{w1Ly<#y8}xhiT=7iVw8n$y$tg~{sc9KKD84LW>zsBwq4lF;$9oVA9L9j7qP zE@E)V3-j54s<~OsXo9iXlhU(fgnEeNPt1^PmpaI@;PiW$3-rC&^r|(dHME_|xoo;& z2HwCebR?yXE-a&M_mY8t2wWCw#-iOgIP_XjG87jm!uBZE%Mzo#j6B9*TE=XfMI_G(nz~k(Y5ipk^$JYhNkm(1RV?KV>ge1QDCQ=N>E{e zbpFG%py{InVwU^j@j2ZpvH>cLaE2{?kj!16bQDL4ySdgfU4$X1vipy`uLc7a4w4c! zmwD)HOk;JX#?)S-bx*Nj9{VHfYi=8sVa&S8Xyip2H1u~Q!`aIE9Qm>#ym@rot)NFl1FISg$;y{+K|xop{;@+;!8L4RJc-b_9Q zq=xuwSdDhh|l48UJ$SXS^xA9(CtrO|nr>u6m)@`sd1XC-xkHg@Q$8CQB+l*Mx9vYI( zB{cVJOJL_^);xK$`(;VN40uJxTVl>7)cktEhF4~GoG}eAv7o&_(~+p%w-<`@js`Eb z)kVPAUM7436>XkbH98vcZL2W1_onTxtDnf`jqT5k7F~m)SxLgby==0?+pp*ra%ME7 zfGkHx!K>PS*|&`9+s4}MALl5KPOX8tm%#vugcu|hldm_636(M^WfVaP)2NWuG{^O&g`JY z3&5|&Rr~#)`)A}mb6DTtD&g6Q)BR@O-Q~VKr~IE4!b@@oznZQGwmOm-75n>o~ko>1Rz--eMR=hVd=d zQ1-NsSvQFjA3E2>HoH*Zv<&_Fu`fls8`F8+5gx^RcUHsl7cN|6bx0{SH#-Q!VqLf$ zJqkAC7!(QPSQG%`SQwabEMCLEZ0h!S_J5~I901UX9T)qF9T@D=r@#lYr@{Hsr@{9j z(-1}J(-HgN8Au~90TXB>pb%_7+mY}L*WUKJwh~R~yNGsg#TU})W470y2@FUX#vY ziukZ+L5-!%-V?mR%&19FQb9}~4sgW_lCKfP3LGBUM~m_JUl_{e@n;et03Tq2Aw(N< z`GznZuqay-zMv=##(Y7MPsgL>?J~CEk&cab!Xs9WK9q+aYPK+>-P|Ovo|Ff$F>|~x z!LGZtiF^5@A)2@`pvs1Ac z2(LU4bGzc|+?&0e5%dBpnw^37bgwa!1cNnsmO^JP@(A{l+NmP#hwJAx_JiyJC$Jj9 zCJ0`<-Gtbn=0_gMVM?Ep|aTHvh3G^kFHxp&So2P6nqYzc?e3dJm<>(TM4^BkSdcIs{d8C&9>FX$1_3w0d$y*@5w1!F^d$4-x%7p~TQ-UdY`Qyd5Mq7fzv;}kGtj+f z!GA#i6aBAQC?l((g5BE!3kYYGWC13ukmn=fhZ9QflGv7qQzx|7H~XW9BP5b1$PCPb zQwkE2W1_L5B+Gzk2%9Sa90873hWp1CXaFaR@XvSX988Bz9|uctJ0)^#)Ac>+{#?1d zc&V(^scEWCsz8#vZ`hZY;<$LTWRoq53Y4?NeGpup(U`VEyo4X%KnUhUj#Mtr!{~6A za4ttPZCE&Rfk>r-l3f-d!5BFD^jd1B-gZFb#IVA5b^Q~w4ARwSW#@d^Hf(bBl`dvWr?f{Wx}PDL@! zS3;&)#GF?iuCXe0k&n;txH}SX@~L!KPM%j4L}<1$VZlIF%$kdWdGG8>-<9^y z7!5C@*Em7KZG>vBTPBO}fRsC$+%?-ihXj0Cjiez@OV{;vLH-!)y$97T%+W|s$rXtudr?WvN6!Xt>ON1n zY?V>gk?Uh8nrp|*Ze(wDL$R#wE_kB;c*=Jikui6+H&@}!@S(m-)}#fd+JFLCDM$?E zg>}pGEF2Xzl-q=0JONyyu~Bn|kNTugGgQR|{?ocQ@^`5(F7A?d9h7PVT4sO3s6;yc zm{GwL4VKIdQhk6;T$LIZrN}V9~sY$-BuQ2+aU!{{>Wu zu9mMahDd!c^t2_X5*J25(2}B@6X`;FM8&qlrd#%n-dTvI`bBD5{%hpy-4m?JLl~8u7uqC`v?=e7T;Fc30T}D))&Fn?+Inb{^>VWc~rR zsvQOg$%^B!#K=#IEuC@TR~p>&fsfq8)#alCG6~LAvExs2->FP2LSDZnqYOPC-@VV! ze(ts#iWWNm7z;H`ojU?yL&XU5koP9YacE|Rl)6WF<;z1{T6T*T3HyfTbR~PV?N;acp!ZX zJ92W}p5M~#fjzs^&Bd>VuQL+)XsC4ej?3w9#;7mI7wEmKQiSpDWtpM# zStEceoa`DoQ9KcFvNx;m>*Cg))~$AtlEEFsuH?*`Vwc}F)!f)03fRedvfhzW&dZ3=h@KJrB00V(r3o~ zd9xBgB4@=0Ru1`n3P4bVVN8pC@g@QdvH2g~o)?HvD zoi1{K9^+;_k23w?C|tR1X+8~TA$FHkuOH8YUvzie4+WxR9cJbiB4_w>JTYpAaJM#+c)Ke0WFg)mN4{{BkYfR!O1Zfc~ zP<$yHa_61vL0HDDP7*b8k`-c>87J&QQA{;+(k2a~2+Y7NNCYz|Aibbf+7Jm)AgZbN z`@Y*o+Cf4gdKjRhYCis*X6&8++0DRTuR@`wh2ci5E#}rn;%B-rvhCTB-pjbG&Tv%y zhhcC!M@f84V49s8%-06rbn2g9^e5rV1_sD@ma9cd!Dq_@hd0hLbNnAsj=Pu09*sVG zp4@sGpXPizjk|%9Dzt4eM;>kD(!SmDTX-+`WGL=k(bCqY2E4O-?-~o2%XnWGXQ8+W zBj4~2+hmsj=xaF!=vE&ES??HMr0sGt$kY7@;GH0A`#-o-?Q~*@NE1d8kKB4Z`o}GB zmpbm`BBc{-vEyis$v4!=6`?fw6$EFKd=>-%BHsgJhP3>ZUPpr&3ljAbUKy=qCNL<2 zD&Q_4Eb}KaG!A(dJ>hxB1`iYLur@6t?-{Z?O!XZl4_bgwd}m7qQhzn}SC$!ag~x zvY)k~-oG7V`8QORW%D!3dq9^*Yb&9JM|O8PZmHycq$U_J?j%C`J*|4mu+f|IM#|$% z0QK@)L(AoYxL>yvb;-e=8{;xUsPji}%Jj5lq&>oq)JQ8nNy05B&hU}0ir4@sp^)o9#`Va#ADAYRDJ9}{rEEK^j= z24c_2mI@dy|IzcJqX+;1yQ#$nN9##_T`;cPDB|ysSfppsfbejzmQCwP2@EsUu*@bX z!Y^&Sz3fL&)O!`EJ%{?T$lHGpPt3)94h{ZAq^_*kIGncI&7--Y0-^rhbPlqxS*j2S zelgwcYHN^Ba+IewOTiSIb$aSW7yQ0D`iZUUt~K~#fH0uQSuZ*8p%^9cILU@_+xxIa zJD17(X@);>CMATD3%|I*=(oS7a2o;~6{`%Vb#0NpwjjNvm0P3ehN-wxevEh%Y9QTN5NdF* z`ieuJSE66l5y_>w9*_ohHwZiJw{LWx7+Sdp2$ zg{(+Co?I9qyc)P6cy6|EPAGaFY(|ju=mQ=eQdaxL^ElUsV2>v2ygMNh zY45y?9+D0><$l`>s>e;Z92^W%E*g?x=VwpnkM+pOE{@&xva!Kv;o8-StKr3GmY39%n2|#*ISg?0fMtf5>_^w)JDrt4 zZfuxXM&slB9k^_DDP3qUVpYN+mY3fgv+?=92u&bF7&Z&RQFk0>;fRf3?m3%Kr+6n= z@$Krl_@*zYEA?w*K8=%BNt4!swgL)25~Kq1VrAQoj&ZA_i;l)$t(NSe`otKY&u?<0 zl(F#QKKW?$2ejPCu;Oe0URNg4jZ=~;*jWLcl&|b*Dc=$j?XKY}oh%}+nFfnV4F1F# zB&zcG`A7WVM;&($IgywtkW8!ItU<_?oG-5npkQW~j6JtAo^7x4h z5E9+X_YLOYoTf(g3_fndc8$uO5ORe`KOWeFHXVT*(-x>w{H_^K*lc?1Ny5&qxf94b z@U1{?fna=;a-KuX6XvRq%}9_a63#bWX4#coqLHfDD|0Vk#MNl(W-AaKypbezz9i?%c)g z%(iGgL4<^G#XI{A6%-1Ec<6f&mLgE`i|g^01nKSeTG#ZbEGF9jb=^%Zv(5f7QHHMu zwbk4DHmX)`WT24C|C~g6Tv>av!&wN6u%SgE7iTF*+U#L#l+D=;BX|6}T@rF3^?Muc zv=AE6y(T>O!Ktq1YWf=@t+V{FLRF@GDd58%@wx`)L(TcjAu2loFO$6H=|ihzZ#B($ znPyIuEGC<2lEi}|8Qls8X-L;JTx92$!$V*Zatl2bnV?N2p02D1$Ji7)nls-e!d7Fz zb*0n;z6TmzP%Nt326KA1V$PMq4wE%Kxf?;TY51qJx4Vzj>Rx}_&{3R%!|SZF=dEl8 z#kQ1}VSQpt zMs|H&k=6Fxi)72>CZjLH9^{y-V8F2tEoNeQJWN9yuJ?QdM3o z0ki%juX?SEtlvu#>ec~V-c&(F`H7`|KO6jrrcIENG%B%i5J}|QGq%R-9C8KsX&+sQ zrJGA>5<2TJlz9&!1|-)Z@DKZPH>{`P`2*H+y5}Y?kN=p1Lg+28W~6=p-3}LBcUf0;@h0$SL9=TDrss^K*g(E}4Yel?veB)nhrLJo zMyj#^rwFka_+sE2u1lWT^q!7q8}_S?x^M=jGrIO)X%8Q3*ULqh&Epu=6>a1Sk6$}x zx9y?=c>jO|-4wDQ5Ix~mI;jKme+GP5r#?3j{VTm*ws+P*1*;BPH4F^!R=ME4s(UcP zOF2^iUYVr>d57EM35=x#C2z~AU!tXloBoFSeyTTC1@*1NJ%+x#HZzBaDv zOxQ8TyI7+|Xlm}0fq6y){Xd1Zs;_kD?I|~gO!&F}dET*WoDS&Y)R;9J3PXMk?rWbj zK~VGuC?2yLQ+@AX43*yWfT>noCp=}Qd`!!nv5#^8_t=`OTKSB3^;hTGS{x*;&u%Ff zcIobPjWdPXQDLYe**ZXJa-a5@Nd*H#rNZU`S~Swtd*$IcD<+3enUd6OI;0FOqcsRr zsGoM>BqiQ-e5fxBXmU z)J(C^Fs0ar{57s#<1HUlLp@nosqewT5KgL&6-#URT2EcA+u2Y%20GUTKvu@rxwZ7H z+0m#Rt6MQ`V<9(}%g=aKR@9j}wi9tj`B}av#`)kNgu5(@U2Iry>%8{%+5>!@Kb^QC z1K3PtFgW$S6MiwlRNKulfoZcA1fUZ9>^piMo(&RkK0HEg7E174HuWPErNH{HY3zXQ z4t#Dt^gi`Y3h>RYBxVtjcJ+3Gw}E&7G?RuAtX}CeK^3!7K_6l?i+|$g62*4-2-_5* z$@cK1%xuKHX%XR$IJs{#TG+JEbal)7`%h!&ma#0bpurcvJoU+;(J|ESNn>2@mRnF` zoqYpo>0m8sFgTgLP3J5M@A`KxVk4N3b`BFGVL^Jl#R)KHO0_2i?hzp)LqQFFGu)TYaQW#_Bd zzt^7+WqRtoz3yF;=u?53rNbs^2EY8*^u!2^V!oA^D>+s$ErDdO=0R*U;-}Zy*GB)# z{2Qd0Fx2(?gMz^*c`4^+>O*5U&BBL$!yjJ`D(K{0DC*``!A$kO4E$k8femG(v=~+U zto>pJX%M=o!f2owi z%HziHHl0@Xj}8(|mlT{vy$@PL@pU7qpN|tQrt601&Epl6OjvL#S($4ZSAW0TeO4VX zv2_xW63{(|U|sK_T;^D$QjBAhwZ-gvQ8lKw{3s+{lZ9K@NKOMmxe?+QXtM1#XNttm z3*fkk%cnjVb^?0=GnrFlGkS^&vGufUgk+Q6+{>##{iCt&P*JqtU3H5FFWdkPZxxMC zHb7OXAuwW||FGDi?%O8(Z`d0)x8nZ$8Xa>DsjRi9X4h;dSwxv_s491g)D~F(va$ao z=H82}apeGRb3cW&ZCh&};G*1lC*(8+Y3fJ(qJeg>o14S%$nHGh5>)gDPqfa>aD=(7=$ zb}eeVe?xro=uU-09l3IMIQ!6Vxca9EE#Xz?e2pzVWW%HOYo$A3I*4Lv$-yE?@>Y;t z$@@ADp!@u208?8NGVpIM~%%H0Mu3 zPQ_gEjr82IBFU5@M%#4QNRgLuNE^^)j)kehZghKfir>&Ce*oQEgIqC8AO|%CBbU^G z=hie?PB{#61TDABPaXGPnU`qne1-#CWXd%9h=`t}^pUijbaew;#1L^(<-`$nBsYcv zTXeYgW}3bP40p1_IMTPA4-uEDI|n!LZ4VbY(C5XbmDPG3BbcIb7o*YlFLLY7|&X zN^?h+SZw$IO-Y>BLv4t;hogQ)G35svCNxA$z$4BR#>2vb)f}g$E(V+Dt1gE9)WG-6)KDebB+5Er@ z>{qbYO<_xoS{>);gdx?YW>3?mlRZ0(I`3$4m1-9^X2BROb|UanKbH8_pl$qwVdM22 z9x+x6jaWvbb;tZP?f}|(b9dkJ{r%aM z#XT*hYV8z%c)+E~?Or-5DqL^P!p+O$j?a9s;5A|7CV_F92q!oR^R&Vq(_%f<$@|Y3S3Al`wC$p zLf6QQ3p?TodLZCyR-Jf(`@7GRn!J1O^&LoQ=cxiOQ}wiTwH~#@C8hnjG=lH@L`DOb z6+8IV(o6}i)QGA*9^DdN`sv%i33&_G`-ub|(Ih5&njq)utv40-m zMWA{kk8(Qcg-4Q}J$=Fbl0;6}WgAR-28~c%#3Do#w@XhK_vC}1{pdzMPo~XE`%MV! zZ3&i94*{?Jj77=m)IsL5m#wvTxxPLTFg+)v77eioO*1S)0& z1zb!LfV`L>On)8~%zGX^M7oF(BDt6#NO>MTQvN#^oiHNY>#gS0#f4aVRGHR1UZC9G z>A6MP&%l1NJs$=}PQXP$5|4{aDHaofx{wLxx{w)$SXBCN!&SY||62X>bP^LJ-eg%m zICx$*R$XTcG6i9X>v zjC{bqx(pF}0xlf!+?Y=oBH@@*QeEOzDEXf5|B+Z!ys*IRo7h90>}OLY!6LGPmrDwN z##wd{1(k)L?o*Ftf+CH~<<&5W6;HJ$vq5DzndNZ=8IzzBI2e=OEcR6Ywq4ZjJonsc zwmh#s&YEWJ{N8x2er7*lyzY)Td@f}NfQ37-BGX0?;tSl%XGoL$wRg)1^CzWfN3LN2 zB*Kc`u0`2sV|S6_blqEh+tGatoxh>|cmN6j2U9oC!#FllsuoPLU~ORqmj=&X_C!kD zeST}sTjMUGBVd5Jmt3P5g>JlSOT8Z-B9Fc0l|f;tCjRVED`mjVLKJWXz+Ubd@$p0d;e=l>alIN%uR}bbeQiHT=)B zz3@XEa8o78Jv^-P;;b2_fVOPPW_cIUzP9Hn73XtMS$zQxB_;zoJ~EMrvu5QS*UQQ3 z!4yzYNo{XgC~Jh9%#OdTmhaXL(`7u;!oweQ%8d4g--!pZmZ}+^e%4BvUhHLAls)$x zR!`K4TCOe?KeK{iOP^-rgB&Q=ax>q68Cm7yVS6ul%9}(T zPq36V-bowLl=qt2$reQVqI3+y;LA}JJZB*o*AbgYei`Z3&Eoc zc2iRcjRGw|IG9^89#z^3IsP}R@kiYPfFcw&;7thd$+*}>K_MbG5Y7SxF4G?-co~1* zvMh7Xqg)Fh&$3;)tX+ZrA!sNNp=30Ew9UxgM!G9-5vww>R370huvIg+q8$&+2qjJk zbjUl14-}O5F`GLc@)`do4&0O^W-ZlV*@_hA_i$M`8ikc}LjEWuf}8Nmt!f-d{+MJx zOK~y3o14Rx!qzz3-C`G{edkyGX5LPNBf2rx{0=LOF4-c!j%7|h{)qro2H?KJ;2zR6lJVnHmZI?dO@1yrO;pn5t3h6cln8tryw)uupp+T z+hBl^>MsnU<+awA4s6WIpZ;dRoZ?OP6GCb-`Wnv#Zy}hbRBs6Vr*c`=A)hfg@ErcE zFdEKe`KCYBlWOL(tSjZ1%S~J^b?SToycf2sP71A|d{!`(McL{vSdvN8k#oXXppb2B zg>aWxzNK*c?>+^Fj}t{(|C$ZOW`w3(ZTLwtr+FCVwT^6}EuZ}j);$XZ0KdN`w3rmK zRAdid&y{a=&w|Xynwu>8irj#fK%nOPXTMK=%7bjKP(}yjjsbnp|JWQabUw{*d%s1j=9^mXWW-UJ92g59JLGzBRa?Pg31_@#fP357d&x9^4jG zRM`YzhEQ+!Vi!kMQL{y;R<9K%1?l)WnyzRrD-u$vSVa9FByt0BFX7F1nvg=Q3jb-y z&1-ow*n)0r#ZsgBuVcwhxrJ47B3{e|ERa@#Tl2iONWGrG!Y^Jdrl7yGcL}5jHk~Qz!VpZU;hWUB(6k7$xOs zY`$?N4qk8=lZl&IOfoSsICmAfFY#KPHTkyl zck+z4<-zO8F9XL^SIx%eBX|tK6VP!3$~C!8#SXY*B+50R*>3k$JU70xBC<~;;sW3cM<%HRB{P(r#UQD*7`93ciu&;L=;6$8*Um7!e2U$DY-go|?)mJLjUw6HX3R*IhpIfY!j+-jMgtkfmOk>eTA*u^G3R@ zCsS~d6CFM3jcvd%j#`?{At zsKq;Z#qU#3c5=Pdf7L%$j&M{_9c17`053#oAfhVm-dC&@d48ck0`AaBsnGk}Xbaq2 z{r~Zj9hNRgp$gx1cU4cNycb>#j+F}VLX&5=GAo;9AfkDL6}5gGxDXX${z!q_u5U)E z9lka2^Y#`FFXarJEKjr{AN)q_ob2f`NZ}R+oh}w<_v%q`FsMACAIh&$8z&TUKgiW~ zdh3!oXd~)J1&qR$4x(N0ULmr+8V)DIW~U77wy{Zn8g~01O~k@5){{E~q@U`1_qM$} zozPJYPMOK_Jh?m8+eL)0+{g(22Zs&4<2&|5Ij3Tooehnz-sN}8$-5uAN9v_h%FnEa zH^-emdxFLHSi0EtGO*V(`(2LrQslebriipIV&Izdy$wOTk%V@0cGmfdYa3l(JTNi7 zNrEBjOb*Agz}!s^NaE^+(I?T=OGzfyY90a)luO5_DB4Ff)kE)aBAtQbzGpWb)y4Oo zOI^%k!(})Ud^;b`@&r@%pMs{=h8`VEpdqFND1`L*zv7EI?!e2(%l0izUAiAdmtkQ- zJmYf+G=C9e`sL^5@26)X?txx6d^>m8sG9q5D`U{pXMi5X4i2#&OiR-5IdT)6{Idx^?q{7e^IP6)rGk3qi`y-_<}I1mVIa7eaG7>C=|lpofxYs}Oo#Uqtfq z(dfrGY=Ucc#K4`K%7=(c{zO(+Vn)V+Nd=^5vC(8Eb^-shQ9*~t{+(C^oahVG_PkvN zs2+VE)d}f8yv{?XSBkiOrXixESqq4r@nK|}f)pSpDx*SM{v_~xKFA}J6RF)BH?)g} zUDA<#8@2SAy3p>_wmu; z^yYTf_kSGl3@_$otF5uHPaSBD)jg)m0uIp#= zFu@Sn@ZuqSWx-ep1wm!vWXrYMT)Y|2zYVLNE1x~Hx|Th+9v!Qh&+4nrx{kBlq0gPX zj~(Pm=V+wlzH}*Cb)Kyr<7jzjxylShY!c`*x06ux9zt`J#4oXPI&i(d3v|LkGfc1u zy>Z&NVqZeC;qdEgBET|)MXBO%C(h0YtEC~9oMC()&p=hfkLm6h;nC^$oxbQ~?7JYm zXl`)GU7pWT2QqS%hBqoSnX|4lwQ6FC4KmJ3iAGDeSG&SL7f2BBng`T&6X^*Kdov?0 z4!Un2Dy=dKPbT&V;t<+imW3R1+Jmcc=1DyQy$OiHDE%Ap<`#!1d`1sGh@m@c=xy*Ux>8(PE zRUD<=uR5};GMBDz@zZ-2*4n(&Q@PHi;ha{&I6fx3zU;aCA4?<1*89qAXCl9{LR$~A zZE)SI>HlQr{yS6|IDgFHtv%+h_2RWke4e!8v_G&}rb@Kf~BKfXMY@ui(Wpuo81 zV_4I^J{Mtgr(&qKtpRTYjSaD(u|Mx`*$5XBQla`qm}u zOuZ{bS_lEux)joe(A+E41Ot2I6o}|nPb!vFoTO0|&NjB%IpnuVj9k2{K@r!4{Wz6O zG7igL{uy#N#j0%Y|Cr6n&bYDowXxluUdWDcwe5)#vNb&xeF^9K4Ho**-8Y|khOsX$ zol2bJ2xR&h%8p~g+JlVXZL@RUpvrsvjFI+Iw`qV+1Rm?YhM*6W#?yqk-GA8N*xh!PzF4bxTgroMmq00sD zmA>2B%9I(#Oz$oFVOZodRbOL3DvqXLy)qZ?F5ii9HBOxLX?T}&Rpve=fL<4-Osvf@ z+n36zHg^gFkZs^-UQ9CSH-_HXQ)?owzJ-KjZ8FVIVP?pG^C1xaVp~k}m=*Gwu-`tL zNEdUmb;lzt2=vU&`&)Xz6?^p{n9? zvz-V$-#yM8#z^^m(sy|4%0C70vaQlIt;3+}^^9J7VhlpvU)iOd+DG9>6Q6WqX;}qh z9oyl?X22KD+%u_kF%5z&3vDIZ8KP-vCTI@H@M{xV@c+mIL?7%a;3(^#hfI_9iTkTo zhPYk<_T2E*_053Zxpy#B}K5wA}J^=U^+S!O>ci!!B1Rz+|rF= z*rJ+(no&3KOY z(+%T5`!=h;X}zAc4waMJfp{nM=G->U{YSn#K`zU1^0~D=ZFj?gr0uG7F4dfZw=xPB zt+=oeJmn80H&w)l-_KNSe=fsUwGHO9EigTmvOa@nEZi~D&U?n*TdwLa^egJ-js7uXd(-r! zG*EGtj}8AyR-Bd9{vKS;{=@l9QWfhmONEwk;Cnv;jDjb4X;Jhp|B+IeCaErV%BLEGh4rNo1V8qX%b~McZ01^{p zjGm-Mm!H*M$dxhTXZs2M4tRmSw&XVe5nnysH=C7D zn4t9=9TSG<%39M%h!`Z=fPHC|?yjDhorf_Md~kY|H-JR?wQ^!6p3Gps$wNw6o*9-m zl?9qZBjp~2T9LbK!q2PbV7;6j%xYPSaxT&+H)n$T{vd{G*vdNAvTfK$GponXIs|0+ zyzr&7Kx3Aqt&C@I5qLH~J`k`&GgpT7$cuFvcnWgf@#jXvHqzD0*FFQ((g+DiIHP@M z%++@Ag~dv<{dHhh4~VIVDExNUW7Td(bt z^|8B?U|o-;Ljxskd=P$T!@1;TRRpSYz6t;McjJ0}?2!F_e#P^>2mK)}QbLyi6aV6O z)7^b8Q%r<~>gidR2Q^bu8fZxa=Ruiz{|k;AeVN!uT!dHLKDlG zP9FK3)P@8$Mxy6@6`pk0Abx6G8;{{kx~IE^@j4i?WRkg{m919fqk`CI8d>f8InDVm zFsPX4DqW;1%8S_z)dYUiyL8)1bG6MI(3sBbDYrTs(k!Y*@N{R@2K2Ke58Y2!8Tjw&!;4lf< zU(2pOlVBPyWn4!1B#OfC-=t}(SO*)r>2BIE8MSLCM8JCMM?d#$JTO9mPgCUG7l1*#9onU}H<0lets^h<7kOD~XO zFtG@dSo;1x&sRIUhkR*(c;po&gHT_-qjT0o;W#_Chr>kHv%0B%HjTgp4%fqT@-Yvc z`UM#*Vls_Da?w#nGLCJMBBeW;mSL2<($D;13cJy^z|yBgv!7aAqncGcHIHhh77erGag>Ws`V;K>Tv;yneRBf;7w#LHiMYWu@7u^EWJeYJZ9UxSq z+K#G?%+6v0A~5Q(r*~8{e}OxJwP6_o^>ZZ$v8r*f1u?*-lVZGA$X->_R!#rA?B)In zwU~6(^R!E)z)*e2ONl5t542xz&iT^^7gu%cpJ|HFjW! z73FbrPaeMkf1)2|+eX!9!*qQ=uUakZJPxZ9n!qx*ws|y=sVh67>*Xwo$X0l_StRss zT#^$Gv`NuKIV}EXlXXvn#d+qx`umx<7zb-^q1KITbQ}KF0&#ZZ=OXO1VSez4EB4NS z%(?q3Jmj<$CL8x`e$T?Uta&0aTk^rayLE?-&EFB}vTRimMuhdhccVRBq z?PVX7jRcq|#f2lW_v6T{v`Rb-(HAN^ngjGee~q3@iq-XK-2+e{tdnC z-l`LqV6-3GAB8A{1p2O2IMimV-@mp(F~bxaU|FG(c5a3d@BMI)tAPZRfQA*?LWbeq z&g7JO$Y^Gc6;`cndoKM;$G`-?eICsgLrAOl#x+EHBXaIuzy!If$F=CUhv8%#hwR$z z6go#$_vy(%S{cdmMHQV9AD;fyG9MPPbB;O{ZB`idm+iXfLE3yPwqUfCF3u~t*ZBj4 zWQ6M&#v0LNfABe0bhcLC??}_BF|B`al{_Y|0Sr~9gbJx*+O(hVD)nWG1GNSAS(ohE zB%g<1*Wmx}UEfTr9!b*tjt&u_Ou$BJ9U0r zp@EyyXrnX*!_b*}NVEZ5R0uzY8h|Z2EqU(7iPRG$q>|G)<-)|}dtAlXHaPGbC>eVa zVW}$L{YJ}KPs2Pn&nh~L@EZ>skpZcN3vZQD6Z_tS{pl*UKTudcz2H#=0+m2Qb9@#l8FP&P!HjbC{hIlWtx2 zpfz?yT!tp0p4fRoOg*;gaSrAoOftPyfI-}H05eebx!!U-s0~Z9=>?U=) zk3ulj{Y#X`k%j-?ABg^;`B%Pn`yXsl9h$=OC~TO`;TenP@0Jn-9b&VC1c@t&LJ|oa zRX9z|>3515nc*vmvPm1+WSFfJu_lwrpVY(ey+3!(xUG2HXsW!{I<0tk=9b>_);=G1 z_x5)8Z2o-tn7tvNC?D|%fsRmuZ%lX-9W_19J{iL%vcs&>L~F50W_C6ySM>Y{v&wC= zp-Vd<5(Qk6ihs7G%x;~`^AozfysY71K7}NLWZZ!FYfF;%2Z;jVM{1Aa)*%9bRv?C~ zE0ID0Dv<&VE0IFXE0KW2Dv`oCl*wR&l-YpVmB|1Yl*u5l%4H!>6?2fG6>^yG%Vj}h z6>_MxampKA%a#HT+N<-a{b2`2DkL{yoXs<7JnP?+OG4!E|b8EecBC;3Tn7M&dm&9v^@!`_p&pu!e;^q z(ux3%lgq|_$IF!dkfEgkE4w#`HUn1X8$(@<$57~_^Ana3lC}Q6HC~dxhaPUNg#Sa@ zPzK`tQ4O6Kp*~a#EI@!BZfl(X`QpDheu}3d@UfCyxWAZOA2gt@(iM`un_sNnf0X5| zZfhdV3I`TCESAV$o9-8 zUkix9W(VCnfBOTr*-u2x)ynp{Mf3D~fH3xz)X!okwPa<*g?<=Vmj^{WrmLk%ksgbd|5Cqw~+&9nl>jZiK&tXN97gKJ&)`}dm8V)%CSz&PYN*E0e@CemHHMMzkJ zyHc)5_UDT?ZKMA_1x+Z$Ifp-<yRY0l#)&}4H> z6)XAxRKrCsoT}!M>Xf%lG;MIC130bGx~mqMN~@aD2=-Klf@;rK;@Y~3D=7Vrayp#v z@r_m~F&{9{X%l!x+mIo=^Z~Y?Zmn(*LF?SpLW{MG-DCQ}!SO*aSPD1gaG6fMCDS+R zYbk`F^kA3s1k)@Q2PZA!stbWmE2uP`$ssZqnW-%800Iv7GV6HU@h@C4>d!Seg5dtPx zi`}vj&hu#35H2*xyUp4bbcvl?^NH(K;NuPZ`CkF#3mfzSk8^WS(Hvd1&Oyc>9d0fZ zyntrz<%~Q>H%J`rqJ1PlBDJGQ8!ghasosR{EXyl9hBB916&9g(L*>`H(6}p>=aq&D zcQ>*C2ZlKkFLG@$Z8836|iPS99pjZ zNVfzXi}6SD`pMt-NkYS%*>+rXQD9w5H~K2}>~5t` z1RTBMDrkWkK2nd{i?+ULQ@eo1eZG9X5(UD05%{XgJZ$W>)IhiUuj_0$g5^Ym7w>yICg>DkF_%R8UQkQIcfEJqYcMYMxI>b(L zKI7-e&ewejdgk|B|IMhV)WB~i(cRm1Pjk_R`+hmS=-o)s%R&?Qb04pmqK|Gqw@@Y#mw~QWq77($)tbJcWjt9$Hv}j%yyP?nFI}Rql_Y*J_RCO`$TyGpt_W zK2;peIdKv{SN%q+qiU~l|M{B_9WNzdy=8#zOS}#wWH||b3d8;GlN%nk!}Gk-X{kze zJU=fOFpubBg9}ILJmVT0!p9kEVP0$yDrzjf$6>>}&HIkDRk>(2HuAT4}qw_zj$mQ(99pqlFBVHefOgE-a6!Fj5a z0~4x~gO^lC2Vbhg>oosK-m)fHSS>Ks<}EJ^!TL)S)68Yy$~Mn+FM_0@DLLscqF1Cq zp6VpQoa!VYi0ULDhw5fPBh`t69My@zDykEMK&lgkY^oE12C5SVI@Jk*8Py3v0M!XX z7S#zrJ=M`b_3SCnJ;hUGgy7_py4jOt=XQ@J5dE3_sp4@`#=sbP!$5aR%JCDaLUkkL zL8==j+o|pYX+w2`WC7I;kOoHDPu_+0k+fI-2VU*#Cd+@)Q~Q|JL7qJ8m2D{Tp8Vym z(#+H|t)xP}S@7abEo9yPw&GUXW>UwPR)46cp3I&-Pdesw9m%;x%C%&^Bx65XwjWlg zCMWC7;{v;$liPOb$=>8tl1X-RijVp}BgJF26BoUCOa}3nc~{z&k?oSJ%=}9qkdBr% zN(y@S$gwd+D<`U|9SCk=SKy2;tu*X^fXZjSEt==k`{w@@6X*iE@Dt!pC@|qHEUWmLra$5 zm!?B_>>HIT&l+xjR))NnClhqs8bjk=u^@(cIk?=34b@rwD*F}K&_D51bY{^cd6ZN? zq9(_Nui3mjYER)uK>+0 z*jdx@`)!g|rd*_-nM?lWKTw+T!yu{S&Pq)>$c8_yyqA_;FChx|3<5^N_!_*Ek7v!9 z>55-B5nSDUM;itD=`{K<&BuPPG$kxUFc+ zvAw1a{l}Ng^uc}Lb?aZqT_z6nyFaTIz7vPE#w{%`Vnm_VD)iE*J$qWy6SoUqj?@6o z($=-v2JHWHZnYV@aNvLjaY=TffRy21jt!Jl~* zG$Ek*L>;afl7XM{w*Ij3E$+V&KX%^JnSLPK(`OKR?9!e`9%#eY&&e;#3^&WI94N3-_sLmE!Z5 z?QT69IY!b33rqG|jgvGDztT!_oOFAy7Va|--=}XxTKnJCez5Fq#YzoG+wZa0-iI}< z7e+h;FVi%j`2^2>PB8Od7XD#=RfM;-hR^`mZ`l;)fcwas7m{drgiCvDw@AX70pUIg zCbxIU0vxMwJR&E>`%~KWlq$8VVJ@vV>~g`%D(?UEb>#c_KE99d<69YJPft~zu}}r| z+r^AHTU8Lw%t%<-ss{Q}4XqwsYLIX-^5PU)?7C;3jw>;(N+Vh`(liO^Es9@4Msa`{ z_F$;0O94y{CFE2l6Ts{IW6ZFc4WlPiQn@-f|8)B8gr_PP5f8;LEF!*YP`YHgSF8l| z3Z*dTs;B`cahdjkNO3Tk|Fe;EI|qEDBi#mq6ydVXZ0`w4C9sIp{*>OT1hmG>l`~i0 zBo7)AGBJ&K-HJIK99qo&FX7OhaNT!!|GGPS4|~ZlDSmw{J~8 zV-VM^Sun6}DFF&%devzM*k84AR;X10O_Ue3YvdQ{JT|=I>U9UjN%6KDZ#(|El@0U{ zf)36GB9OLPmdVW$2Az{)N$dU)h5q?6E)E^+X`TCFVz^3C034t5M$=~yp!1)GPvbv+ z|7-o%)|qa->HKHc`BQ$}wr=Aq0ua82$1>IkOxk3(H2{R_~sn;39yt)76DpEdPPzx zF|Emq(=%HJ#i2Pq?#SpVg@4)bg#Yf+fQ!%B&&7b|z|!X8`HFMV_1VPKETGY?&k3BD zgz&BQZ9i=uA=85NH*rc;;7@6ze@(lw86US=f;Ytxk?l_^r#N(xs{Ay!KZ zLJL_*YIwmy(7fy{Er#Z#vJ}%~mYSM*AvMX;&iQu!B!q!V5uQH}?|aV9Ba-K-4oNPS^Y}p+KfX7~3KLoND)(bJQr1ijQc3>A= z!J{O~_n;!To1=%L1c~n;Ba^gNs3hGrZ=frPOz;xcv-37U7TaGpZ)60zL;RgmV7O+> zr>Z5uDEh=}yd!bmqrfx}Q`2m|KL%5^M6uFRIy)7|Cr!*THRqO&PT~1v@oAfY`OVd85`c{V^ zM$ZTmQt`W^tV?w7EQ+(o_h7VjnHA2<@)|NM$h$Od_R^BNM+&@C31h7!(WJiFQK|#v z-VUdfhm(68Y*&l^K?;TzHkPf(%O>ZSX`7ry3qTcLY!Xbqd!K7T0r>sO`YP_OBlUmE zduSJdW4WVcKK@?0!UQXS^4%5=x1N#weaeb{iC^cJRWw)m;B(Y^9*HCGsK&qmr`Nea$-B)FB5O69O6 Date: Fri, 28 Mar 2025 11:20:15 +0800 Subject: [PATCH 011/146] =?UTF-8?q?=E4=B8=8A=E5=82=B3=E6=AA=94=E6=A1=88?= =?UTF-8?q?=E5=88=B0=E3=80=8Csrc/unitdev04=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit XBee --- src/unitdev04/change_mode.py | 109 +++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 src/unitdev04/change_mode.py diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py new file mode 100644 index 0000000..e81f195 --- /dev/null +++ b/src/unitdev04/change_mode.py @@ -0,0 +1,109 @@ + import curses +import serial +import struct +from pymavlink.dialects.v20 import ardupilotmega as mavlink2 + +PORT = "COM5" # or "/dev/ttyUSB0" +BAUD = 57600 +target_system = 5 +target_component = 1 +MAV_CMD_DO_SET_MODE = 176 + +mode_list = [ + ("STABILIZE", 0), + ("AUTO", 3), + ("GUIDED", 4), + ("LOITER", 5) +] + +ser = serial.Serial(PORT, BAUD) + +class PacketCapture: + def __init__(self): + self.data = bytearray() + def write(self, b): + self.data.extend(b) + return len(b) + +def build_api_tx_frame(data: bytes, frame_id=0x01): + frame_type = 0x10 + dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 + 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 curses_main(stdscr): + curses.curs_set(0) + selected = 0 + + while True: + stdscr.clear() + h, w = stdscr.getmaxyx() + + stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") + + for i, (name, _) in enumerate(mode_list): + if i == selected: + stdscr.attron(curses.A_REVERSE) + stdscr.addstr(i + 3, 4, f"> {name}") + stdscr.attroff(curses.A_REVERSE) + else: + stdscr.addstr(i + 3, 4, f" {name}") + + key = stdscr.getch() + + if key == curses.KEY_UP: + selected = (selected - 1) % len(mode_list) + elif key == curses.KEY_DOWN: + selected = (selected + 1) % len(mode_list) + elif key in [10, 13]: # Enter + name, custom_mode = mode_list[selected] + capture = PacketCapture() + mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) + mav.version = 2 + + msg = mav.command_long_encode( + target_system=target_system, + target_component=target_component, + command=MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=1, + param2=custom_mode, + param3=0, param4=0, param5=0, param6=0, param7=0 + ) + print("🧪 msg =", msg) # 確認封包物件生成 + + mav.send(msg) # ✅ 改為 send() 會寫入 capture + print("📦 RAW HEX:", capture.data.hex()) + + api_frame = build_api_tx_frame(capture.data) + ser.write(api_frame) + + # 顯示封包資訊 + msg_line = min(h - 4, len(mode_list) + 5) + stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") + stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") + stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") + stdscr.refresh() + curses.napms(1500) + + elif key in [ord('q'), ord('Q')]: + break + + stdscr.refresh() + + +try: + curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser + +except Exception as e: + print(f"❌ 發生錯誤: {e}") +finally: + ser.close() + print("👋 程式結束，串口已關閉") From 21dce163edb7d3fa8f8a67b75866420512b672f2 Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Fri, 28 Mar 2025 11:22:57 +0800 Subject: [PATCH 012/146] =?UTF-8?q?=E5=88=AA=E9=99=A4=E3=80=8Csrc/unitdev0?= =?UTF-8?q?4/change=5Fmode.py=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/change_mode.py | 109 ----------------------------------- 1 file changed, 109 deletions(-) delete mode 100644 src/unitdev04/change_mode.py diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py deleted file mode 100644 index e81f195..0000000 --- a/src/unitdev04/change_mode.py +++ /dev/null @@ -1,109 +0,0 @@ - import curses -import serial -import struct -from pymavlink.dialects.v20 import ardupilotmega as mavlink2 - -PORT = "COM5" # or "/dev/ttyUSB0" -BAUD = 57600 -target_system = 5 -target_component = 1 -MAV_CMD_DO_SET_MODE = 176 - -mode_list = [ - ("STABILIZE", 0), - ("AUTO", 3), - ("GUIDED", 4), - ("LOITER", 5) -] - -ser = serial.Serial(PORT, BAUD) - -class PacketCapture: - def __init__(self): - self.data = bytearray() - def write(self, b): - self.data.extend(b) - return len(b) - -def build_api_tx_frame(data: bytes, frame_id=0x01): - frame_type = 0x10 - dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 - 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 curses_main(stdscr): - curses.curs_set(0) - selected = 0 - - while True: - stdscr.clear() - h, w = stdscr.getmaxyx() - - stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") - - for i, (name, _) in enumerate(mode_list): - if i == selected: - stdscr.attron(curses.A_REVERSE) - stdscr.addstr(i + 3, 4, f"> {name}") - stdscr.attroff(curses.A_REVERSE) - else: - stdscr.addstr(i + 3, 4, f" {name}") - - key = stdscr.getch() - - if key == curses.KEY_UP: - selected = (selected - 1) % len(mode_list) - elif key == curses.KEY_DOWN: - selected = (selected + 1) % len(mode_list) - elif key in [10, 13]: # Enter - name, custom_mode = mode_list[selected] - capture = PacketCapture() - mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) - mav.version = 2 - - msg = mav.command_long_encode( - target_system=target_system, - target_component=target_component, - command=MAV_CMD_DO_SET_MODE, - confirmation=0, - param1=1, - param2=custom_mode, - param3=0, param4=0, param5=0, param6=0, param7=0 - ) - print("🧪 msg =", msg) # 確認封包物件生成 - - mav.send(msg) # ✅ 改為 send() 會寫入 capture - print("📦 RAW HEX:", capture.data.hex()) - - api_frame = build_api_tx_frame(capture.data) - ser.write(api_frame) - - # 顯示封包資訊 - msg_line = min(h - 4, len(mode_list) + 5) - stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") - stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") - stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") - stdscr.refresh() - curses.napms(1500) - - elif key in [ord('q'), ord('Q')]: - break - - stdscr.refresh() - - -try: - curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser - -except Exception as e: - print(f"❌ 發生錯誤: {e}") -finally: - ser.close() - print("👋 程式結束，串口已關閉") From 716b3726c237e826c38de4b806d96a2211d9d73a Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Fri, 28 Mar 2025 11:23:42 +0800 Subject: [PATCH 013/146] Mavlink and XBee packet transfer --- src/unitdev04/change_mode.py | 109 +++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) create mode 100644 src/unitdev04/change_mode.py diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py new file mode 100644 index 0000000..e81f195 --- /dev/null +++ b/src/unitdev04/change_mode.py @@ -0,0 +1,109 @@ + import curses +import serial +import struct +from pymavlink.dialects.v20 import ardupilotmega as mavlink2 + +PORT = "COM5" # or "/dev/ttyUSB0" +BAUD = 57600 +target_system = 5 +target_component = 1 +MAV_CMD_DO_SET_MODE = 176 + +mode_list = [ + ("STABILIZE", 0), + ("AUTO", 3), + ("GUIDED", 4), + ("LOITER", 5) +] + +ser = serial.Serial(PORT, BAUD) + +class PacketCapture: + def __init__(self): + self.data = bytearray() + def write(self, b): + self.data.extend(b) + return len(b) + +def build_api_tx_frame(data: bytes, frame_id=0x01): + frame_type = 0x10 + dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 + 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 curses_main(stdscr): + curses.curs_set(0) + selected = 0 + + while True: + stdscr.clear() + h, w = stdscr.getmaxyx() + + stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") + + for i, (name, _) in enumerate(mode_list): + if i == selected: + stdscr.attron(curses.A_REVERSE) + stdscr.addstr(i + 3, 4, f"> {name}") + stdscr.attroff(curses.A_REVERSE) + else: + stdscr.addstr(i + 3, 4, f" {name}") + + key = stdscr.getch() + + if key == curses.KEY_UP: + selected = (selected - 1) % len(mode_list) + elif key == curses.KEY_DOWN: + selected = (selected + 1) % len(mode_list) + elif key in [10, 13]: # Enter + name, custom_mode = mode_list[selected] + capture = PacketCapture() + mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) + mav.version = 2 + + msg = mav.command_long_encode( + target_system=target_system, + target_component=target_component, + command=MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=1, + param2=custom_mode, + param3=0, param4=0, param5=0, param6=0, param7=0 + ) + print("🧪 msg =", msg) # 確認封包物件生成 + + mav.send(msg) # ✅ 改為 send() 會寫入 capture + print("📦 RAW HEX:", capture.data.hex()) + + api_frame = build_api_tx_frame(capture.data) + ser.write(api_frame) + + # 顯示封包資訊 + msg_line = min(h - 4, len(mode_list) + 5) + stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") + stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") + stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") + stdscr.refresh() + curses.napms(1500) + + elif key in [ord('q'), ord('Q')]: + break + + stdscr.refresh() + + +try: + curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser + +except Exception as e: + print(f"❌ 發生錯誤: {e}") +finally: + ser.close() + print("👋 程式結束，串口已關閉") From df2a4590706becce7c3670e30a8f68e48f2118f1 Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Fri, 28 Mar 2025 11:24:02 +0800 Subject: [PATCH 014/146] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E3=80=8Csrc/unitdev0?= =?UTF-8?q?4/change=5Fmode.py=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/change_mode.py | 218 +++++++++++++++++------------------ 1 file changed, 109 insertions(+), 109 deletions(-) diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py index e81f195..58d0023 100644 --- a/src/unitdev04/change_mode.py +++ b/src/unitdev04/change_mode.py @@ -1,109 +1,109 @@ - import curses -import serial -import struct -from pymavlink.dialects.v20 import ardupilotmega as mavlink2 - -PORT = "COM5" # or "/dev/ttyUSB0" -BAUD = 57600 -target_system = 5 -target_component = 1 -MAV_CMD_DO_SET_MODE = 176 - -mode_list = [ - ("STABILIZE", 0), - ("AUTO", 3), - ("GUIDED", 4), - ("LOITER", 5) -] - -ser = serial.Serial(PORT, BAUD) - -class PacketCapture: - def __init__(self): - self.data = bytearray() - def write(self, b): - self.data.extend(b) - return len(b) - -def build_api_tx_frame(data: bytes, frame_id=0x01): - frame_type = 0x10 - dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 - 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 curses_main(stdscr): - curses.curs_set(0) - selected = 0 - - while True: - stdscr.clear() - h, w = stdscr.getmaxyx() - - stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") - - for i, (name, _) in enumerate(mode_list): - if i == selected: - stdscr.attron(curses.A_REVERSE) - stdscr.addstr(i + 3, 4, f"> {name}") - stdscr.attroff(curses.A_REVERSE) - else: - stdscr.addstr(i + 3, 4, f" {name}") - - key = stdscr.getch() - - if key == curses.KEY_UP: - selected = (selected - 1) % len(mode_list) - elif key == curses.KEY_DOWN: - selected = (selected + 1) % len(mode_list) - elif key in [10, 13]: # Enter - name, custom_mode = mode_list[selected] - capture = PacketCapture() - mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) - mav.version = 2 - - msg = mav.command_long_encode( - target_system=target_system, - target_component=target_component, - command=MAV_CMD_DO_SET_MODE, - confirmation=0, - param1=1, - param2=custom_mode, - param3=0, param4=0, param5=0, param6=0, param7=0 - ) - print("🧪 msg =", msg) # 確認封包物件生成 - - mav.send(msg) # ✅ 改為 send() 會寫入 capture - print("📦 RAW HEX:", capture.data.hex()) - - api_frame = build_api_tx_frame(capture.data) - ser.write(api_frame) - - # 顯示封包資訊 - msg_line = min(h - 4, len(mode_list) + 5) - stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") - stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") - stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") - stdscr.refresh() - curses.napms(1500) - - elif key in [ord('q'), ord('Q')]: - break - - stdscr.refresh() - - -try: - curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser - -except Exception as e: - print(f"❌ 發生錯誤: {e}") -finally: - ser.close() - print("👋 程式結束，串口已關閉") +import curses +import serial +import struct +from pymavlink.dialects.v20 import ardupilotmega as mavlink2 + +PORT = "COM5" # or "/dev/ttyUSB0" +BAUD = 57600 +target_system = 5 +target_component = 1 +MAV_CMD_DO_SET_MODE = 176 + +mode_list = [ + ("STABILIZE", 0), + ("AUTO", 3), + ("GUIDED", 4), + ("LOITER", 5) +] + +ser = serial.Serial(PORT, BAUD) + +class PacketCapture: + def __init__(self): + self.data = bytearray() + def write(self, b): + self.data.extend(b) + return len(b) + +def build_api_tx_frame(data: bytes, frame_id=0x01): + frame_type = 0x10 + dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 + 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 curses_main(stdscr): + curses.curs_set(0) + selected = 0 + + while True: + stdscr.clear() + h, w = stdscr.getmaxyx() + + stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") + + for i, (name, _) in enumerate(mode_list): + if i == selected: + stdscr.attron(curses.A_REVERSE) + stdscr.addstr(i + 3, 4, f"> {name}") + stdscr.attroff(curses.A_REVERSE) + else: + stdscr.addstr(i + 3, 4, f" {name}") + + key = stdscr.getch() + + if key == curses.KEY_UP: + selected = (selected - 1) % len(mode_list) + elif key == curses.KEY_DOWN: + selected = (selected + 1) % len(mode_list) + elif key in [10, 13]: # Enter + name, custom_mode = mode_list[selected] + capture = PacketCapture() + mav = mavlink2.MAVLink(capture, srcSystem=1, srcComponent=1) + mav.version = 2 + + msg = mav.command_long_encode( + target_system=target_system, + target_component=target_component, + command=MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=1, + param2=custom_mode, + param3=0, param4=0, param5=0, param6=0, param7=0 + ) + print("🧪 msg =", msg) # 確認封包物件生成 + + mav.send(msg) # ✅ 改為 send() 會寫入 capture + print("📦 RAW HEX:", capture.data.hex()) + + api_frame = build_api_tx_frame(capture.data) + ser.write(api_frame) + + # 顯示封包資訊 + msg_line = min(h - 4, len(mode_list) + 5) + stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") + stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") + stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") + stdscr.refresh() + curses.napms(1500) + + elif key in [ord('q'), ord('Q')]: + break + + stdscr.refresh() + + +try: + curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser + +except Exception as e: + print(f"❌ 發生錯誤: {e}") +finally: + ser.close() + print("👋 程式結束，串口已關閉") From 30f29f678b7178cad9c9fc1751c3eaf75ec87bb9 Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:24:36 +0800 Subject: [PATCH 015/146] =?UTF-8?q?=E6=B8=AC=E8=A9=A6=E6=8E=A5=E6=94=B6?= =?UTF-8?q?=E7=AB=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/0328/testserver.py | 55 ++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 src/unitdev03/0328/testserver.py diff --git a/src/unitdev03/0328/testserver.py b/src/unitdev03/0328/testserver.py new file mode 100644 index 0000000..cce1478 --- /dev/null +++ b/src/unitdev03/0328/testserver.py @@ -0,0 +1,55 @@ +import socket +import re + +def start_udp_server(host='0.0.0.0', port=5000, buffer_size=1024): + server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_socket.bind((host, port)) + + print(f"UDP Server running on {host}:{port}") + print("Waiting for data...") + + position = None + velocity = None + + try: + while True: + data, client_address = server_socket.recvfrom(buffer_size) + + print(f"\n----- 接收到的資料 -----") + print(f"從 {client_address} 接收:") + + try: + decoded_data = data.decode('utf-8') + print(f"文本數據: {decoded_data}") + + # 直接使用正則表達式提取數據 + pos_match = re.search(r'"position":\[([-\d\.\,]+)\]', decoded_data) + vel_match = re.search(r'"velocity":\[([-\d\.\,]+)\]', decoded_data) + + if pos_match: + position_str = pos_match.group(1) + position = [float(x) for x in position_str.split(',')] + print(f"Position: {position}") + + if vel_match: + velocity_str = vel_match.group(1) + velocity = [float(x) for x in velocity_str.split(',')] + print(f"Velocity: {velocity}") + + # 這裡可以對 position 和 velocity 進行進一步操作 + + except UnicodeDecodeError: + print(f"二進制數據: {data.hex()}") + except Exception as e: + print(f"處理數據時出錯: {e}") + + print(f"數據大小: {len(data)} 字節") + print("-----------------------") + + except KeyboardInterrupt: + print("Server shutting down...") + finally: + server_socket.close() + +if __name__ == "__main__": + start_udp_server(port=5000) \ No newline at end of file From 6c1441d0c614a600e9bab396c8f373f5bb2a05ea Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Fri, 28 Mar 2025 11:25:16 +0800 Subject: [PATCH 016/146] Mavlink and XBee packet transfer --- src/unitdev04/change_mode.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py index 58d0023..4a71d5f 100644 --- a/src/unitdev04/change_mode.py +++ b/src/unitdev04/change_mode.py @@ -3,7 +3,7 @@ import serial import struct from pymavlink.dialects.v20 import ardupilotmega as mavlink2 -PORT = "COM5" # or "/dev/ttyUSB0" +PORT = "COM5" # or "/dev/ttyUSB0" BAUD = 57600 target_system = 5 target_component = 1 From f73e067c0bb5a12848571dbe44a975b0fa8bdc58 Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:26:20 +0800 Subject: [PATCH 017/146] =?UTF-8?q?=E4=B9=8B=E5=89=8D=E7=B0=A1=E6=98=93?= =?UTF-8?q?=E6=A8=A1=E6=93=AC=E7=9A=84=E5=B0=8F=E7=A8=8B=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/0328/pymavlink_02.py | 497 +++++++++++++++++++++++++++++ 1 file changed, 497 insertions(+) create mode 100644 src/unitdev03/0328/pymavlink_02.py diff --git a/src/unitdev03/0328/pymavlink_02.py b/src/unitdev03/0328/pymavlink_02.py new file mode 100644 index 0000000..5dba35b --- /dev/null +++ b/src/unitdev03/0328/pymavlink_02.py @@ -0,0 +1,497 @@ +#!/usr/bin/env python3 +from pymavlink import mavutil +import time +import math +import threading +import sys + +class DroneController: + def __init__(self, connection_string="udp:127.0.0.1:14550", drone_id=0): + self.master = mavutil.mavlink_connection(connection_string) + self.drone_id = drone_id + self.connection_string = connection_string + self.master.wait_heartbeat() + print(f"無人機 #{self.drone_id} 已建立連接!") + + def set_mode(self, mode): + if mode == 'STABILIZE': + mode_id = 0 + elif mode == 'AUTO': + mode_id = 3 + elif mode == 'GUIDED': + mode_id = 4 + elif mode == 'LOITER': + mode_id = 5 + elif mode == 'RTL': + mode_id = 6 + elif mode == 'LAND': + mode_id = 9 + else: + print(f"不支援的模式: {mode}") + return False + + self.master.mav.set_mode_send( + self.master.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + mode_id + ) + + for _ in range(5): + msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1) + if msg and msg.custom_mode == mode_id: + return True + return False + + def arm(self, arm_state=True): + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 1 if arm_state else 0, 0, 0, 0, 0, 0, 0 + ) + + for _ in range(5): + msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=1) + if msg: + armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + if (arm_state and armed) or (not arm_state and not armed): + return True + return False + + def takeoff(self, altitude): + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, + 0, 0, 0, 0, 0, 0, + altitude + ) + + timeout = time.time() + 30 + while time.time() < timeout: + msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if msg: + current_alt = msg.relative_alt / 1000.0 + if abs(current_alt - altitude) < 0.5: + return True + time.sleep(0.1) + return False + + def goto_position_local(self, x, y, z, vx=0, vy=0, vz=0, yaw=0, yaw_rate=0): + z = -z + + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE + ) + + print(f"無人機 #{self.drone_id} 發送位置指令: x={x}, y={y}, z={-z}, vx={vx}, vy={vy}, vz={vz}") + + self.master.mav.set_position_target_local_ned_send( + 0, + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + x, y, z, + vx, vy, vz, + 0, 0, 0, + yaw, yaw_rate + ) + + print(f"無人機 #{self.drone_id} 開始監控位置變化...") + start_time = time.time() + last_progress_time = start_time + min_dist = float('inf') + + while time.time() - start_time < 30: + msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1) + if msg: + dist = math.sqrt((msg.x - x)**2 + (msg.y - y)**2 + (msg.z - z)**2) + min_dist = min(min_dist, dist) + + current_time = time.time() + if current_time - last_progress_time >= 5: + if dist > min_dist + 0.1: + print(f"無人機 #{self.drone_id} 偏離目標，重新調整") + self.master.mav.set_position_target_local_ned_send( + 0, self.master.target_system, self.master.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + x, y, z, vx, vy, vz, 0, 0, 0, yaw, yaw_rate + ) + last_progress_time = current_time + + print(f"無人機 #{self.drone_id} 當前位置: x={msg.x:.2f}, y={msg.y:.2f}, z={-msg.z:.2f}, 距離目標: {dist:.2f}m") + + if dist < 0.5: + print(f"無人機 #{self.drone_id} 到達目標位置") + return True + + vel_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=False) + if vel_msg: + vel_total = math.sqrt(vel_msg.vx**2 + vel_msg.vy**2 + vel_msg.vz**2) + if dist < 1.0 and vel_total < 0.1: + print(f"無人機 #{self.drone_id} 已穩定懸停在目標位置附近") + return True + + time.sleep(0.1) + + print(f"無人機 #{self.drone_id} 移動超時") + return False + + def get_current_state(self): + state = {} + + # Mode and armed status + hb_msg = self.master.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5) + if hb_msg: + mode_id = hb_msg.custom_mode + mode_name = { + 0: "STABILIZE", + 3: "AUTO", + 4: "GUIDED", + 5: "LOITER", + 6: "RTL", + 9: "LAND" + }.get(mode_id, f"Unknown({mode_id})") + + state['mode'] = mode_name + state['armed'] = bool(hb_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) + + # Position + pos_msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1.0) + if pos_msg: + state['position'] = { + 'x': round(pos_msg.x, 2), + 'y': round(pos_msg.y, 2), + 'z': round(-pos_msg.z, 2) + } + + # GPS + gps_msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1.0) + if gps_msg: + state['gps_position'] = { + 'lat': gps_msg.lat / 1e7, + 'lon': gps_msg.lon / 1e7, + 'alt': gps_msg.alt / 1000, + 'relative_alt': gps_msg.relative_alt / 1000 + } + + return state + +class MultiDroneController: + def __init__(self, num_drones=5): + self.drones = [] + self.num_drones = num_drones + + for i in range(num_drones): + port = 14550 + i + connection_string = f"udp:127.0.0.1:{port}" + drone = DroneController(connection_string, i) + self.drones.append(drone) + print(f"已初始化無人機 #{i} 在端口 {port}") + + def get_drone(self, drone_id): + if 0 <= drone_id < len(self.drones): + return self.drones[drone_id] + return None + + def all_drones_command(self, command, *args): + results = [] + threads = [] + + for drone in self.drones: + thread = threading.Thread( + target=lambda d, cmd, args, results: results.append((d.drone_id, getattr(d, cmd)(*args))), + args=(drone, command, args, results) + ) + threads.append(thread) + thread.start() + + for thread in threads: + thread.join() + + for drone_id, success in results: + print(f"無人機 #{drone_id} 執行 {command} 結果: {'成功' if success else '失敗'}") + + return all(success for _, success in results) + +def print_menu(): + menu = [ + "\n=== 多機無人機控制界面 ===", + "1. 選擇操作的無人機", + "2. 對所有無人機執行命令", + "3. 查看所有無人機狀態", + "4. 退出", + "0. 顯示此清單", + "==========================" + ] + for item in menu: + print(item) + +def print_drone_menu(): + menu = [ + "\n=== 單機無人機控制界面 ===", + "1. 切換至 GUIDED 模式", + "2. 切換至 LAND 模式", + "3. 解鎖", + "4. 上鎖", + "5. 起飛", + "6. 飛到指定位置", + "7. 切換至 RTL 模式(返航)", + "8. 查看當前狀態", + "9. 返回主菜單", + "0. 顯示此清單", + "==========================" + ] + for item in menu: + print(item) + +def print_all_drones_menu(): + menu = [ + "\n=== 所有無人機控制界面 ===", + "1. 全部切換至 GUIDED 模式", + "2. 全部切換至 LAND 模式", + "3. 全部解鎖", + "4. 全部上鎖", + "5. 全部起飛", + "6. 全部返航", + "7. 返回主菜單", + "0. 顯示此清單", + "==========================" + ] + for item in menu: + print(item) + +def control_single_drone(drone): + print_drone_menu() + current_drone_id = drone.drone_id + + while True: + try: + choice = input(f"\n無人機 #{current_drone_id} - 請輸入指令編號: ") + + if choice == '0': + print_drone_menu() + + elif choice == '1': + if drone.set_mode("GUIDED"): + print(f"無人機 #{current_drone_id} 已切換至 GUIDED 模式") + else: + print(f"無人機 #{current_drone_id} 切換模式失敗") + + elif choice == '2': + if drone.set_mode("LAND"): + print(f"無人機 #{current_drone_id} 已切換至 LAND 模式") + else: + print(f"無人機 #{current_drone_id} 切換模式失敗") + + elif choice == '3': + if drone.arm(True): + print(f"無人機 #{current_drone_id} 已解鎖") + else: + print(f"無人機 #{current_drone_id} 解鎖失敗") + + elif choice == '4': + if drone.arm(False): + print(f"無人機 #{current_drone_id} 已上鎖") + else: + print(f"無人機 #{current_drone_id} 上鎖失敗") + + elif choice == '5': + altitude = float(input("請輸入起飛高度(公尺): ")) + if drone.takeoff(altitude): + print(f"無人機 #{current_drone_id} 已到達目標高度: {altitude}公尺") + else: + print(f"無人機 #{current_drone_id} 起飛失敗") + + elif choice == '6': + x = float(input("請輸入X座標(公尺): ")) + y = float(input("請輸入Y座標(公尺): ")) + z = float(input("請輸入Z座標(公尺,高度為正值): ")) + vx = float(input("請輸入X方向速度(公尺/秒，可選): ") or "0") + vy = float(input("請輸入Y方向速度(公尺/秒，可選): ") or "0") + if drone.goto_position_local(x, y, z, vx, vy): + print(f"無人機 #{current_drone_id} 已到達目標位置") + else: + print(f"無人機 #{current_drone_id} 移動失敗") + + elif choice == '7': + if drone.set_mode("RTL"): + print(f"無人機 #{current_drone_id} 已切換至 RTL 模式，正在返航") + else: + print(f"無人機 #{current_drone_id} 切換至 RTL 模式失敗") + + elif choice == '8': + state = drone.get_current_state() + if state: + print(f"\n=== 無人機 #{current_drone_id} 狀態 ===") + print(f"模式: {state.get('mode', '未知')}") + print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}") + + if 'gps_position' in state: + gps = state['gps_position'] + print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m") + + if 'position' in state: + pos = state['position'] + print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m") + print("=============================") + else: + print(f"無法獲取無人機 #{current_drone_id} 狀態") + + elif choice == '9': + print("返回主菜單") + break + + else: + print("無效的指令，請重新輸入") + + except ValueError as e: + print(f"輸入錯誤: {e}") + except Exception as e: + print(f"發生錯誤: {e}") + +def control_all_drones(controller): + print_all_drones_menu() + + while True: + try: + choice = input("\n請輸入指令編號: ") + + if choice == '0': + print_all_drones_menu() + + elif choice == '1': + if controller.all_drones_command("set_mode", "GUIDED"): + print("所有無人機已切換至 GUIDED 模式") + else: + print("部分無人機切換模式失敗") + + elif choice == '2': + if controller.all_drones_command("set_mode", "LAND"): + print("所有無人機已切換至 LAND 模式") + else: + print("部分無人機切換模式失敗") + + elif choice == '3': + if controller.all_drones_command("arm", True): + print("所有無人機已解鎖") + else: + print("部分無人機解鎖失敗") + + elif choice == '4': + if controller.all_drones_command("arm", False): + print("所有無人機已上鎖") + else: + print("部分無人機上鎖失敗") + + elif choice == '5': + altitude = float(input("請輸入所有無人機起飛高度(公尺): ")) + if controller.all_drones_command("takeoff", altitude): + print(f"所有無人機已到達目標高度: {altitude}公尺") + else: + print("部分無人機起飛失敗") + + elif choice == '6': + if controller.all_drones_command("set_mode", "RTL"): + print("所有無人機已切換至 RTL 模式，正在返航") + else: + print("部分無人機切換至 RTL 模式失敗") + + elif choice == '7': + print("返回主菜單") + break + + else: + print("無效的指令，請重新輸入") + + except ValueError as e: + print(f"輸入錯誤: {e}") + except Exception as e: + print(f"發生錯誤: {e}") + +def display_all_drone_states(controller): + """顯示所有無人機狀態""" + print("\n=== 所有無人機狀態 ===") + + for drone in controller.drones: + max_attempts = 5 + state = None + + for attempt in range(max_attempts): + state = drone.get_current_state() + if state and 'position' in state: + break + time.sleep(0.2) + + print(f"\n--- 無人機 #{drone.drone_id} ---") + print(f"連接: {drone.connection_string}") + + if not state: + print("狀態: 無法獲取完整數據") + continue + + print(f"飛行模式: {state.get('mode', '未知')}") + print(f"解鎖狀態: {'已解鎖' if state.get('armed', False) else '已上鎖'}") + + if 'gps_position' in state: + gps = state['gps_position'] + print(f"GPS位置: 緯度={gps['lat']:.6f}, 經度={gps['lon']:.6f}, 高度={gps['alt']:.2f}m") + print(f"相對高度: {gps['relative_alt']:.2f}m") + + if 'position' in state: + pos = state['position'] + print(f"本地位置: X={pos['x']:.2f}m, Y={pos['y']:.2f}m, Z={pos['z']:.2f}m") + + print("-----------------------") + + print("=========================") + +def main(): + controller = MultiDroneController(5) + print_menu() + + while True: + try: + choice = input("\n請輸入指令編號: ") + + if choice == '0': + print_menu() + + elif choice == '1': + drone_id = int(input("請輸入要操作的無人機ID (0-4): ")) + drone = controller.get_drone(drone_id) + if drone: + control_single_drone(drone) + else: + print(f"無效的無人機ID: {drone_id}") + + elif choice == '2': + control_all_drones(controller) + + elif choice == '3': + display_all_drone_states(controller) + + elif choice == '4': + print("程式結束") + break + + else: + print("無效的指令，請重新輸入") + + except ValueError as e: + print(f"輸入錯誤: {e}") + except Exception as e: + print(f"發生錯誤: {e}") + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\n程式被使用者中斷") + sys.exit(0) \ No newline at end of file From 598e80d07066394a25bf780f57358db49376de91 Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:28:05 +0800 Subject: [PATCH 018/146] =?UTF-8?q?=E6=96=B0=E5=A2=9Efdm=5Fswitch=5Fexampl?= =?UTF-8?q?e=5Fone=5Fwith=5Fremote=5Fforwarding=E9=80=99=E5=80=8B=E5=89=AF?= =?UTF-8?q?=E5=87=BD=E5=BC=8F=EF=BC=8C=E5=8F=AF=E4=BB=A5=E6=8A=8A=E8=B3=87?= =?UTF-8?q?=E6=96=99=E6=89=93=E5=8C=85=E8=BD=89=E6=8E=A5=E5=88=B0=E5=85=B6?= =?UTF-8?q?=E4=BB=96ip?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/0328/test_transform.py | 331 +++++++++++++++++++++++++++ 1 file changed, 331 insertions(+) create mode 100644 src/unitdev03/0328/test_transform.py diff --git a/src/unitdev03/0328/test_transform.py b/src/unitdev03/0328/test_transform.py new file mode 100644 index 0000000..af72a2d --- /dev/null +++ b/src/unitdev03/0328/test_transform.py @@ -0,0 +1,331 @@ +import rclpy +from pymavlink import mavutil +from time import sleep +import time + +import socket +import struct + +# used at fdm_switch_example_two +import threading +import queue + +import json + +def mavlink_analyzer_simple(count = 500): + # rclpy.init() + # node = rclpy.create_node('mavlink_analyzer_simple') + + print('Inintializing connection') + + connection_string="udp:127.0.0.1:14550" + connection = mavutil.mavlink_connection(connection_string) + + print('Catch messages') + msg_count = {} + + start_time = time.time() + + for _ in range(count): + msg = connection.recv_msg() + # msg = connection.recv_match(blocking=True) + if msg: + msg_type = msg.get_type() + if msg_type in msg_count: + msg_count[msg_type] += 1 + else: + msg_count[msg_type] = 1 + + if msg.get_type() == 'SERVO_OUTPUT_RAW': + print(msg) + pass + + else: + print('No message yet, 1 second for data to fill') + sleep(1) + + print('Messages Type received:') + print(msg_count) + + end_time = time.time() + print('Time elapsed: ', end_time - start_time) + + print('Closing connection') + connection.close() + + +def fdm_parser_example(data=None): + if data is None: + data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000') + # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000 + + + parse_format = 'HHI16H' + + if len(data) != struct.calcsize(parse_format): + print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format))) + exit(1) + + decoded = struct.unpack(parse_format,data) + + magic = decoded[0] + frame_rate_hz = decoded[1] + frame_count = decoded[2] + pwm = decoded[3:] + + + print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm)) + +def fdm_switch_example_one(): + + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 沒有轉換數據格式 + ''' + # make a link get fdm from SITL, as a server + sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock_s2g.bind(('', 9002)) + sock_s2g.settimeout(0.1) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19002) + + packet_count = 0 + start_time = time.time() + + while True: + try: + # 接到SITL的數據 並轉發給Gazebo + data_s2g, addr_s2g = sock_s2g.recvfrom(1024) + sock_g2s.sendto(data_s2g, server_address) + # 得到Gazebo的回應 並轉發給SITL + data_g2s, addr_g2s = sock_g2s.recvfrom(1024) + sock_s2g.sendto(data_g2s, addr_s2g) + + # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo) + try: + # 解析從 SITL 到 Gazebo 的數據 + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # 解析從 Gazebo 到 SITL 的數據 + # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON + try: + g2s_data_str = data_g2s.decode('utf-8') + # 保存原始數據，適合後續分析 + with open('gazebo_response_raw.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + # 如果不是 UTF-8 格式，可能是二進制數據 + g2s_data_str = str(data_g2s) + + # 合併數據到一個 JSON 對象 + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time() + } + + # 寫入 JSON 檔案 + with open('fdm_one_data.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + except Exception as e: + print(f"JSON 處理錯誤: {e}") + + packet_count += 1 + except socket.timeout: + print(f'no value') + #print(f'address:{addr_s2g}') + #pass + + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + #print(f"Packets received in the last second: {packet_count}") + print(f'JSON Pack:{data_g2s}') + packet_count = 0 + start_time = current_time + fdm_parser_example(data_s2g) + + +def fdm_switch_example_two(): + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 有轉換數據格式 + + time_usec + servo1_raw + servo16_raw + ''' + # read info from SITL via MAVLink + connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB + connection = mavutil.mavlink_connection(connection_string) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19002) + + data_queue = queue.Queue() + servo_out = [0] * 16 + data_queue.put(servo_out) + + def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1): + interval = 1.0 / frame_rate_fdm + while True: + start_time = time.time() + try: + data = q.get(timeout=0.1) + if data is None: + break + last_data = data + except queue.Empty: + data = last_data + data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out) + sock.sendto(data, server_address) + + # 新增：寫入 JSON 檔案 + data_json = { + "magic": 0x481a, + "frame_rate_hz": frame_rate_fdm, + "frame_count": frame_count_fdm, + "pwm": list(servo_out) + } + with open('fdm_data.json', 'w') as f: + json.dump(data_json, f) + + frame_count_fdm += 1 + fdm_parser_example(data) # debug + + elapsed_time = time.time() - start_time + sleep_time = interval - elapsed_time + if sleep_time > 0: + sleep(sleep_time) + + Running = True + thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue)) + thread.start() + + while Running: + msgb1 = None + msg = connection.recv_msg() + while msg : + if msg.get_type() == 'SERVO_OUTPUT_RAW': + msgb1 = msg + # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值 + msg = connection.recv_msg() + + if msgb1: + for i in range(16): + servo_out[i] = getattr(msgb1, f'servo{i+1}_raw') + data_queue.put(servo_out) + msgb1 = None + # Running = False # debug + else: + # print('No message yet, 10 ms for data to fill') + sleep(0.01) + + # Example of putting data into the queue + # data_queue.put(data) + # To stop the thread, you can put None into the queue + # data_queue.put(None) + + +def fdm_switch_example_one_with_remote_forwarding(): + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 並且將JSON數據轉發到指定的遠端IP + ''' + # make a link get fdm from SITL, as a server + sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock_s2g.bind(('', 9002)) + sock_s2g.settimeout(0.1) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19002) + + # 建立與遠端IP的socket連接 + remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + remote_address = ('140.120.31.130', 9003) # 遠端IP與埠號，您可以根據需要更改埠號 + print(f'Setting up forwarding to remote address: {remote_address}') + + packet_count = 0 + start_time = time.time() + + while True: + try: + # 接到SITL的數據 並轉發給Gazebo + data_s2g, addr_s2g = sock_s2g.recvfrom(1024) + sock_g2s.sendto(data_s2g, server_address) + + # 得到Gazebo的回應 並轉發給SITL + data_g2s, addr_g2s = sock_g2s.recvfrom(1024) + sock_s2g.sendto(data_g2s, addr_s2g) + + # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo) + try: + # 解析從 SITL 到 Gazebo 的數據 + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # 解析從 Gazebo 到 SITL 的數據 + try: + g2s_data_str = data_g2s.decode('utf-8') + # 保存原始數據，適合後續分析 + with open('gazebo_response_raw.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + # 如果不是 UTF-8 格式，可能是二進制數據 + g2s_data_str = str(data_g2s) + + # 合併數據到一個 JSON 對象 + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time() + } + + # 寫入 JSON 檔案 + with open('fdm_one_data.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + # 將JSON數據轉發到遠端IP + remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) + + except Exception as e: + print(f"JSON 處理錯誤: {e}") + + packet_count += 1 + except socket.timeout: + print(f'no value') + #pass + + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + print(f'JSON Pack:{data_g2s}') + print(f'Forwarded data to remote IP: 140.120.31.130') + packet_count = 0 + start_time = current_time + fdm_parser_example(data_s2g) + +print('Start') +#fdm_switch_example_two() +#mavlink_analyzer_simple() +#fdm_switch_example_one() +fdm_switch_example_one_with_remote_forwarding() +#fdm_parser_example() \ No newline at end of file From a934ccfa3cf1da65b39143e4e57d1b124654defe Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:30:48 +0800 Subject: [PATCH 019/146] =?UTF-8?q?=E5=AF=A6=E7=8F=BEclose=5Floop=E7=9A=84?= =?UTF-8?q?=E9=83=A8=E4=BB=BD=EF=BC=8C=E4=B8=BB=E8=A6=81=E6=98=AF=E8=B2=A0?= =?UTF-8?q?=E8=B2=AC=E6=8E=A5=E6=94=B6matlab=E5=9B=9E=E5=82=B3=E7=9A=84?= =?UTF-8?q?=E5=B0=81=E5=8C=85=EF=BC=8C=E4=B8=A6=E4=BB=A5mavlink=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E7=9A=84=E6=96=B9=E5=BC=8F=E6=89=93=E9=80=B2=E5=8E=BB?= =?UTF-8?q?Ardupilot=E8=A3=A1=E9=9D=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/0328/close_loop.py | 102 +++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) create mode 100644 src/unitdev03/0328/close_loop.py diff --git a/src/unitdev03/0328/close_loop.py b/src/unitdev03/0328/close_loop.py new file mode 100644 index 0000000..639e0cf --- /dev/null +++ b/src/unitdev03/0328/close_loop.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +from pymavlink import mavutil +import socket +import re +import time +import math + +def main(): + # 建立MAVLink連接 + print("建立MAVLink連接...") + connection_string = "udp:127.0.0.1:14550" + master = mavutil.mavlink_connection(connection_string) + master.wait_heartbeat() + print("MAVLink連接已建立") + + # 切換到GUIDED模式 + master.mav.set_mode_send( + master.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 4 # GUIDED模式ID + ) + print("已切換到GUIDED模式") + + # 解鎖無人機 + master.mav.command_long_send( + master.target_system, + master.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, + 1, 0, 0, 0, 0, 0, 0 # 1表示解鎖 + ) + print("無人機已解鎖") + + # 設置UDP服務器 + server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_socket.bind(('0.0.0.0', 5000)) + server_socket.settimeout(0.1) # 非阻塞操作 + print("UDP服務器已啟動，監聽端口5000") + + # 主循環，持續處理數據 + while True: + try: + # 接收UDP數據 + try: + data, _ = server_socket.recvfrom(1024) + decoded_data = data.decode('utf-8') + + # 提取velocity數據 + vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data) + + if vel_match: + velocity_str = vel_match.group(1) + velocity = [float(x) for x in velocity_str.split(',')] + vx, vy, vz = velocity + + # 獲取當前位置 + pos_msg = master.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=0.5) + if pos_msg: + x, y, z = pos_msg.x, pos_msg.y, pos_msg.z + + # 設置type_mask - 僅使用速度控制 + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE + ) + + # 發送位置目標命令 + print(f"發送速度指令: vx={vx}, vy={vy}, vz={vz}") + master.mav.set_position_target_local_ned_send( + 0, + master.target_system, + master.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + 0, 0, 0, + vx, vy, vz, # 速度 + 0, 0, 0, # 加速度 + 0, 0 # 航向 + ) + print("success") + # 要再去偵錯沒有data進來的狀況 + except socket.timeout: + pass + + time.sleep(0.05) # 控制循環速率 + + except KeyboardInterrupt: + print("程序終止") + server_socket.close() + break + except Exception as e: + print(f"錯誤: {e}") + time.sleep(1) + +if __name__ == "__main__": + main() \ No newline at end of file From ce06a4160b57d0082a9f11b8a0dd788ebac2895e Mon Sep 17 00:00:00 2001 From: wenchun Date: Fri, 28 Mar 2025 11:45:36 +0800 Subject: [PATCH 020/146] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E3=80=8CREADME.md?= =?UTF-8?q?=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: wenchun --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index c93bd79..cef8e3b 100644 --- a/README.md +++ b/README.md @@ -17,4 +17,6 @@ === Package 簡述 - +=== +簡述0328 +主要是透過test_transform這隻程式執行fdm_switch_example_one_with_remote_forwarding這個副函式，透過UDP將封包轉接到Matlab，並在Matlab進行演算法迭代後，打包JSON檔，並再次透過port口傳回封包，傳回來的封包會在close_loop這隻程式被解析，並提取其中有關velocity的資料儲存成變數，以mavlink的方式打入Ardupilot，以實現封閉迴路的構思。 From b6c730f74a3be485036cccdaf138d5155066191e Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Sun, 30 Mar 2025 16:19:30 +0800 Subject: [PATCH 021/146] =?UTF-8?q?MAVLink=20simple=20message=20receiver?= =?UTF-8?q?=20=E6=8E=A5=E9=80=9A=E4=BA=86=20=E5=86=8D=E4=BE=86=E8=A6=81?= =?UTF-8?q?=E5=AF=A6=E8=A3=9D=20node=20topic?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mavlinkObject.py | 357 ++++++++++++++---- src/unitdev02/unitdev02/test01.py | 29 ++ 2 files changed, 313 insertions(+), 73 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..3421a0d 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -10,7 +10,6 @@ import time ''' - # 不同的匯流排只有單一種通訊協定 # 匯流排接到訊息後透過 queue stack 傳送到分析器 # 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 @@ -21,13 +20,14 @@ import time # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 # 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function - ''' fixed_stream_analyzer_queue = queue.Queue() return_packet_processor_queue = queue.Queue() converte_format_queues = [] +mavlink_systems = {} # 用來記錄每個 system 的資訊 + class MavlinkObject(): def __init__(self, mavlink_socket, socket_id = None): self.socket_id = socket_id @@ -35,18 +35,24 @@ class MavlinkObject(): 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 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() @@ -66,6 +72,7 @@ class MavlinkObject(): def _run_thread(self): start_time = time.time() while self.running: + timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 try: mavlinkMsg = self.mavlink_socket.recv_msg() except Exception as e: @@ -75,7 +82,13 @@ class MavlinkObject(): break if mavlinkMsg: - self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息 + # 統計收到的訊息 & 透過 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) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug # break # Debug @@ -83,115 +96,313 @@ class MavlinkObject(): 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') + convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) - 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 - - def publish_mavlink_data(self, mavlinkMsg): - msg = String() - msg.data = str(mavlinkMsg) - self.publisher_.publish(msg) + # thread 結束 + print('[mavlinkObject.py] End of MavlinkObject._run_thread') 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') + print('[mavlinkObject.py] Multiplexing List Format Error, Please check the list') + return False # 若有錯誤則回傳 False + + check = len(self.multiplexingToConvert) != len(converte_format_queues) + if check: + print('[mavlinkObject.py] Multiplexing Queue not fit List , Please check the list') return False return True +# ===================== 分割線 ===================================== + class mavlink_analyzer(Node): ''' - 這個 class 是用來接收 mavlink 訊息 並進行分析 + 這個 class 就是 固定串流分析器 + 是用來接收 mavlink 訊息 並進行分析 + 這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object 並且看是否有重複 system id - 也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒? + 整段代碼包含兩大區塊 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): - super().__init__('mavlink_analyzer') - - # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10) - # self.mavlink_object = None - # self.create_mavlink_object() + if not hasattr(self, "initialized"): # 防止重複初始化 + self.initialized = True + rclpy.init() + super().__init__('mavlink_analyzer') + + # 關聯到全域變數 + 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: + print('[mavlinkObject.py] WARNING : mavlink_analyzer instance already exists') + def stop(self): + self.running = False - def create_mavlink_object(self): - # connection_string="udp: + # === Thread 區塊 === + def _run_thread(self): + start_time = time.time() # debug + # 從 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() + + if msg.get_msgId() == 0: # HEARTBEAT 處理 + + # 若這個 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 + this_component.mav_autopilot = msg.autopilot + + + # 初始化 emitParams 方便之後更新與利用 + this_component.emitParams.add_parameter('base_mode') + this_component.emitParams.add_parameter('flight_mode') + + # TODO 這邊先不過度設計 但是也許之後要依照不同的載具類型再定義 + # for BATTERY_STATUS (147) + this_component.emitParams.add_parameter('battery_voltage') + # for GPS_RAW_INT (24) + this_component.emitParams.add_parameter('gps_fix_type') + # for ATTITUDE (30) + this_component.emitParams.add_parameter('roll') + this_component.emitParams.add_parameter('pitch') + this_component.emitParams.add_parameter('yaw') + # for GLOBAL_POSITION_INT (33) + this_component.emitParams.add_parameter('latitude') + this_component.emitParams.add_parameter('longitude') + this_component.emitParams.add_parameter('altitude') + # for VFR_HUD (74) + this_component.emitParams.add_parameter('heading') + + this_component.emitParams.base_mode = msg.base_mode + this_component.emitParams.flight_mode = mavutil.mode_string_v10(msg) + + elif msg.get_msgId() == 147: # BATTERY_STATUS 處理 + this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()] + + + # 若未定義的訊息類型則不處理 並跳出訊息 + else: + print('[mavlinkObject.py] Warning This Message Type Did not define process method : ',msg.get_msgId(), get_type()) + continue + + + print('[mavlinkObject.py] End of mavlink_analyzer._run_thread') + + # === Node 區塊 === + def _init_node(self): pass + # self.publisher = self.create_publisher(String, 'mavlink_analyzer', 10) + # self.subscription = self.create_subscription(String, 'mavlink_analyzer', self.listener_callback, 10) + + def emit_info(self): + # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 + # 把 emitParams 的參數發送出去 + for sysid in self.mavlink_systems: + for compid in self.mavlink_systems[sysid].components: + + # 先抓 base_mode 驗證看看算法是否正確 + msg_str = self.mavlink_systems[sysid].components[compid].base_mode + self.publisher.publish(msg_str) + # self.get_logger().info('Publishing: "%s"' % msg.data) + +# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component class mavlink_device(): def __init__(self): self.socket_id = None # 記錄來自於哪個 socket self.sysid = None - self.component_count = 1 - self.compid_list = [] - + self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object + + def __str__(self): + p_str = '' + p_str += f'socket_id : {self.socket_id}\n' + p_str += f'sysid : {self.sysid}\n' + p_str += 'has components : \n' + for compid in self.components: + p_str += f'compid : {compid}\n' + p_str += f'mav_type : {self.components[compid].mav_type}\n' + p_str += '=====================\n' + return p_str + + def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): + # 這段檢查遺失封包 + try: + last_seq = self.components[compid].msg_seq + except KeyError: + # 這個 component id 還不存在 + print('[mavlinkObject.py] Warning System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid)) + return + if last_seq != None: + expected_seq = (last_seq + 1) % 256 + diff = current_seq - expected_seq + # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug + if diff < 0: + diff += 256 + self.components[compid].lost_packet_count += diff + + # 這段更新封包的基本資訊 + self.components[compid].msg_seq = current_seq + self.components[compid].last_msg_time = current_time + if current_type in self.components[compid].msg_count: + self.components[compid].msg_count[current_type] += 1 + else: + self.components[compid].msg_count[current_type] = 1 + + def resetComponentPacketCount(self, compid): + self.components[compid].msg_count = {} + self.components[compid].msg_seq = None + self.components[compid].lost_packet_count = 0 + + class mavlink_component(): + # 這個 class 用來存放該 component 參數，然而這些參數是固定以一個頻率要輸出給 ros2 topic + class emitParamSet(): + def __init__(self): + pass + def add_parameter(self, param_name): + setattr(self, param_name, None) + + def __init__(self): + self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) + self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) + self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) + + # 紀錄從這個 component 收到最後的訊息序號和時間 + self.msg_count = {} + self.msg_seq = None + self.lost_packet_count = 0 + self.last_msg_time = 0 + + # 存放該 emit component 參數的區域 + self.emitParams = self.emitParamSet() def main(args=None): + # print('Start of Program .Test 1') + # # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 + # connection_string="udp:127.0.0.1:14550" + # mavlink_socket = mavutil.mavlink_connection(connection_string) + + # 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 + + # # 做一個空的 queue list 驗證 multiplexingToConvert + # converte_format_queues.append(queue.Queue()) + # # 啟動連線的模組 + # mavlink_object.run() + + # # 運行幾秒並印出 queue 的資料 + # 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[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_object.running = False + # mavlink_object.thread.join() + # mavlink_socket.close() + # print('End of Program .Test 1') + + # # # ======================================================================== + print('Start of Program .Test 2') connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) - # 這邊是測試代碼 運行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()) + mavlink_object.multiplexingToAnalysis = [0] # only HEARTBEAT + mavlink_object.multiplexingToReturn = [] # only HEARTBEAT + mavlink_object.multiplexingToConvert = [] # + # 啟動連線的模組 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])) - - 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_analyzer + analyzer = mavlink_analyzer() + compid = 1 - mavlink_object.running = False + # 運行幾秒 + start_time = time.time() + show_time = time.time() + while time.time() - start_time < 10: + 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.flight_mode) + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + + # 結束程式 退出所有 thread + mavlink_object.stop() mavlink_object.thread.join() - print('End of Program') - + analyzer.stop() + analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 instance + mavlink_socket.close() if __name__ == '__main__': main() \ No newline at end of file diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index c1b5276..e00d946 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -205,6 +205,21 @@ def mavlink_btye_generator_test(): custom_mode=0, system_status=mavutil.mavlink.MAV_STATE_ACTIVE ) + msg = mav.command_long_encode( + target_system=1, + target_component=1, + command=mavutil.mavlink.MAV_CMD_DO_SET_MODE, + confirmation=0, + param1=0, # Custom mode + param2=0, # Custom sub-mode + param3=0, + param4=0, + param5=0, + param6=0, + param7=0 + ) + + # 取得 MAVLink 訊息的 bytes mavlink_bytes = msg.pack(mav) @@ -219,6 +234,19 @@ def mavlink_btye_generator_test(): print(dir(mav2)) # print(my_msg) +def simple_getMavlink(): + connection_string="udp:127.0.0.1:14550" + # connection_string='/dev/ttyUSB0' + connection = mavutil.mavlink_connection(connection_string) + + while True: + msg = connection.recv_msg() + if msg: + print(msg) + else: + print('No message yet, 0.1 second for data to fill') + sleep(0.1) + print('Start') @@ -226,5 +254,6 @@ print('Start') # fdm_parser_example() # mavlink_analyzer_simple(8) # mavlink_btye_generator_test() +simple_getMavlink() From bdd6cceb96c0b0aadf705429ef29d54c821d4a28 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 7 Apr 2025 18:26:16 +0800 Subject: [PATCH 022/146] =?UTF-8?q?=E5=AE=8C=E6=88=90=E5=9B=BA=E5=AE=9A?= =?UTF-8?q?=E4=B8=B2=E6=B5=81=E5=88=86=E6=9E=90=E5=99=A8=E7=9A=84=E5=9F=BA?= =?UTF-8?q?=E7=A4=8E=E7=B5=90=E6=A7=8B=E4=B8=A6=E9=80=A3=E7=B5=90=E5=88=B0?= =?UTF-8?q?=20node=20topic=20=E6=9B=B4=E5=9A=B4=E6=A0=BC=E7=9A=84=E6=A8=A1?= =?UTF-8?q?=E7=B5=84=E5=8C=96=20=E5=88=86=E6=9E=90=E5=99=A8=20&=20socket?= =?UTF-8?q?=20adapter(mavlink=20object)=20&=20device=20=E4=B9=8B=E9=96=93?= =?UTF-8?q?=20=E7=A8=8B=E5=BC=8F=E5=8F=AF=E6=B8=AC=E8=A9=A6=E9=80=B2?= =?UTF-8?q?=E5=85=A5=E9=BB=9E=E7=82=BA=20mavlinkObject.py=20=20=E4=BE=9D?= =?UTF-8?q?=E9=96=8B=E7=99=BC=E9=80=B2=E5=BA=A6=E6=8F=90=E4=BE=9B=E4=B8=89?= =?UTF-8?q?=E5=80=8B=E6=B8=AC=E8=A9=A6=E9=A0=85=E7=9B=AE=20=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=20logger=20=E5=8A=9F=E8=83=BD=20=E8=A8=98=E9=8C=84?= =?UTF-8?q?=E5=9F=B7=E8=A1=8C=E7=8B=80=E6=B3=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 +- README.md | 5 +- .../fc_network_adapter/mavlinkDevice.py | 84 ++++ .../fc_network_adapter/mavlinkObject.py | 412 +++++++++--------- .../fc_network_adapter/mavlinkPublish.py | 52 +++ .../fc_network_adapter/theLogger.py | 43 ++ src/unitdev02/unitdev02/test01.md | 26 +- 7 files changed, 419 insertions(+), 206 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py create mode 100644 src/fc_network_adapter/fc_network_adapter/theLogger.py diff --git a/.gitignore b/.gitignore index 34df110..5905970 100644 --- a/.gitignore +++ b/.gitignore @@ -8,7 +8,8 @@ # 編譯的資料夾 **/build/ **/install/ -log/ +**/log/ +**/logs/ # CMAKE 的衍生檔 CMakeCache.txt diff --git a/README.md b/README.md index d03f8a7..852fcd0 100644 --- a/README.md +++ b/README.md @@ -6,9 +6,13 @@ 2. Python 3. Ardupilot + === 必要相依套件 +ROS2 預啟動 +1. source ~/ros2_humble/install/setup.bash +2. === 建議的開發測試 @@ -27,4 +31,3 @@ Package 簡述 同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 處理無線模組的通訊格式 (XBee) - diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py new file mode 100644 index 0000000..ba12465 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -0,0 +1,84 @@ + + +from theLogger import setup_logger + +logger = setup_logger("mavlinkDevice.py") + +# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component +class mavlink_device(): + def __init__(self): + self.socket_id = None # 記錄來自於哪個 socket + self.sysid = None + self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object + + def __str__(self): + p_str = '' + p_str += f'socket_id : {self.socket_id}\n' + p_str += f'sysid : {self.sysid}\n' + p_str += 'has components : \n' + for compid in self.components: + p_str += f'compid : {compid}\n' + p_str += f'mav_type : {self.components[compid].mav_type}\n' + p_str += '=====================\n' + return p_str + + def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): + # 這段檢查遺失封包 + try: + 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)) + return + + if last_seq != None: + expected_seq = (last_seq + 1) % 256 + diff = current_seq - expected_seq + # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug + if diff < 0: + diff += 256 + self.components[compid].lost_packet_count += diff + + # 這段更新封包的基本資訊 + self.components[compid].msg_seq = current_seq + self.components[compid].last_msg_time = current_time + if current_type in self.components[compid].msg_count: + self.components[compid].msg_count[current_type] += 1 + else: + self.components[compid].msg_count[current_type] = 1 + + def resetComponentPacketCount(self, compid): + self.components[compid].msg_count = {} + self.components[compid].msg_seq = None + self.components[compid].lost_packet_count = 0 + + class mavlink_component(): + # 程式用不到的參數 但是做個記錄 + # paraEmitList = ['base_mode', 'flightMode_mode', + # 'battery_voltage', # from BATTERY_STATUS (147) + # 'gps_fix_type', # from GPS_RAW_INT (24) + # 'roll', 'pitch', 'yaw', # from ATTITUDE (30) + # 'position_latitude', 'position_longitude', 'position_altitude', # from GLOBAL_POSITION_INT (33) + # 'heading', # for VFR_HUD (74) + # ] + + def __init__(self): + self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) + self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) + self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) + + # 紀錄從這個 component 收到最後的訊息序號和時間 + self.msg_count = {} + self.msg_seq = None + self.lost_packet_count = 0 + self.last_msg_time = 0 + + # 存放該 emit component 參數的區域 + # 內容格式為 {param_name(字串) : param_value} + # param_name 請見上面 paraEmitList + 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 3421a0d..b76aa89 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,14 +1,3 @@ -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 傳送到分析器 @@ -17,18 +6,41 @@ 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 std_msgs.msg + +# 自定義的 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 = [] -mavlink_systems = {} # 用來記錄每個 system 的資訊 +# 用來記錄每個 system 的資訊 +# 資料格式 { sysid : mavlink_device object } +mavlink_systems = {} -class MavlinkObject(): +class mavlink_object(): def __init__(self, mavlink_socket, socket_id = None): self.socket_id = socket_id self.mavlink_socket = mavlink_socket @@ -60,7 +72,7 @@ class MavlinkObject(): 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() @@ -70,18 +82,20 @@ 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: + if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 sysid = mavlinkMsg.get_srcSystem() compid = mavlinkMsg.get_srcComponent() @@ -89,7 +103,7 @@ class MavlinkObject(): # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) - # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug + # print(mavlinkMsg) # debug # break # Debug # 將訊息依照 multiplexing list 分發到不同的 queue @@ -104,7 +118,7 @@ class MavlinkObject(): convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) # thread 結束 - print('[mavlinkObject.py] End of MavlinkObject._run_thread') + logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) def updateMultiplexingList(self): ''' @@ -117,18 +131,18 @@ class MavlinkObject(): # 檢查 multiplexing list 格式是否有錯誤 check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) if not check: - print('[mavlinkObject.py] Multiplexing List Format 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: - print('[mavlinkObject.py] Multiplexing Queue not fit List , Please check the list') + logger.error('Multiplexing Queue not fit List , Please check the list') return False return True -# ===================== 分割線 ===================================== +# ===================== 分割線 ===================== -class mavlink_analyzer(Node): +class mavlink_analyzer(Node, mavlink_publisher): ''' 這個 class 就是 固定串流分析器 是用來接收 mavlink 訊息 並進行分析 @@ -159,8 +173,6 @@ class mavlink_analyzer(Node): def __init__(self): if not hasattr(self, "initialized"): # 防止重複初始化 self.initialized = True - rclpy.init() - super().__init__('mavlink_analyzer') # 關聯到全域變數 global mavlink_systems @@ -171,13 +183,14 @@ class mavlink_analyzer(Node): self.thread = threading.Thread(target=self._run_thread) self.thread.start() else: - print('[mavlinkObject.py] WARNING : mavlink_analyzer instance already exists') + 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 + # 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(): @@ -187,6 +200,8 @@ class mavlink_analyzer(Node): msg = msg_pack[2] sysid = msg.get_srcSystem() + # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ + if msg.get_msgId() == 0: # HEARTBEAT 處理 # 若這個 system id 還不存在 執行完整建立 device object 的流程 @@ -201,208 +216,199 @@ class mavlink_analyzer(Node): this_component.mav_type = msg.type this_component.mav_autopilot = msg.autopilot - - # 初始化 emitParams 方便之後更新與利用 - this_component.emitParams.add_parameter('base_mode') - this_component.emitParams.add_parameter('flight_mode') - - # TODO 這邊先不過度設計 但是也許之後要依照不同的載具類型再定義 - # for BATTERY_STATUS (147) - this_component.emitParams.add_parameter('battery_voltage') - # for GPS_RAW_INT (24) - this_component.emitParams.add_parameter('gps_fix_type') - # for ATTITUDE (30) - this_component.emitParams.add_parameter('roll') - this_component.emitParams.add_parameter('pitch') - this_component.emitParams.add_parameter('yaw') - # for GLOBAL_POSITION_INT (33) - this_component.emitParams.add_parameter('latitude') - this_component.emitParams.add_parameter('longitude') - this_component.emitParams.add_parameter('altitude') - # for VFR_HUD (74) - this_component.emitParams.add_parameter('heading') - - this_component.emitParams.base_mode = msg.base_mode - this_component.emitParams.flight_mode = mavutil.mode_string_v10(msg) - + this_component.emitParams['base_mode'] = msg.base_mode + this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg) + elif msg.get_msgId() == 147: # BATTERY_STATUS 處理 this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()] + # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑ # 若未定義的訊息類型則不處理 並跳出訊息 else: - print('[mavlinkObject.py] Warning This Message Type Did not define process method : ',msg.get_msgId(), get_type()) + logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type())) continue - - print('[mavlinkObject.py] End of mavlink_analyzer._run_thread') + logger.info('End of mavlink_analyzer._run_thread') # === Node 區塊 === def _init_node(self): - pass - # self.publisher = self.create_publisher(String, 'mavlink_analyzer', 10) - # self.subscription = self.create_subscription(String, 'mavlink_analyzer', self.listener_callback, 10) + 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 in self.mavlink_systems: - for compid in self.mavlink_systems[sysid].components: + 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 - # 先抓 base_mode 驗證看看算法是否正確 - msg_str = self.mavlink_systems[sysid].components[compid].base_mode - self.publisher.publish(msg_str) - # self.get_logger().info('Publishing: "%s"' % msg.data) +def main(args=None): -# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component -class mavlink_device(): - def __init__(self): - self.socket_id = None # 記錄來自於哪個 socket - self.sysid = None - self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object - - def __str__(self): - p_str = '' - p_str += f'socket_id : {self.socket_id}\n' - p_str += f'sysid : {self.sysid}\n' - p_str += 'has components : \n' - for compid in self.components: - p_str += f'compid : {compid}\n' - p_str += f'mav_type : {self.components[compid].mav_type}\n' - p_str += '=====================\n' - return p_str - - def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): - # 這段檢查遺失封包 - try: - last_seq = self.components[compid].msg_seq - except KeyError: - # 這個 component id 還不存在 - print('[mavlinkObject.py] Warning System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid)) - return + test_item = 2 + print('test_item : ', test_item) - if last_seq != None: - expected_seq = (last_seq + 1) % 256 - diff = current_seq - expected_seq - # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug - if diff < 0: - diff += 256 - self.components[compid].lost_packet_count += diff - - # 這段更新封包的基本資訊 - self.components[compid].msg_seq = current_seq - self.components[compid].last_msg_time = current_time - if current_type in self.components[compid].msg_count: - self.components[compid].msg_count[current_type] += 1 - else: - self.components[compid].msg_count[current_type] = 1 - - def resetComponentPacketCount(self, compid): - self.components[compid].msg_count = {} - self.components[compid].msg_seq = None - self.components[compid].lost_packet_count = 0 - - class mavlink_component(): - # 這個 class 用來存放該 component 參數，然而這些參數是固定以一個頻率要輸出給 ros2 topic - class emitParamSet(): - def __init__(self): - pass - def add_parameter(self, param_name): - setattr(self, param_name, None) - - def __init__(self): - self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) - self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) - self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) + 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 + + # 做一個空的 queue list 驗證 multiplexingToConvert + converte_format_queues.append(queue.Queue()) + # 啟動連線的模組 + mavlink_object1.run() + + # 運行幾秒並印出 queue 的資料 + start_time = time.time() + while time.time() - start_time < 2: - # 紀錄從這個 component 收到最後的訊息序號和時間 - self.msg_count = {} - self.msg_seq = None - self.lost_packet_count = 0 - self.last_msg_time = 0 + 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() - # 存放該 emit component 參數的區域 - self.emitParams = self.emitParamSet() + 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]) -def main(args=None): + - # print('Start of Program .Test 1') - # # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 - # connection_string="udp:127.0.0.1:14550" - # mavlink_socket = mavutil.mavlink_connection(connection_string) - # 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 + # 結束程式 退出所有 thread + mavlink_object2.stop() + mavlink_object2.thread.join() + analyzer.stop() - # # 做一個空的 queue list 驗證 multiplexingToConvert - # converte_format_queues.append(queue.Queue()) - # # 啟動連線的模組 - # mavlink_object.run() + analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 instance + analyzer.thread.join() + mavlink_socket.close() + print('End of Program .Test 2') - # # 運行幾秒並印出 queue 的資料 - # 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[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) + 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] # only HEARTBEAT + 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]) + end_time = time.time() + print(f"Execution time for create_flightMode: {end_time - start_time} seconds") + + print("start emit info") + + 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[2])) - - # # 結束程式 退出所有 thread - # mavlink_object.running = False - # mavlink_object.thread.join() - # mavlink_socket.close() - # print('End of Program .Test 1') - - # # # ======================================================================== - print('Start of Program .Test 2') - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - - mavlink_object = MavlinkObject(mavlink_socket, 1) - mavlink_object.multiplexingToAnalysis = [0] # only HEARTBEAT - mavlink_object.multiplexingToReturn = [] # only HEARTBEAT - mavlink_object.multiplexingToConvert = [] # - - # 啟動連線的模組 - mavlink_object.run() - # 啟動 mavlink_analyzer - analyzer = mavlink_analyzer() - compid = 1 - - # 運行幾秒 - start_time = time.time() - show_time = time.time() - while time.time() - start_time < 10: - 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.flight_mode) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - - # 結束程式 退出所有 thread - mavlink_object.stop() - mavlink_object.thread.join() - analyzer.stop() - - analyzer = mavlink_analyzer() # 這邊是測試是否只有一個 instance - mavlink_socket.close() + 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() 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..b47416e --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -0,0 +1,52 @@ + +''' +這個檔案只有一個 class +是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生 + +主要概念是將 "離散的" mavlink 參數轉換成 ROS topic +包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit + +publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> +''' + +import std_msgs.msg + +from theLogger import setup_logger + +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 訊息 請放在這裡 ↓↓↓↓↓↓↓↓↓↓↓↓ + + + + # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/theLogger.py b/src/fc_network_adapter/fc_network_adapter/theLogger.py new file mode 100644 index 0000000..c4dccfe --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/theLogger.py @@ -0,0 +1,43 @@ +import logging +import os +from logging.handlers import TimedRotatingFileHandler + +# 全域 Logger 實例 +_global_logger = None + +def setup_logger(name: str, log_dir: str = "log", level=logging.DEBUG) -> logging.Logger: + global _global_logger + + if _global_logger is None: + # 確保 logs 資料夾存在 + os.makedirs(log_dir, exist_ok=True) + + # 建立全域 Logger + _global_logger = logging.getLogger("global_logger") + _global_logger.setLevel(level) + _global_logger.propagate = False # 防止重複輸出 + + formatter = logging.Formatter( + fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s", + datefmt="%m-%d %H:%M:%S" + ) + + # 檔案輸出（每天輪替） + file_handler = TimedRotatingFileHandler( + filename=os.path.join(log_dir, "application.log"), + when="midnight", # 每天 0 點輪替 + backupCount=7, # 保留 7 天 + encoding="utf-8" + ) + file_handler.setFormatter(formatter) + _global_logger.addHandler(file_handler) + + # 終端機輸出 + console_handler = logging.StreamHandler() + console_handler.setFormatter(formatter) + _global_logger.addHandler(console_handler) + + # 為每個模組建立子 Logger，並設定名稱 + module_logger = _global_logger.getChild(name) + module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱 + return module_logger diff --git a/src/unitdev02/unitdev02/test01.md b/src/unitdev02/unitdev02/test01.md index 053d1ac..af13f0b 100644 --- a/src/unitdev02/unitdev02/test01.md +++ b/src/unitdev02/unitdev02/test01.md @@ -75,7 +75,31 @@ mavlink 封包 種類名稱 ===================================== + HEARTBEAT {type : 2, autopilot : 3, base_mode : 81, custom_mode : 0, system_status : 3, mavlink_version : 3} -['_MAVLink__callbacks', '_MAVLink__parse_char_legacy', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'actuator_control_target_encode', 'actuator_control_target_send', 'actuator_output_status_encode', 'actuator_output_status_send', 'adsb_vehicle_encode', 'adsb_vehicle_send', 'ais_vessel_encode', 'ais_vessel_send', 'altitude_encode', 'altitude_send', 'att_pos_mocap_encode', 'att_pos_mocap_send', 'attitude_encode', 'attitude_quaternion_cov_encode', 'attitude_quaternion_cov_send', 'attitude_quaternion_encode', 'attitude_quaternion_send', 'attitude_send', 'attitude_target_encode', 'attitude_target_send', 'auth_key_encode', 'auth_key_send', 'autopilot_state_for_gimbal_device_encode', 'autopilot_state_for_gimbal_device_send', 'autopilot_version_encode', 'autopilot_version_send', 'battery_status_encode', 'battery_status_send', 'buf', 'buf_index', 'buf_len', 'button_change_encode', 'button_change_send', 'bytes_needed', 'callback', 'callback_args', 'callback_kwargs', 'camera_capture_status_encode', 'camera_capture_status_send', 'camera_fov_status_encode', 'camera_fov_status_send', 'camera_image_captured_encode', 'camera_image_captured_send', 'camera_information_encode', 'camera_information_send', 'camera_settings_encode', 'camera_settings_send', 'camera_thermal_range_encode', 'camera_thermal_range_send', 'camera_tracking_geo_status_encode', 'camera_tracking_geo_status_send', 'camera_tracking_image_status_encode', 'camera_tracking_image_status_send', 'camera_trigger_encode', 'camera_trigger_send', 'can_filter_modify_encode', 'can_filter_modify_send', 'can_frame_encode', 'can_frame_send', 'canfd_frame_encode', 'canfd_frame_send', 'change_operator_control_ack_encode', 'change_operator_control_ack_send', 'change_operator_control_encode', 'change_operator_control_send', 'check_signature', 'collision_encode', 'collision_send', 'command_ack_encode', 'command_ack_send', 'command_int_encode', 'command_int_send', 'command_long_encode', 'command_long_send', 'control_system_state_encode', 'control_system_state_send', 'crc_extra', 'data_stream_encode', 'data_stream_send', 'data_transmission_handshake_encode', 'data_transmission_handshake_send', 'debug_encode', 'debug_float_array_encode', 'debug_float_array_send', 'debug_send', 'debug_vect_encode', 'debug_vect_send', 'decode', 'distance_sensor_encode', 'distance_sensor_send', 'efi_status_encode', 'efi_status_send', 'encapsulated_data_encode', 'encapsulated_data_send', 'estimator_status_encode', 'estimator_status_send', 'expected_length', 'extended_sys_state_encode', 'extended_sys_state_send', 'fence_status_encode', 'fence_status_send', 'file', 'file_transfer_protocol_encode', 'file_transfer_protocol_send', 'flight_information_encode', 'flight_information_send', 'follow_target_encode', 'follow_target_send', 'generator_status_encode', 'generator_status_send', 'gimbal_device_attitude_status_encode', 'gimbal_device_attitude_status_send', 'gimbal_device_information_encode', 'gimbal_device_information_send', 'gimbal_device_set_attitude_encode', 'gimbal_device_set_attitude_send', 'gimbal_manager_information_encode', 'gimbal_manager_information_send', 'gimbal_manager_set_attitude_encode', 'gimbal_manager_set_attitude_send', 'gimbal_manager_set_manual_control_encode', 'gimbal_manager_set_manual_control_send', 'gimbal_manager_set_pitchyaw_encode', 'gimbal_manager_set_pitchyaw_send', 'gimbal_manager_status_encode', 'gimbal_manager_status_send', 'global_position_int_cov_encode', 'global_position_int_cov_send', 'global_position_int_encode', 'global_position_int_send', 'global_vision_position_estimate_encode', 'global_vision_position_estimate_send', 'gps2_raw_encode', 'gps2_raw_send', 'gps2_rtk_encode', 'gps2_rtk_send', 'gps_global_origin_encode', 'gps_global_origin_send', 'gps_inject_data_encode', 'gps_inject_data_send', 'gps_input_encode', 'gps_input_send', 'gps_raw_int_encode', 'gps_raw_int_send', 'gps_rtcm_data_encode', 'gps_rtcm_data_send', 'gps_rtk_encode', 'gps_rtk_send', 'gps_status_encode', 'gps_status_send', 'have_prefix_error', 'heartbeat_encode', 'heartbeat_send', 'high_latency2_encode', 'high_latency2_send', 'high_latency_encode', 'high_latency_send', 'highres_imu_encode', 'highres_imu_send', 'hil_actuator_controls_encode', 'hil_actuator_controls_send', 'hil_controls_encode', 'hil_controls_send', 'hil_gps_encode', 'hil_gps_send', 'hil_optical_flow_encode', 'hil_optical_flow_send', 'hil_rc_inputs_raw_encode', 'hil_rc_inputs_raw_send', 'hil_sensor_encode', 'hil_sensor_send', 'hil_state_encode', 'hil_state_quaternion_encode', 'hil_state_quaternion_send', 'hil_state_send', 'home_position_encode', 'home_position_send', 'hygrometer_sensor_encode', 'hygrometer_sensor_send', 'isbd_link_status_encode', 'isbd_link_status_send', 'landing_target_encode', 'landing_target_send', 'little_endian', 'local_position_ned_cov_encode', 'local_position_ned_cov_send', 'local_position_ned_encode', 'local_position_ned_send', 'local_position_ned_system_global_offset_encode', 'local_position_ned_system_global_offset_send', 'log_data_encode', 'log_data_send', 'log_entry_encode', 'log_entry_send', 'log_erase_encode', 'log_erase_send', 'log_request_data_encode', 'log_request_data_send', 'log_request_end_encode', 'log_request_end_send', 'log_request_list_encode', 'log_request_list_send', 'logging_ack_encode', 'logging_ack_send', 'logging_data_acked_encode', 'logging_data_acked_send', 'logging_data_encode', 'logging_data_send', 'mag_cal_report_encode', 'mag_cal_report_send', 'manual_control_encode', 'manual_control_send', 'manual_setpoint_encode', 'manual_setpoint_send', 'mav10_unpacker', 'mav20_h3_unpacker', 'mav20_unpacker', 'mav_csum_unpacker', 'mav_sign_unpacker', 'memory_vect_encode', 'memory_vect_send', 'message_interval_encode', 'message_interval_send', 'mission_ack_encode', 'mission_ack_send', 'mission_clear_all_encode', 'mission_clear_all_send', 'mission_count_encode', 'mission_count_send', 'mission_current_encode', 'mission_current_send', 'mission_item_encode', 'mission_item_int_encode', 'mission_item_int_send', 'mission_item_reached_encode', 'mission_item_reached_send', 'mission_item_send', 'mission_request_encode', 'mission_request_int_encode', 'mission_request_int_send', 'mission_request_list_encode', 'mission_request_list_send', 'mission_request_partial_list_encode', 'mission_request_partial_list_send', 'mission_request_send', 'mission_set_current_encode', 'mission_set_current_send', 'mission_write_partial_list_encode', 'mission_write_partial_list_send', 'mount_orientation_encode', 'mount_orientation_send', 'named_value_float_encode', 'named_value_float_send', 'named_value_int_encode', 'named_value_int_send', 'nav_controller_output_encode', 'nav_controller_output_send', 'obstacle_distance_encode', 'obstacle_distance_send', 'odometry_encode', 'odometry_send', 'open_drone_id_arm_status_encode', 'open_drone_id_arm_status_send', 'open_drone_id_authentication_encode', 'open_drone_id_authentication_send', 'open_drone_id_basic_id_encode', 'open_drone_id_basic_id_send', 'open_drone_id_location_encode', 'open_drone_id_location_send', 'open_drone_id_message_pack_encode', 'open_drone_id_message_pack_send', 'open_drone_id_operator_id_encode', 'open_drone_id_operator_id_send', 'open_drone_id_self_id_encode', 'open_drone_id_self_id_send', 'open_drone_id_system_encode', 'open_drone_id_system_send', 'open_drone_id_system_update_encode', 'open_drone_id_system_update_send', 'optical_flow_encode', 'optical_flow_rad_encode', 'optical_flow_rad_send', 'optical_flow_send', 'param_ext_ack_encode', 'param_ext_ack_send', 'param_ext_request_list_encode', 'param_ext_request_list_send', 'param_ext_request_read_encode', 'param_ext_request_read_send', 'param_ext_set_encode', 'param_ext_set_send', 'param_ext_value_encode', 'param_ext_value_send', 'param_map_rc_encode', 'param_map_rc_send', 'param_request_list_encode', 'param_request_list_send', 'param_request_read_encode', 'param_request_read_send', 'param_set_encode', 'param_set_send', 'param_value_encode', 'param_value_send', 'parse_buffer', 'parse_char', 'ping_encode', 'ping_send', 'play_tune_encode', 'play_tune_send', 'position_target_global_int_encode', 'position_target_global_int_send', 'position_target_local_ned_encode', 'position_target_local_ned_send', 'power_status_encode', 'power_status_send', 'protocol_marker', 'radio_status_encode', 'radio_status_send', 'raw_imu_encode', 'raw_imu_send', 'raw_pressure_encode', 'raw_pressure_send', 'raw_rpm_encode', 'raw_rpm_send', 'rc_channels_encode', 'rc_channels_override_encode', 'rc_channels_override_send', 'rc_channels_raw_encode', 'rc_channels_raw_send', 'rc_channels_scaled_encode', 'rc_channels_scaled_send', 'rc_channels_send', 'relay_status_encode', 'relay_status_send', 'request_data_stream_encode', 'request_data_stream_send', 'resource_request_encode', 'resource_request_send', 'robust_parsing', 'safety_allowed_area_encode', 'safety_allowed_area_send', 'safety_set_allowed_area_encode', 'safety_set_allowed_area_send', 'scaled_imu2_encode', 'scaled_imu2_send', 'scaled_imu3_encode', 'scaled_imu3_send', 'scaled_imu_encode', 'scaled_imu_send', 'scaled_pressure2_encode', 'scaled_pressure2_send', 'scaled_pressure3_encode', 'scaled_pressure3_send', 'scaled_pressure_encode', 'scaled_pressure_send', 'send', 'send_callback', 'send_callback_args', 'send_callback_kwargs', 'seq', 'serial_control_encode', 'serial_control_send', 'servo_output_raw_encode', 'servo_output_raw_send', 'set_actuator_control_target_encode', 'set_actuator_control_target_send', 'set_attitude_target_encode', 'set_attitude_target_send', 'set_callback', 'set_gps_global_origin_encode', 'set_gps_global_origin_send', 'set_home_position_encode', 'set_home_position_send', 'set_mode_encode', 'set_mode_send', 'set_position_target_global_int_encode', 'set_position_target_global_int_send', 'set_position_target_local_ned_encode', 'set_position_target_local_ned_send', 'set_send_callback', 'setup_signing_encode', 'setup_signing_send', 'signing', 'sim_state_encode', 'sim_state_send', 'smart_battery_info_encode', 'smart_battery_info_send', 'sort_fields', 'srcComponent', 'srcSystem', 'startup_time', 'statustext_encode', 'statustext_send', 'storage_information_encode', 'storage_information_send', 'sys_status_encode', 'sys_status_send', 'system_time_encode', 'system_time_send', 'terrain_check_encode', 'terrain_check_send', 'terrain_data_encode', 'terrain_data_send', 'terrain_report_encode', 'terrain_report_send', 'terrain_request_encode', 'terrain_request_send', 'timesync_encode', 'timesync_send', 'total_bytes_received', 'total_bytes_sent', 'total_packets_received', 'total_packets_sent', 'total_receive_errors', 'trajectory_representation_bezier_encode', 'trajectory_representation_bezier_send', 'trajectory_representation_waypoints_encode', 'trajectory_representation_waypoints_send', 'tunnel_encode', 'tunnel_send', 'uavcan_node_info_encode', 'uavcan_node_info_send', 'uavcan_node_status_encode', 'uavcan_node_status_send', 'utm_global_position_encode', 'utm_global_position_send', 'v2_extension_encode', 'v2_extension_send', 'vfr_hud_encode', 'vfr_hud_send', 'vibration_encode', 'vibration_send', 'vicon_position_estimate_encode', 'vicon_position_estimate_send', 'video_stream_information_encode', 'video_stream_information_send', 'video_stream_status_encode', 'video_stream_status_send', 'vision_position_estimate_encode', 'vision_position_estimate_send', 'vision_speed_estimate_encode', 'vision_speed_estimate_send', 'wheel_distance_encode', 'wheel_distance_send', 'wifi_config_ap_encode', 'wifi_config_ap_send', 'winch_status_encode', 'winch_status_send', 'wind_cov_encode', 'wind_cov_send'] +===================================== + + +? + +https://github.com/ros/angles +https://github.com/ros-geographic-info/geographic_info + + +指令 + +mkdir -p ~/ros2_geographic_info/src +cd ~/ros2_geographic_info/src +git clone https://github.com/ros/angles.git -b ros2 +git clone https://github.com/ros-geographic-info/geographic_info.git -b ros2 + +cd .. +rosdep check --from-paths src --ignore-src + +colcon build --packages-select angles +. ./install/setup.bash +colcon build --packages-select geographic_info +. ./install/setup.bash +colcon build --packages-select mavros_msgs From 5c08a961cfa7db1a7fb5aded8f124fe4c0a1d173 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 9 Apr 2025 11:25:40 +0800 Subject: [PATCH 023/146] =?UTF-8?q?code=20=E6=B2=92=E6=9C=89=E5=8B=95=20?= =?UTF-8?q?=E5=8F=AA=E6=98=AF=E8=AA=BF=E6=95=B4=E7=89=88=E9=9D=A2=20?= =?UTF-8?q?=E8=88=87=20=E6=96=B0=E5=A2=9E=E4=B8=80=E4=BA=9B=E8=A8=BB?= =?UTF-8?q?=E8=A7=A3=20=E5=A2=9E=E5=8A=A0=E5=8F=AF=E8=AE=80=E6=80=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mavlinkObject.py | 228 ++++++++++-------- 1 file changed, 125 insertions(+), 103 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index b76aa89..185bea2 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -30,6 +30,7 @@ from theLogger import setup_logger # ====================== 分割線 ===================== + logger = setup_logger("mavlinkObject.py") fixed_stream_analyzer_queue = queue.Queue() @@ -40,107 +41,7 @@ converte_format_queues = [] # 資料格式 { sysid : mavlink_device object } mavlink_systems = {} -class mavlink_object(): - 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 - 24, # GPS_RAW_INT - 30, # ATTITUDE - 33, # GLOBAL_POSITION_INT - 74, # VFR_HUD - 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [ - 0, # HEARTBEAT - # 30, # ATTITUDE - ] - self.multiplexingToConvert = [ - ] - def __del__(self): - self.mavlink_socket.close() - self.stop() - - def run(self): - if not self.socket_id: - logger.error('Please set socket id before running') - return - self.thread = threading.Thread(target=self._run_thread) - self.running = self.updateMultiplexingList() - self.thread.start() - - def stop(self): - 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: # 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,timestamp,mavlinkMsg)) - elif i == 1: - return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) - else: - convert_queue = converte_format_queues[i-2] - convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) - - # 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) - if not check: - 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, mavlink_publisher): ''' @@ -170,6 +71,7 @@ class mavlink_analyzer(Node, mavlink_publisher): 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 @@ -184,6 +86,7 @@ class mavlink_analyzer(Node, mavlink_publisher): self.thread.start() else: logger.error('mavlink_analyzer instance already exists. Do not create another one.') + def stop(self): self.running = False @@ -253,8 +156,127 @@ class mavlink_analyzer(Node, mavlink_publisher): # 再註銷 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 + 24, # GPS_RAW_INT + 30, # ATTITUDE + 33, # GLOBAL_POSITION_INT + 74, # VFR_HUD + 147, # BATTERY_STATUS + ] + self.multiplexingToReturn = [ + 0, # HEARTBEAT + # 30, # ATTITUDE + ] + self.multiplexingToConvert = [ + ] + + def __del__(self): + self.mavlink_socket.close() + self.stop() + + def run(self): + if not self.socket_id: + logger.error('Please set socket id before running') + return + self.thread = threading.Thread(target=self._run_thread) + self.running = self.updateMultiplexingList() + self.thread.start() + + def stop(self): + 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: # 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) -def main(args=None): + # 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,timestamp,mavlinkMsg)) + elif i == 1: + return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) + else: + convert_queue = converte_format_queues[i-2] + convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) + + # 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) + if not check: + 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 + +# ====================== 分割線 ===================== + +# 整合到 ros2 之後的程式進入點 +def main_node(): + pass + +# ====================== 分割線 ===================== +# 測試時的程式進入點 +def main_develop(args=None): test_item = 2 print('test_item : ', test_item) @@ -411,4 +433,4 @@ def main(args=None): if __name__ == '__main__': import rclpy # 這邊是為了測試而加的 - main() + main_develop() From cdf139f4e81380a8facc1326371dbeb1426c7d5a Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Wed, 9 Apr 2025 14:20:29 +0800 Subject: [PATCH 024/146] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E3=80=8Csrc/unitdev0?= =?UTF-8?q?4/change=5Fmode.py=E3=80=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/change_mode.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py index 4a71d5f..30106de 100644 --- a/src/unitdev04/change_mode.py +++ b/src/unitdev04/change_mode.py @@ -46,7 +46,7 @@ def curses_main(stdscr): stdscr.clear() h, w = stdscr.getmaxyx() - stdscr.addstr(1, 2, "🛫 模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") + stdscr.addstr(1, 2, "模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") for i, (name, _) in enumerate(mode_list): if i == selected: @@ -77,19 +77,19 @@ def curses_main(stdscr): param2=custom_mode, param3=0, param4=0, param5=0, param6=0, param7=0 ) - print("🧪 msg =", msg) # 確認封包物件生成 + print("msg =", msg) # 確認封包物件生成 - mav.send(msg) # ✅ 改為 send() 會寫入 capture - print("📦 RAW HEX:", capture.data.hex()) + mav.send(msg) # 改為 send() 會寫入 capture + print("RAW HEX:", capture.data.hex()) api_frame = build_api_tx_frame(capture.data) ser.write(api_frame) # 顯示封包資訊 msg_line = min(h - 4, len(mode_list) + 5) - stdscr.addstr(msg_line, 2, f"✅ 發送模式切換：{name} ({custom_mode})") - stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") - stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") + stdscr.addstr(msg_line, 2, f"發送模式切換：{name} ({custom_mode})") + stdscr.addstr(msg_line + 1, 2, f"MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}") + stdscr.addstr(msg_line + 2, 2, f"XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}") stdscr.refresh() curses.napms(1500) @@ -103,7 +103,7 @@ try: curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser except Exception as e: - print(f"❌ 發生錯誤: {e}") + print(f"發生錯誤: {e}") finally: ser.close() - print("👋 程式結束，串口已關閉") + print("程式結束，串口已關閉") From f7f725a2abc4362dd27880ebf885c310cf9828f5 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 9 Apr 2025 20:35:56 +0800 Subject: [PATCH 025/146] 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 027/146] 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 028/146] 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 029/146] =?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 e4919cfe782608fc72cf82c8b835ccacd0b83c2c Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 00:39:19 +0800 Subject: [PATCH 030/146] =?UTF-8?q?XBee=E5=B0=81=E5=8C=85=E8=BD=89?= =?UTF-8?q?=E6=8F=9B(change=20mode)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/change_mode.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitdev04/change_mode.py b/src/unitdev04/change_mode.py index 30106de..53c96f3 100644 --- a/src/unitdev04/change_mode.py +++ b/src/unitdev04/change_mode.py @@ -46,7 +46,7 @@ def curses_main(stdscr): stdscr.clear() h, w = stdscr.getmaxyx() - stdscr.addstr(1, 2, "模式選單（使用 ↑↓ 選擇，Enter 發送，q 離開）") + stdscr.addstr(1, 2, "模式選單（使用:箭頭選擇，Enter:發送，q:離開）") for i, (name, _) in enumerate(mode_list): if i == selected: From d312f644c89b2c392f7d969b703d39271bfde05c Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 00:40:12 +0800 Subject: [PATCH 031/146] =?UTF-8?q?XBee=E8=A7=A3=E8=AE=80heartbeat?= =?UTF-8?q?=E8=B3=87=E8=A8=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/receive.py | 60 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 src/unitdev04/receive.py diff --git a/src/unitdev04/receive.py b/src/unitdev04/receive.py new file mode 100644 index 0000000..9136d3c --- /dev/null +++ b/src/unitdev04/receive.py @@ -0,0 +1,60 @@ +from xbee import XBee +import serial +from pymavlink import mavutil +import time + +# 配置 XBee 串口連接 +#ser = serial.Serial('COM5', 57600) # XBee 串口 + + +# 無人機系統 ID（假設為 5）和目標系統 ID（假設為 1） +system_id = 5 +target_system_id = 1 # 設定目標系統 ID，這裡假設目標系統 ID 是 1 + +# 目標設備的 64 位地址（請替換為實際的無人機地址） +target_address = b'\x00\x13\xa2\x00\x40\x5f\x88\x56' # 例如：00 13 A2 00 40 5F 88 56 + +def decode_mavlink_data(data,master): + """解碼 MAVLink 的原始數據並處理 HEARTBEAT 訊息""" + + try: + msg = master.parse_char(data) + if msg: + if msg.get_type() == "HEARTBEAT": + print(f"Raw MAVLink Data (Hex): {data.hex()}") + print(f"Received MAVLink message: {msg}") + print(f"System status: {msg.system_status}") + print(f"Flight mode: {mavutil.mode_string_v10(msg)}") + print(f"System ID: {msg.get_srcSystem()}") # 使用 get_srcSystem() 獲取 system_id + except Exception as e: + print(f"Failed to decode MAVLink data: {e}") + +def receive_packets(ser): + xbee = XBee(ser) + # 創建 MAVLink 解析器並與 XBee 串口連接 + master = mavutil.mavlink.MAVLink(ser) + + while True: + try: + # 從 XBee 接收數據 + xbee_data = xbee.wait_read_frame() + + # 讀取 `rf_data` 而非 `payload` + raw_data = xbee_data.get('rf_data', None) + if raw_data is None: + print("Warning: No 'rf_data' found in received XBee data!") + continue # 跳過此次循環 + + # 解碼 MAVLink 訊息 + decode_mavlink_data(raw_data,master) + + # 根據需要觸發模式切換，例如根據用戶輸入更改模式 + + + except KeyboardInterrupt: + print("Exiting...") + break + except Exception as e: + print(f"Error: {e}") + + time.sleep(0.1) From 5f73324a34d01f012e326ab7921bd201512b5cff Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 00:42:11 +0800 Subject: [PATCH 032/146] =?UTF-8?q?API=20XBee=E5=82=B3=E9=80=81=E5=AD=97?= =?UTF-8?q?=E4=B8=B2=E8=A8=8A=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/coordinator_api.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/unitdev04/coordinator_api.py b/src/unitdev04/coordinator_api.py index a4c98bf..e29f0a3 100644 --- a/src/unitdev04/coordinator_api.py +++ b/src/unitdev04/coordinator_api.py @@ -35,7 +35,6 @@ def send_messages(): # Start the message sending thread send_thread = threading.Thread(target=send_messages, daemon=True) send_thread.start() - print("Broadcast chat mode activated. Type a message and press Enter to send.\n") # Keep the program running From f465b44b795a74a3b911a64207aa84b8e9ba27ef Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 00:43:03 +0800 Subject: [PATCH 033/146] =?UTF-8?q?API=20XBee=E5=82=B3=E9=80=81=E4=B8=A6?= =?UTF-8?q?=E6=8E=A5=E6=94=B6=E5=AD=97=E4=B8=B2=E8=A8=8A=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/coordinator_api.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/unitdev04/coordinator_api.py b/src/unitdev04/coordinator_api.py index e29f0a3..a4c98bf 100644 --- a/src/unitdev04/coordinator_api.py +++ b/src/unitdev04/coordinator_api.py @@ -35,6 +35,7 @@ def send_messages(): # Start the message sending thread send_thread = threading.Thread(target=send_messages, daemon=True) send_thread.start() + print("Broadcast chat mode activated. Type a message and press Enter to send.\n") # Keep the program running From 7073eaf3e6cf62b9aadd5733107ed11951efed38 Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 00:43:58 +0800 Subject: [PATCH 034/146] =?UTF-8?q?AT=20XBee=E5=82=B3=E9=80=81=E4=B8=A6?= =?UTF-8?q?=E6=8E=A5=E6=94=B6=E5=AD=97=E4=B8=B2=E8=A8=8A=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/endpoint_at.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitdev04/endpoint_at.py b/src/unitdev04/endpoint_at.py index b6e0bfb..21c53f4 100644 --- a/src/unitdev04/endpoint_at.py +++ b/src/unitdev04/endpoint_at.py @@ -38,4 +38,4 @@ try: pass except KeyboardInterrupt: print("\nProgram terminated.") - ser.close() + ser.close() \ No newline at end of file From b1109ad10200a017a9f892db96fe274b34164f0a Mon Sep 17 00:00:00 2001 From: lenting1027 Date: Tue, 15 Apr 2025 10:20:09 +0800 Subject: [PATCH 035/146] =?UTF-8?q?XBee=E8=A7=A3=E8=AE=80heartbeat?= =?UTF-8?q?=E8=B3=87=E8=A8=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev04/receive.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/unitdev04/receive.py b/src/unitdev04/receive.py index 9136d3c..eab6436 100644 --- a/src/unitdev04/receive.py +++ b/src/unitdev04/receive.py @@ -4,7 +4,7 @@ from pymavlink import mavutil import time # 配置 XBee 串口連接 -#ser = serial.Serial('COM5', 57600) # XBee 串口 +ser = serial.Serial('COM5', 57600) # XBee 串口 # 無人機系統 ID（假設為 5）和目標系統 ID（假設為 1） From c38f277db46a19d918f85fe68d24c2db05c7d3e8 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Apr 2025 22:57:49 +0800 Subject: [PATCH 036/146] 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 037/146] 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 038/146] 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 039/146] 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 0af71f8c8e9aa968f883b558d7bd2ec1a4250d02 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 18 Apr 2025 15:09:24 +0800 Subject: [PATCH 040/146] =?UTF-8?q?1.=20=E9=87=8D=E8=A6=81=E5=8F=83?= =?UTF-8?q?=E6=95=B8=E5=91=BD=E5=90=8D=E8=AE=8A=E6=9B=B4=202.=20mavlink=5F?= =?UTF-8?q?object=20=E5=BB=BA=E6=A7=8B=E5=BC=8F=E8=AE=8A=E6=9B=B4=203.=20?= =?UTF-8?q?=E6=8A=8A=E9=96=8B=E7=99=BC=E4=BB=A3=E7=A2=BC=E7=8D=A8=E7=AB=8B?= =?UTF-8?q?=E5=88=B0=E9=A1=8D=E5=A4=96=E6=AA=94=E6=A1=88=20devRun.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 166 ++++++++++++ .../fc_network_adapter/mavlinkDevice.py | 6 +- .../fc_network_adapter/mavlinkObject.py | 251 ++++-------------- 3 files changed, 229 insertions(+), 194 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py new file mode 100644 index 0000000..538938e --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -0,0 +1,166 @@ +# 基礎功能的 import +import queue +import time + +# ROS2 的 import +import rclpy + +# mavlink 的 import +from pymavlink import mavutil + +# 自定義的 import +import mavlinkObject as mo + +# ====================== 分割線 ===================== + +test_item = 3 +print('test_item : ', test_item) + +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 = mo.mavlink_object(mavlink_socket) + mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE + mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT + mavlink_object1.multiplexingToConvert = [[74,]] # only VFR_HUD + + # 做一個空的 queue list 驗證 multiplexingToConvert + mo.swap_queues.append(queue.Queue()) + # 啟動連線的模組 + mavlink_object1.run() + + # 運行幾秒並印出 queue 的資料 + start_time = time.time() + while time.time() - start_time < 2: + while not mo.fixed_stream_bridge_queue.empty(): + print('fixed_stream_bridge_queue:') + t = mo.fixed_stream_bridge_queue.get() + print('from {} : {}'.format(t[0],t[2])) + while not mo.return_packet_processor_queue.empty(): + print('return_packet_processor_queue:') + t = mo.return_packet_processor_queue.get() + # print('from {} : {}'.format(t[0],t[2])) + print(t[2].type) + + for q in mo.swap_queues: + while not q.empty(): + print('swap_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_bridge的thread區塊 的功能 + print('Start of Program .Test 2') + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + + mavlink_object2 = mo.mavlink_object(mavlink_socket) + mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT + mavlink_object2.multiplexingToReturn = [] # only HEARTBEAT + mavlink_object2.multiplexingToConvert = [] # + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + # 啟動連線的模組 + mavlink_object2.run() + + 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 analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + + + + # 結束程式 退出所有 thread + mavlink_object2.stop() + mavlink_object2.thread.join() + analyzer.stop() + + analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 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 = mo.mavlink_object(mavlink_socket) + mavlink_object3.multiplexingToAnalysis = [0] # only HEARTBEAT + mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT + mavlink_object3.multiplexingToConvert = [] # + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + # 關於 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]) + end_time = time.time() + print(f"Execution time for create_flightMode: {end_time - start_time} seconds") + + print("start emit info") + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < 10: + 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_socket.close() + print('End of Program .Test 3') diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index ba12465..cdb6768 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -1,9 +1,13 @@ - +# 自定義的 import from theLogger import setup_logger +# ====================== 分割線 ===================== + logger = setup_logger("mavlinkDevice.py") + + # 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component class mavlink_device(): def __init__(self): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 185bea2..741170f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,9 +1,9 @@ ''' # 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 queue stack 傳送到分析器 -# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 +# 匯流排接到訊息後透過 queue stack 傳送到橋接器 +# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流 # 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue -# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 +# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 # 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 @@ -21,21 +21,19 @@ from pymavlink import mavutil # ROS2 的 import from rclpy.node import Node -import std_msgs.msg # 自定義的 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() +fixed_stream_bridge_queue = queue.Queue() return_packet_processor_queue = queue.Queue() -converte_format_queues = [] +swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer # 用來記錄每個 system 的資訊 # 資料格式 { sysid : mavlink_device object } @@ -43,18 +41,18 @@ mavlink_systems = {} # ====================== 分割線 ===================== -class mavlink_analyzer(Node, mavlink_publisher): +class mavlink_bridge(Node, mavlink_publisher): ''' - 這個 class 就是 固定串流分析器 - 是用來接收 mavlink 訊息 並進行分析 - 這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的 + 這個 class 就是 固定串流橋接器 + 是用來接收 mavlink 訊息 並進行橋接 + 這個地方是針對 fixed_stream_bridge_queue 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object 並且看是否有重複 system id 整段代碼包含兩大區塊 thread 和 node - thread 區塊內會對 fixed_stream_analyzer_queue 進行監聽 並且將收到的訊息進行處理 + thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 @@ -69,7 +67,7 @@ class mavlink_analyzer(Node, mavlink_publisher): def __new__(cls, *args, **kwargs): with cls._lock: # 確保多線程環境下只有一個實例被創建 if cls._instance is None: - cls._instance = super(mavlink_analyzer, cls).__new__(cls) + cls._instance = super(mavlink_bridge, cls).__new__(cls) return cls._instance def __init__(self): @@ -85,7 +83,7 @@ class mavlink_analyzer(Node, mavlink_publisher): self.thread = threading.Thread(target=self._run_thread) self.thread.start() else: - logger.error('mavlink_analyzer instance already exists. Do not create another one.') + logger.error('mavlink_bridge instance already exists. Do not create another one.') def stop(self): self.running = False @@ -93,12 +91,12 @@ class mavlink_analyzer(Node, mavlink_publisher): # === 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 進行後處理 + logger.info('Start of mavlink_bridge._run_thread') + # 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 while self.running: - if fixed_stream_analyzer_queue.empty(): + if fixed_stream_bridge_queue.empty(): continue - msg_pack = fixed_stream_analyzer_queue.get() + msg_pack = fixed_stream_bridge_queue.get() msg = msg_pack[2] sysid = msg.get_srcSystem() @@ -132,12 +130,12 @@ class mavlink_analyzer(Node, mavlink_publisher): 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') + logger.info('End of mavlink_bridge._run_thread') # === Node 區塊 === def _init_node(self): - logger.info('Start of mavlink_analyzer._init_node') - super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒 + logger.info('Start of mavlink_bridge._init_node') + super().__init__('mavlink_bridge') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒 def emit_info(self): # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 @@ -163,14 +161,31 @@ class mavlink_object(): 每個 mavlink bus 都會有一個 mavlink_object 其中主要是 thread 做統計封包與分流 分流表的控制在三個 list 分別為 - multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的 + multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToConvert : 這個 list 是用來分流到轉格式的 + multiplexingToConvert : 這個 list 是用來分流到轉拋串流的 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 ''' - def __init__(self, mavlink_socket, socket_id = None): - self.socket_id = socket_id - self.mavlink_socket = mavlink_socket + mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } + def __new__(cls, *args, **kwargs): + # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 + # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 + # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號 + # socket_id 從 1 開始 + + instance = super().__new__(cls) + socket_id = 1 + for k in cls.mavlinkObjects.keys(): + if k == socket_id: + socket_id += 1 + else: + break + instance.socket_id = socket_id + cls.mavlinkObjects[socket_id] = instance + return instance + + def __init__(self, socket): + self.mavlink_socket = socket self.msg_count = {} self.multiplexingList = [] @@ -193,15 +208,14 @@ class mavlink_object(): ] self.multiplexingToConvert = [ ] + logger.info('mavlink_object instance {} created'.format(self.socket_id)) def __del__(self): self.mavlink_socket.close() self.stop() + self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance def run(self): - if not self.socket_id: - logger.error('Please set socket id before running') - return self.thread = threading.Thread(target=self._run_thread) self.running = self.updateMultiplexingList() self.thread.start() @@ -214,6 +228,8 @@ class mavlink_object(): start_time = time.time() while self.running: timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 + + # 處理接收到的封包 try: mavlinkMsg = self.mavlink_socket.recv_msg() except Exception as e: @@ -223,7 +239,7 @@ class mavlink_object(): self.running = False break - if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' + if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...' # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 sysid = mavlinkMsg.get_srcSystem() compid = mavlinkMsg.get_srcComponent() @@ -231,19 +247,25 @@ class mavlink_object(): # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) - # print(mavlinkMsg) # debug + # print(type(mavlinkMsg.pack)) # debug + # print("mark A") + # # print(dir(mavlinkMsg)) # debug + # print(mavlinkMsg.pack()) # 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,timestamp,mavlinkMsg)) + fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) elif i == 1: return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) else: - convert_queue = converte_format_queues[i-2] - convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) + _queue = swap_queues[i-2] + # _queue.put((self.socket_id,timestamp,mavlinkMsg)) # 測試看看 也許不需要別的資訊 只需要封包 + _queue.put(mavlinkMsg) + + # 處理要送出的封包 # thread 結束 logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) @@ -262,7 +284,7 @@ class mavlink_object(): logger.error('Multiplexing List Format Error, Please check the list') return False # 若有錯誤則回傳 False - check = len(self.multiplexingToConvert) != len(converte_format_queues) + check = len(self.multiplexingToConvert) != len(swap_queues) if check: logger.error('Multiplexing Queue not fit List , Please check the list') return False @@ -274,163 +296,6 @@ class mavlink_object(): def main_node(): pass -# ====================== 分割線 ===================== -# 測試時的程式進入點 -def main_develop(args=None): - - test_item = 2 - print('test_item : ', test_item) - - 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 - - # 做一個空的 queue list 驗證 multiplexingToConvert - converte_format_queues.append(queue.Queue()) - # 啟動連線的模組 - mavlink_object1.run() - - # 運行幾秒並印出 queue 的資料 - 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[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() - - 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] # only HEARTBEAT - 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]) - end_time = time.time() - print(f"Execution time for create_flightMode: {end_time - start_time} seconds") - - print("start emit info") - - 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 - - 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__': - import rclpy # 這邊是為了測試而加的 - main_develop() + pass From d8ef37fdc0981d906d5c0abe7a0359020b6085a2 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 29 Apr 2025 10:49:50 +0800 Subject: [PATCH 041/146] 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 042/146] 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 043/146] 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 044/146] 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 f79aaf86fa895962df5fc8fe9e8b9c4af3a33bed Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 7 May 2025 16:19:17 +0800 Subject: [PATCH 045/146] =?UTF-8?q?mavlink=5Fobject=20=E6=9E=B6=E6=A7=8B?= =?UTF-8?q?=E6=94=B9=E5=96=84=20=E6=96=B0=E5=A2=9E=E5=8A=9F=E8=83=BD=20?= =?UTF-8?q?=E8=BD=89=E6=8E=A5=E4=B8=8D=E5=90=8C=20socket=20=E7=9A=84?= =?UTF-8?q?=E6=95=B8=E6=93=9A=E6=B5=81=20=E6=9C=89=E6=99=82=E5=80=99?= =?UTF-8?q?=E4=B8=8D=E7=A9=A9=E5=AE=9A=20=E6=9C=83=E6=9C=89=E4=BA=9B?= =?UTF-8?q?=E5=B0=8F=E5=95=8F=E9=A1=8C=20=E4=B9=8B=E5=BE=8C=E5=86=8D?= =?UTF-8?q?=E6=94=B9=E5=96=84=20=E5=8F=A6=E5=A4=96=20devRun=20=E4=B9=9F?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=E4=BA=86=E5=B9=BE=E5=80=8B=E7=AF=84=E4=BE=8B?= =?UTF-8?q?=2020=E8=99=9F=E9=A0=85=20=E5=8F=AF=E4=BB=A5=E7=8D=A8=E7=AB=8B?= =?UTF-8?q?=E6=B8=AC=E8=A9=A6=20node=20=E5=8A=9F=E8=83=BD=2012=E8=99=9F?= =?UTF-8?q?=E9=A0=85=20=E5=B1=95=E7=A4=BA=E6=9C=80=E7=B0=A1=E5=96=AE?= =?UTF-8?q?=E7=9A=84=E8=BD=89=E6=8E=A5=E5=88=B0=20GCS=20=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 233 ++++++++++++++++-- .../fc_network_adapter/mavlinkDevice.py | 4 + .../fc_network_adapter/mavlinkObject.py | 141 ++++++++--- 3 files changed, 319 insertions(+), 59 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 538938e..21b68dc 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -13,29 +13,93 @@ import mavlinkObject as mo # ====================== 分割線 ===================== -test_item = 3 +test_item = 12 print('test_item : ', test_item) if test_item == 1: + # 測試 updateMultiplexingList 的輸出 + print('===> Start of Program .Test ', test_item) + mavlink_object_none = mo.mavlink_object(None) + + print('=====================') + print('正常範例') + mavlink_object_none.multiplexingToAnalysis = [] + mavlink_object_none.multiplexingToReturn = [] + # mavlink_object_none.multiplexingToSwap = [[]] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + print('multiplexingList : ', mavlink_object_none._multiplexingList) + + print('=====================') + print('錯誤範例 長度不一致') + mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + + print('=====================') + print('正常範例') + mo.swap_queues.append(queue.Queue()) + mo.swap_queues.append(queue.Queue()) + mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5] + mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10] + mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + print('multiplexingList : ', mavlink_object_none._multiplexingList) + + print('=====================') + print('錯誤範例 multiplexingToAnalysis 不可以有 -1') + mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + + print('End of Program') + + +elif test_item == 2: + # 測試 updateMultiplexingList 的輸出 + print('===> Start of Program .Test ', test_item) + mavlink_object_none1 = mo.mavlink_object(None) + mavlink_object_none2 = mo.mavlink_object(None) + mavlink_object_none3 = mo.mavlink_object(None) + del mavlink_object_none2 + + print(len(mo.swap_queues)) + + mavlink_object_none2 = mo.mavlink_object(None) + print(len(mo.swap_queues)) + + print(mavlink_object_none1._multiplexingList) + + print('End of Program') + + +elif test_item == 10: # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 # 只啟用了 mavlink_object 的功能 - print('Start of Program .Test 1') + print('===> Start of Program .Test ', test_item) + mavlink_object_none = mo.mavlink_object(None) + connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) mavlink_object1 = mo.mavlink_object(mavlink_socket) + + print(mavlink_object1.multiplexingToSwap) + print(mo.swap_queues) mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT - mavlink_object1.multiplexingToConvert = [[74,]] # only VFR_HUD + mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD + # mavlink_object1.multiplexingToSwap[0] = [-1, ] # all - # 做一個空的 queue list 驗證 multiplexingToConvert - mo.swap_queues.append(queue.Queue()) # 啟動連線的模組 mavlink_object1.run() + print(mavlink_object1._multiplexingList) + # 運行幾秒並印出 queue 的資料 start_time = time.time() - while time.time() - start_time < 2: + while time.time() - start_time < 3: while not mo.fixed_stream_bridge_queue.empty(): print('fixed_stream_bridge_queue:') t = mo.fixed_stream_bridge_queue.get() @@ -43,32 +107,35 @@ if test_item == 1: while not mo.return_packet_processor_queue.empty(): print('return_packet_processor_queue:') t = mo.return_packet_processor_queue.get() - # print('from {} : {}'.format(t[0],t[2])) - print(t[2].type) + print('from {} : {}'.format(t[0],t[2])) + # print(t[2].get_msgbuf()) + # print(t[2].type) - for q in mo.swap_queues: + for n in range(len(mo.swap_queues)): + q = mo.swap_queues[n] while not q.empty(): print('swap_queues:') t = q.get() - print('from {} : {}'.format(t[0],t[2])) + print('{} gets : {}'.format(n,t)) + # 結束程式 退出所有 thread mavlink_object1.running = False mavlink_object1.thread.join() mavlink_socket.close() - print('End of Program .Test 1') + print('End of Program') -elif test_item == 2: + +elif test_item == 11: # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 # 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能 - print('Start of Program .Test 2') + print('===> Start of Program .Test ', test_item) connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) mavlink_object2 = mo.mavlink_object(mavlink_socket) mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT - mavlink_object2.multiplexingToReturn = [] # only HEARTBEAT - mavlink_object2.multiplexingToConvert = [] # + mavlink_object2.multiplexingToReturn = [] # 啟動 mavlink_bridge analyzer = mo.mavlink_bridge() @@ -105,11 +172,137 @@ elif test_item == 2: analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance analyzer.thread.join() mavlink_socket.close() - print('End of Program .Test 2') + print('End of Program') + +elif test_item == 12: + # 我這邊會測試 mavlink object 作為交換器的功能 + print('===> Start of Program .Test ', test_item) + + # 初始化兩個通道 + 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] # only HEARTBEAT + + connection_string_out="udpout:127.0.0.1:14553" + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200) + + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + mavlink_object_out.multiplexingToAnalysis = [0] + + + # 讓兩個通道互相傳輸 + mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all + mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + + # 做一個空的通道驗證 + mavlink_object_none = mo.mavlink_object(None) + # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all + + # 啟動連線的模組 + analyzer = mo.mavlink_bridge() + + # 啟動通道 + mavlink_object_in.run() + mavlink_object_out.run() + + start_time = time.time() + show_time = time.time() + print("connection established!") + + print(type(mavlink_socket_out)) + print(type(mavlink_socket_out.mav)) + + while time.time() - start_time < 10: + try: + test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) + print('none object : ', test) + except queue.Empty: + pass + + if (time.time() - show_time) >= 2: + show_time = time.time() + for sysid in analyzer.mavlink_systems: + for compid in analyzer.mavlink_systems[sysid].components: + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + + + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + print("===================") + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + + # 結束程式 退出所有 thread + mavlink_object_in.stop() + mavlink_object_in.thread.join() + mavlink_object_out.stop() + mavlink_object_out.thread.join() + mavlink_socket_in.close() + mavlink_socket_out.close() + + analyzer.stop() + print('End of Program') + +elif test_item == 20: + sysid = 1 + compid = 1 + + # 這邊測試 node 生成 topic 的功能 + rclpy.init() # 注意要初始化 rclpy 才能使用 node + # mavlink_object_none = mo.mavlink_object(None) + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + from pymavlink.dialects.v20 import common as mavlink2 + + mav = mavlink2.MAVLink(None) + mav.srcSystem = sysid + mav.srcComponent = compid + + # 這是一個 heartbeat 封包 + fake_heartbeat = mavlink2.MAVLink_heartbeat_message( + type=mavlink2.MAV_TYPE_QUADROTOR, + autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode=3, + custom_mode=0, + system_status=0, + mavlink_version=3 + ) + mavlink_bytes = fake_heartbeat.pack(mav) + fake_heartbeat_msg = mav.parse_char(mavlink_bytes) + + print(analyzer.mavlink_systems) + + mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data + time.sleep(0.1) + print(analyzer.mavlink_systems) + + analyzer._init_node() + + analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid + print("topic created") + time.sleep(5) + + analyzer.emit_info() + + # 結束程式 + analyzer.running = False + analyzer.destroy_node() + + rclpy.shutdown() + analyzer.stop() + analyzer.thread.join() + print('End of Program') + -elif test_item == 3: +elif test_item == 21: # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - print('Start of Program .Test 3') + print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) @@ -117,7 +310,7 @@ elif test_item == 3: mavlink_object3 = mo.mavlink_object(mavlink_socket) mavlink_object3.multiplexingToAnalysis = [0] # only HEARTBEAT mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT - mavlink_object3.multiplexingToConvert = [] # + mavlink_object3.multiplexingToSwap = [] # # 啟動 mavlink_bridge analyzer = mo.mavlink_bridge() @@ -163,4 +356,4 @@ elif test_item == 3: analyzer.thread.join() mavlink_socket.close() - print('End of Program .Test 3') + print('End of Program') diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index cdb6768..d8c6e04 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -6,7 +6,11 @@ from theLogger import setup_logger logger = setup_logger("mavlinkDevice.py") +# 用來記錄每個 system 的資訊 +# 資料格式 { sysid : mavlink_device object } +mavlink_systems = {} +# ====================== 分割線 ===================== # 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component class mavlink_device(): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 741170f..7ecfb02 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -23,7 +23,7 @@ from pymavlink import mavutil from rclpy.node import Node # 自定義的 import -from mavlinkDevice import mavlink_device +from mavlinkDevice import mavlink_device, mavlink_systems from mavlinkPublish import mavlink_publisher from theLogger import setup_logger @@ -35,10 +35,6 @@ fixed_stream_bridge_queue = queue.Queue() return_packet_processor_queue = queue.Queue() swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer -# 用來記錄每個 system 的資訊 -# 資料格式 { sysid : mavlink_device object } -mavlink_systems = {} - # ====================== 分割線 ===================== class mavlink_bridge(Node, mavlink_publisher): @@ -163,31 +159,39 @@ class mavlink_object(): 分流表的控制在三個 list 分別為 multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToConvert : 這個 list 是用來分流到轉拋串流的 + multiplexingToSwap : 這個 list 是用來分流到轉拋串流的 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 ''' mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } + socket_num = 0 # 用來記錄目前的 socket 數量 def __new__(cls, *args, **kwargs): # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號 - # socket_id 從 1 開始 + # socket_id 從 0 開始 instance = super().__new__(cls) - socket_id = 1 + socket_id = 0 for k in cls.mavlinkObjects.keys(): if k == socket_id: socket_id += 1 else: break instance.socket_id = socket_id + cls.socket_num += 1 cls.mavlinkObjects[socket_id] = instance return instance def __init__(self, socket): self.mavlink_socket = socket - self.msg_count = {} - self.multiplexingList = [] + # 這邊變數是執行的時候被使用的 不要直接寫入它 + self._multiplexingList = [] + # 存放要發送的訊息的 queue 或稱 buffer + self.output_buffer = queue.Queue() + if self.socket_id >= len(swap_queues): + swap_queues.append(self.output_buffer) + else: + swap_queues[self.socket_id] = self.output_buffer # 關聯到全域變數 global mavlink_systems @@ -195,27 +199,57 @@ class mavlink_object(): # 這三個 list 用來分配不同的訊息到不同的 queue self.multiplexingToAnalysis = [ - 0, # HEARTBEAT - 24, # GPS_RAW_INT - 30, # ATTITUDE - 33, # GLOBAL_POSITION_INT - 74, # VFR_HUD - 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [ - 0, # HEARTBEAT - # 30, # ATTITUDE + 0, # HEARTBEAT # 挺必要的項目 + # 24, # GPS_RAW_INT + # 30, # ATTITUDE + # 33, # GLOBAL_POSITION_INT + # 74, # VFR_HUD + # 147, # BATTERY_STATUS ] - self.multiplexingToConvert = [ + self.multiplexingToReturn = [] + self.multiplexingToSwap = [ + [] for _ in range(len(swap_queues)) ] + + # 刷新其他 mavlink_object 的 multiplexingToSwap + for k, object in self.mavlinkObjects.items(): + if (k != self.socket_id) and (len(object.multiplexingToSwap) <= self.socket_id): + object.multiplexingToSwap.append([]) + object.updateMultiplexingList() + logger.info('mavlink_object instance {} created'.format(self.socket_id)) def __del__(self): - self.mavlink_socket.close() - self.stop() + # 停下自己的 thread + if self.mavlink_socket != None: + self.mavlink_socket.close() + self.stop() + + # 移除其他 mavlink_object 的 multiplexingToSwap + for k, object in self.mavlinkObjects.items(): + if (k != self.socket_id) and (len(object.multiplexingToSwap) > self.socket_id): + object.multiplexingToSwap[self.socket_id] = [] + object.updateMultiplexingList() + + # 移除自己的 swap_queues + swap_queues[self.socket_id] = None + + # 處理 class 的 instance 記錄 + self.socket_num -= 1 self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance + # logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替 + print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # try: + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # except Exception as e: + # print(f"Error logging in __del__: {e}") + # if 'logger' in globals() and logger: + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + def run(self): + # TODO 檢查 socket 是否有連線 self.thread = threading.Thread(target=self._run_thread) self.running = self.updateMultiplexingList() self.thread.start() @@ -247,15 +281,13 @@ class mavlink_object(): # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) - # print(type(mavlinkMsg.pack)) # debug - # print("mark A") - # # print(dir(mavlinkMsg)) # debug - # print(mavlinkMsg.pack()) # debug - # break # Debug + # break # Debug function can put here you can see the input data from mavlink # 將訊息依照 multiplexing list 分發到不同的 queue - for i in range(len(self.multiplexingList)): - if mavlinkMsg.get_msgId() in self.multiplexingList[i]: + for i in range(len(self._multiplexingList)): + if (self._multiplexingList[i] == []): + continue + elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1): if i == 0: fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) elif i == 1: @@ -266,6 +298,20 @@ class mavlink_object(): _queue.put(mavlinkMsg) # 處理要送出的封包 + # 如果 有資料在 output_buffer 中則將其取出並發送 + # 沒有就略過發送 + + try: + mavlinkMsg_send = self.output_buffer.get(block=False) + except queue.Empty: + mavlinkMsg_send = None + # except Exception as e: + # logger.error(f"Error getting data from output_buffer: {e}") + # mavlinkMsg_send = None + + if mavlinkMsg_send: + # self.mavlink_socket.mav.send(mavlinkMsg_send) + self.mavlink_socket.write(mavlinkMsg_send.get_msgbuf()) # 這邊會將封包寫入 socket 中 # thread 結束 logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) @@ -274,20 +320,37 @@ class mavlink_object(): ''' 更新 multiplexing list 並做簡單的檢查 ''' + # 檢查 multiplexingToAnalysis 與 multiplexingToReturn 是否有 -1 值 + check = (-1 in self.multiplexingToAnalysis) or (-1 in self.multiplexingToReturn) + if check: + logger.error('MultiplexingToAnalysis or MultiplexingToReturn NOT all type spilt. socket id : {}'.format(self.socket_id)) + return False - # 更新 multiplexing list - self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + # 檢查 multiplexingToSwap 與 swap_queues 的長度是否一致 而且 swap_queues 的長度不能為 0 + check = len(self.multiplexingToSwap) != len(swap_queues) or len(swap_queues) == 0 + if check: + logger.error('Multiplexing Queue not fit List , Please check the list. socket id : {}'.format(self.socket_id)) + return False + + # 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋 + try: + self.multiplexingToSwap[self.socket_id] = [] + except IndexError: + logger.error('Multiplexing List of self socket id should be void. socket id : {}'.format(self.socket_id)) + return False - # 檢查 multiplexing list 格式是否有錯誤 - check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) + # 組合 multiplexing list + multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap + + # 檢查 multiplexing list 格式是否有錯誤 # 全部都要是 list 每個 list 裡面都要是 int + check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in multiL_tmp) if not check: - logger.error('Multiplexing List Format Error, Please check the list') + logger.error('Multiplexing List Format Error, Please check the list. socket id : {}'.format(self.socket_id)) return False # 若有錯誤則回傳 False - check = len(self.multiplexingToConvert) != len(swap_queues) - if check: - logger.error('Multiplexing Queue not fit List , Please check the list') - return False + # 更新 multiplexing list + self._multiplexingList = multiL_tmp + return True # ====================== 分割線 ===================== From 4d694f28b41451c4919fe1a51555e27e0c9685af Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 7 May 2025 23:45:14 +0800 Subject: [PATCH 046/146] 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" From 84119b788e6e48020912453a45264794e7a26128 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 14 May 2025 09:51:54 +0800 Subject: [PATCH 047/146] =?UTF-8?q?=E6=B8=AC=E8=A9=A6=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E6=95=B4=E7=90=86=20=E6=9C=89bug=E4=BF=AE=E6=AD=A3=E4=B8=AD=20?= =?UTF-8?q?=E8=AC=9B=E8=A7=A3=E7=A8=8B=E5=BC=8F=E5=85=88=20commit=20?= =?UTF-8?q?=E4=B8=80=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 113 ++++++++++++++++-- .../fc_network_adapter/mavlinkObject.py | 5 +- src/unitdev02/unitdev02/client.py | 14 +++ src/unitdev02/unitdev02/lifecycle_node.py | 45 +++++++ src/unitdev02/unitdev02/server.py | 33 +++++ 5 files changed, 197 insertions(+), 13 deletions(-) create mode 100644 src/unitdev02/unitdev02/client.py create mode 100644 src/unitdev02/unitdev02/lifecycle_node.py create mode 100644 src/unitdev02/unitdev02/server.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 21b68dc..6736ae7 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -13,7 +13,7 @@ import mavlinkObject as mo # ====================== 分割線 ===================== -test_item = 12 +test_item = 22 print('test_item : ', test_item) if test_item == 1: @@ -214,7 +214,7 @@ elif test_item == 12: print(type(mavlink_socket_out)) print(type(mavlink_socket_out.mav)) - while time.time() - start_time < 10: + while time.time() - start_time < 100: try: test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) print('none object : ', test) @@ -304,27 +304,28 @@ elif test_item == 21: # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - - mavlink_object3 = mo.mavlink_object(mavlink_socket) - mavlink_object3.multiplexingToAnalysis = [0] # only HEARTBEAT - mavlink_object3.multiplexingToReturn = [] # only HEARTBEAT - mavlink_object3.multiplexingToSwap = [] # # 啟動 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_socket = mavutil.mavlink_connection(connection_string) + mavlink_object3 = mo.mavlink_object(mavlink_socket) + + # 啟動通道 mavlink_object3.run() print('waiting for mavlink data ...') time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) compid = 1 sysid = 1 @@ -341,7 +342,7 @@ elif test_item == 21: try: # rclpy.spin(analyzer) analyzer.emit_info() # 這邊是測試 node 的運行 - time.sleep(0.5) + time.sleep(1) except KeyboardInterrupt: break @@ -357,3 +358,91 @@ elif test_item == 21: mavlink_socket.close() print('End of Program') + + +elif test_item == 22: + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + 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_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) + + connection_string_out="udpout:127.0.0.1:14553" + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + + mavlink_object_none = mo.mavlink_object(None) + + # 讓兩個通道互相傳輸 + mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all + mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + mavlink_object_in.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] + + # 啟動通道 + mavlink_object_in.run() + mavlink_object_out.run() + + print('waiting for mavlink data ...') + time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + compid = 1 + sysid = 1 + show_time = time.time() + analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) + print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") + + print("start emit info") + + start_time = time.time() + show_time = time.time() + show_time2 = time.time() + + while time.time() - start_time < 100: + if (time.time() - show_time2) >= 1: + analyzer.emit_info() # 這邊是測試 node 的運行 + # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] + fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] + sss = mavutil.mode_string_v10(fmsg) + print("目前的飛行模式 : ", sss) + show_time2 = time.time() + + # if (time.time() - show_time) >= 2: + # show_time = time.time() + # for sysid in analyzer.mavlink_systems: + # for compid in analyzer.mavlink_systems[sysid].components: + # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + # print("===================") + + time.sleep(1) + + analyzer.destroy_node() + rclpy.shutdown() + + + # 結束程式 退出所有 thread + mavlink_object_in.stop() + mavlink_object_in.thread.join() + mavlink_socket_in.close() + mavlink_object_out.stop() + mavlink_object_out.thread.join() + mavlink_socket_out.close() + analyzer.stop() + analyzer.thread.join() + + + print('End of Program') \ 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 7ecfb02..e7223b0 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -115,6 +115,9 @@ class mavlink_bridge(Node, mavlink_publisher): this_component.emitParams['base_mode'] = msg.base_mode this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg) + # print("get mode :", mavutil.mode_string_v10(msg)) # debug + # print("record mode :", this_component.emitParams['flightMode_mode']) # debug + this_component.emitParams['flightMode'] = msg # debug elif msg.get_msgId() == 147: # BATTERY_STATUS 處理 this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()] @@ -239,7 +242,7 @@ class mavlink_object(): self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance # logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替 - print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug + # print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) # try: # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) diff --git a/src/unitdev02/unitdev02/client.py b/src/unitdev02/unitdev02/client.py new file mode 100644 index 0000000..ce00e92 --- /dev/null +++ b/src/unitdev02/unitdev02/client.py @@ -0,0 +1,14 @@ +import socket + +SOCKET_PATH = "/tmp/unix_socket_example" + +# 建立 socket +client = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) +client.connect(SOCKET_PATH) + +# 傳送資料 +client.sendall(b"Hello from client!") +data = client.recv(1024) +print("📤 Server replied:", data.decode()) + +client.close() diff --git a/src/unitdev02/unitdev02/lifecycle_node.py b/src/unitdev02/unitdev02/lifecycle_node.py new file mode 100644 index 0000000..bebc82e --- /dev/null +++ b/src/unitdev02/unitdev02/lifecycle_node.py @@ -0,0 +1,45 @@ + + + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +import time + +# import mavros_msgs.srv + +class TalkerNode(Node): + def __init__(self): + start_time = time.time() + super().__init__('talker_node') + end_time = time.time() + print(f"Node initialization took {end_time - start_time:.2f} seconds") + + self.publisher_ = self.create_publisher(String, 'hahatest/_1', 10) + self.timer = self.create_timer(1.0, self.timer_callback) # 每秒執行一次 + self.get_logger().info('TalkerNode has been started.') + + def timer_callback(self): + msg = String() + msg.data = 'Hello, ROS 2!' + self.publisher_.publish(msg) + self.get_logger().info(f'Published: "{msg.data}"') + +def main(args=None): + rclpy.init(args=args) + node = TalkerNode() + + print("Before sleep") + time.sleep(5) # 等待 5 秒鐘 + print("After sleep") + try: + start_time = time.time() + while time.time() - start_time < 10: # 持續 10 秒鐘 + rclpy.spin_once(node) + time.sleep(1) # 每秒執行一次 + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/unitdev02/unitdev02/server.py b/src/unitdev02/unitdev02/server.py new file mode 100644 index 0000000..148edc5 --- /dev/null +++ b/src/unitdev02/unitdev02/server.py @@ -0,0 +1,33 @@ +import socket +import os + +# socket file path +SOCKET_PATH = "/tmp/unix_socket_example" + +# 若檔案存在就先刪除 +if os.path.exists(SOCKET_PATH): + os.remove(SOCKET_PATH) + +# 建立 socket +server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + +# 綁定 socket 到檔案 +server.bind(SOCKET_PATH) +server.listen(1) + +print("🔌 Server waiting for connection...") + +# 等待 client 連線 +conn, _ = server.accept() +print("✅ Client connected.") + +while True: + data = conn.recv(1024) + if not data: + break + print("📥 Received:", data.decode()) + conn.sendall(b"Echo: " + data) + +conn.close() +server.close() +os.remove(SOCKET_PATH) From f9080a5015b3bbd2cd1ae6ee24736e205a5cff52 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 16 May 2025 22:37:40 +0800 Subject: [PATCH 048/146] =?UTF-8?q?1.=20=E4=BF=AE=E6=94=B9=20mavlinkObject?= =?UTF-8?q?.py=20=E7=84=A1=E6=B3=95=E9=A0=86=E5=88=A9=E8=AD=98=E5=88=A5?= =?UTF-8?q?=E5=A4=9A=E5=8F=B0=20vehicle=20=E7=9A=84=E9=87=8D=E5=A4=A7bug?= =?UTF-8?q?=202.=20=E9=87=8D=E6=96=B0=E7=B7=A8=E5=AF=AB=20devRun.py=20?= =?UTF-8?q?=E8=AE=93=E5=85=B6=E6=9B=B4=E5=A5=BD=E9=96=B1=E8=AE=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 151 ++++++++++-------- .../fc_network_adapter/mavlinkDevice.py | 5 +- .../fc_network_adapter/mavlinkObject.py | 43 +++-- src/unitdev02/unitdev02/test01.py | 3 + 4 files changed, 116 insertions(+), 86 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 6736ae7..b0e83e2 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -10,14 +10,16 @@ from pymavlink import mavutil # 自定義的 import import mavlinkObject as mo +import mavlinkDevice as md # ====================== 分割線 ===================== test_item = 22 +running_time = 20 print('test_item : ', test_item) if test_item == 1: - # 測試 updateMultiplexingList 的輸出 + # 測試 mavlink_object 中 updateMultiplexingList 的輸出 print('===> Start of Program .Test ', test_item) mavlink_object_none = mo.mavlink_object(None) @@ -55,9 +57,9 @@ if test_item == 1: print('End of Program') - elif test_item == 2: - # 測試 updateMultiplexingList 的輸出 + # 測試 mavlink_object 創建時 socket_id 是否正確 + # 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試 print('===> Start of Program .Test ', test_item) mavlink_object_none1 = mo.mavlink_object(None) mavlink_object_none2 = mo.mavlink_object(None) @@ -73,33 +75,36 @@ elif test_item == 2: print('End of Program') - elif test_item == 10: + # 需要開啟一個 ardupilot 的模擬器 # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 # 只啟用了 mavlink_object 的功能 print('===> Start of Program .Test ', test_item) + + # 創建一個空的通道 這個通道的 socket_id 是 0 mavlink_object_none = mo.mavlink_object(None) + # 創建另一個通道 connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object1 = mo.mavlink_object(mavlink_socket) - - print(mavlink_object1.multiplexingToSwap) - print(mo.swap_queues) + # 設定通道轉發的參數 mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD - # mavlink_object1.multiplexingToSwap[0] = [-1, ] # all - # 啟動連線的模組 + # print(mavlink_object1.multiplexingToSwap) + # print(mo.swap_queues) + + # 啟動通道 mavlink_object1.run() - print(mavlink_object1._multiplexingList) + # 確認轉拋的設定有沒有錯 + print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList) # 運行幾秒並印出 queue 的資料 start_time = time.time() - while time.time() - start_time < 3: + while time.time() - start_time < running_time: while not mo.fixed_stream_bridge_queue.empty(): print('fixed_stream_bridge_queue:') t = mo.fixed_stream_bridge_queue.get() @@ -125,28 +130,38 @@ elif test_item == 10: mavlink_socket.close() print('End of Program') - elif test_item == 11: + # 需要開啟一個 ardupilot 的模擬器 # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 # 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能 print('===> Start of Program .Test ', test_item) - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - - mavlink_object2 = mo.mavlink_object(mavlink_socket) - mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT - mavlink_object2.multiplexingToReturn = [] # 啟動 mavlink_bridge analyzer = mo.mavlink_bridge() + # 創建通道 + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object2 = mo.mavlink_object(mavlink_socket) + # mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT + # mavlink_object2.multiplexingToReturn = [] + # 啟動連線的模組 mavlink_object2.run() + # 做點延遲 讓 heartbeat 先吃進來 + time.sleep(2) + print("=== connection established! ===") + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + start_time = time.time() show_time = time.time() compid = 1 - while time.time() - start_time < 5: + while time.time() - start_time < running_time: if (time.time() - show_time) >= 1: show_time = time.time() for sysid in analyzer.mavlink_systems: @@ -156,14 +171,6 @@ elif test_item == 11: analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) print("===================") - # 印出目前所有 mavlink_systems 的內容 - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - - - # 結束程式 退出所有 thread mavlink_object2.stop() mavlink_object2.thread.join() @@ -175,46 +182,51 @@ elif test_item == 11: print('End of Program') elif test_item == 12: - # 我這邊會測試 mavlink object 作為交換器的功能 + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試 mavlink object 作為交換器功能的代碼 print('===> Start of Program .Test ', test_item) - # 初始化兩個通道 + # 啟動連線的模組 + analyzer = mo.mavlink_bridge() + + # 初始化輸入通道 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] # only HEARTBEAT + # 初始化輸出通道 connection_string_out="udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200) - + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) mavlink_object_out = mo.mavlink_object(mavlink_socket_out) mavlink_object_out.multiplexingToAnalysis = [0] + # 做一個空的通道驗證 可以拿來 debug + mavlink_object_none = mo.mavlink_object(None) # 讓兩個通道互相傳輸 mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - - # 做一個空的通道驗證 - mavlink_object_none = mo.mavlink_object(None) # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - # 啟動通道 mavlink_object_in.run() mavlink_object_out.run() - start_time = time.time() - show_time = time.time() - print("connection established!") + # 做點延遲 讓 heartbeat 先吃進來 + time.sleep(3) + print("=== connection established! ===") + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) - print(type(mavlink_socket_out)) - print(type(mavlink_socket_out.mav)) + # print(type(mavlink_socket_out)) + # print(type(mavlink_socket_out.mav)) - while time.time() - start_time < 100: + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: try: test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) print('none object : ', test) @@ -226,14 +238,9 @@ elif test_item == 12: for sysid in analyzer.mavlink_systems: for compid in analyzer.mavlink_systems[sysid].components: print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) print("===================") - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) # 結束程式 退出所有 thread @@ -248,18 +255,19 @@ elif test_item == 12: print('End of Program') elif test_item == 20: - sysid = 1 - compid = 1 - # 這邊測試 node 生成 topic 的功能 + # 利用 空的通道 發出假的 heartbeat 封包 + print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node - # mavlink_object_none = mo.mavlink_object(None) + + from pymavlink.dialects.v20 import common as mavlink2 + + sysid = 1 + compid = 1 # 啟動 mavlink_bridge analyzer = mo.mavlink_bridge() - from pymavlink.dialects.v20 import common as mavlink2 - mav = mavlink2.MAVLink(None) mav.srcSystem = sysid mav.srcComponent = compid @@ -282,12 +290,15 @@ elif test_item == 20: time.sleep(0.1) print(analyzer.mavlink_systems) + # ROS2 初始化 analyzer._init_node() + # 創建 ROS2 topic analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid print("topic created") time.sleep(5) + # 丟出 topic analyzer.emit_info() # 結束程式 @@ -299,9 +310,10 @@ elif test_item == 20: analyzer.thread.join() print('End of Program') - elif test_item == 21: + # 需要開啟一個 ardupilot 的模擬器 # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node @@ -316,11 +328,11 @@ elif test_item == 21: connection_string="udp:127.0.0.1:15551" mavlink_socket = mavutil.mavlink_connection(connection_string) mavlink_object3 = mo.mavlink_object(mavlink_socket) - # 啟動通道 mavlink_object3.run() - print('waiting for mavlink data ...') + + print('=== waiting for mavlink data ...') time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 print('目前所有的系統 : ') @@ -338,7 +350,7 @@ elif test_item == 21: start_time = time.time() show_time = time.time() - while time.time() - start_time < 10: + while time.time() - start_time < running_time: try: # rclpy.spin(analyzer) analyzer.emit_info() # 這邊是測試 node 的運行 @@ -346,9 +358,10 @@ elif test_item == 21: except KeyboardInterrupt: break + + # 程式結束 analyzer.destroy_node() - rclpy.shutdown() - + rclpy.shutdown() # 結束程式 退出所有 thread mavlink_object3.stop() @@ -359,8 +372,8 @@ elif test_item == 21: mavlink_socket.close() print('End of Program') - elif test_item == 22: + # 需要開啟一個 ardupilot 的模擬器 與 GCS # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node @@ -377,16 +390,17 @@ elif test_item == 22: mavlink_socket_in = mavutil.mavlink_connection(connection_string_in) mavlink_object_in = mo.mavlink_object(mavlink_socket_in) + connection_string_out="udpout:127.0.0.1:14553" mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + mavlink_object_out.multiplexingToAnalysis = [] + - mavlink_object_none = mo.mavlink_object(None) # 讓兩個通道互相傳輸 mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - mavlink_object_in.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # 啟動通道 mavlink_object_in.run() @@ -411,13 +425,14 @@ elif test_item == 22: show_time = time.time() show_time2 = time.time() - while time.time() - start_time < 100: + while time.time() - start_time < running_time: if (time.time() - show_time2) >= 1: analyzer.emit_info() # 這邊是測試 node 的運行 # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] sss = mavutil.mode_string_v10(fmsg) - print("目前的飛行模式 : ", sss) + # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) + print("目前的飛行模式 : {}".format(sss)) show_time2 = time.time() # if (time.time() - show_time) >= 2: @@ -428,7 +443,7 @@ elif test_item == 22: # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) # print("===================") - time.sleep(1) + analyzer.destroy_node() rclpy.shutdown() diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index d8c6e04..f090b41 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -20,14 +20,15 @@ class mavlink_device(): self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object def __str__(self): - p_str = '' + p_str = '=====================\n' + p_str += f'object id : {id(self)}\n' # debug p_str += f'socket_id : {self.socket_id}\n' p_str += f'sysid : {self.sysid}\n' p_str += 'has components : \n' for compid in self.components: p_str += f'compid : {compid}\n' p_str += f'mav_type : {self.components[compid].mav_type}\n' - p_str += '=====================\n' + # p_str += '=====================\n' return p_str def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index e7223b0..9130a15 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -55,7 +55,6 @@ class mavlink_bridge(Node, mavlink_publisher): 藉由控制 ros2 的機制再把 device object 的資訊發送出去 ps. 我限制了這個 class 只能有一個 instance - ''' _instance = None _lock = threading.Lock() # 確保多線程安全 @@ -96,28 +95,41 @@ class mavlink_bridge(Node, mavlink_publisher): msg = msg_pack[2] sysid = msg.get_srcSystem() + compid = msg.get_srcComponent() + + # 若這個 system id 還不存在 則建立 device object + if not sysid in self.mavlink_systems: + this_device = mavlink_device() # 創建一個新的 device object + self.mavlink_systems[sysid] = this_device + this_device.socket_id = msg_pack[0] + this_device.sysid = sysid + else: + this_device = self.mavlink_systems[sysid] + + # 若該 component id 存在 則直接使用該 component object + # 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object + # 若該 component id 不存在 又不是 heartbeat 則不處理 + if compid in self.mavlink_systems[sysid].components: + this_component = self.mavlink_systems[sysid].components[compid] + elif msg.get_msgId() == 0: + # 只有透過 heartbeat 可以創建一個新的 component object + this_component = this_device.mavlink_component() + this_device.components[msg.get_srcComponent()] = this_component + this_component.mav_type = msg.type + this_component.mav_autopilot = msg.autopilot + else: + continue # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ if msg.get_msgId() == 0: # HEARTBEAT 處理 - - # 若這個 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 - this_component.mav_autopilot = msg.autopilot - this_component.emitParams['base_mode'] = msg.base_mode this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg) + this_component.emitParams['flightMode'] = msg # debug + + # print("mav_type : ", msg.type) # debug # print("get mode :", mavutil.mode_string_v10(msg)) # debug # print("record mode :", this_component.emitParams['flightMode_mode']) # debug - this_component.emitParams['flightMode'] = msg # debug elif msg.get_msgId() == 147: # BATTERY_STATUS 處理 this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()] @@ -303,7 +315,6 @@ class mavlink_object(): # 處理要送出的封包 # 如果 有資料在 output_buffer 中則將其取出並發送 # 沒有就略過發送 - try: mavlinkMsg_send = self.output_buffer.get(block=False) except queue.Empty: diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index e00d946..5beda2e 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -257,3 +257,6 @@ print('Start') simple_getMavlink() + + +# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200) From a25372979b855b1a14918431ed2175cde1eec0fb Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Sat, 17 May 2025 16:33:33 +0800 Subject: [PATCH 049/146] =?UTF-8?q?=E6=95=B4=E5=90=88=E5=BE=8C=E9=A0=85?= =?UTF-8?q?=E7=9B=AE=E4=BF=AE=E6=AD=A3=201.=20mavlinkObject.py=20=E7=B5=B1?= =?UTF-8?q?=E5=90=88=20msg=5Fid=20=E5=8F=83=E6=95=B8=E5=90=8D=E7=A8=B1=202?= =?UTF-8?q?.=20mavlinkDevice.py=20=E7=A7=BB=E9=99=A4=E5=9C=A8=E9=A0=90?= =?UTF-8?q?=E6=9C=9F=E6=93=8D=E4=BD=9C=E4=B8=8B=20=E4=BB=8D=E6=9C=83?= =?UTF-8?q?=E5=A4=A7=E9=87=8F=E8=B7=B3=E9=8C=AF=E8=AA=A4=E8=A8=8A=E6=81=AF?= =?UTF-8?q?=E7=9A=84=E8=AD=A6=E7=A4=BA=203.=20devRun.py=20=E9=87=8D?= =?UTF-8?q?=E6=96=B0=E9=A9=97=E8=AD=89=20merge=20=E9=A0=85=E7=9B=AE?= =?UTF-8?q?=E8=88=87=E7=89=88=E6=9C=AC=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 14 +-- .../fc_network_adapter/devRun.py | 88 ++++++++++++++++--- .../fc_network_adapter/mavlinkDevice.py | 4 +- .../fc_network_adapter/mavlinkObject.py | 7 +- 4 files changed, 92 insertions(+), 21 deletions(-) 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') From e4f0b2dc938127d733c6183d08307ad4c251307a Mon Sep 17 00:00:00 2001 From: wenchun Date: Mon, 19 May 2025 12:21:06 +0800 Subject: [PATCH 050/146] =?UTF-8?q?Matlab=E8=88=87Gazebo=E7=9A=84=E9=96=89?= =?UTF-8?q?=E7=92=B0=E6=A8=A1=E6=93=AC=E6=89=80=E9=9C=80=E7=A8=8B=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/unitdev03/0519/close_loop_multi_1.py | 297 ++++++++++ src/unitdev03/0519/enemy_status.py | 340 ++++++++++++ src/unitdev03/0519/test_transform.py | 663 +++++++++++++++++++++++ 3 files changed, 1300 insertions(+) create mode 100644 src/unitdev03/0519/close_loop_multi_1.py create mode 100644 src/unitdev03/0519/enemy_status.py create mode 100644 src/unitdev03/0519/test_transform.py diff --git a/src/unitdev03/0519/close_loop_multi_1.py b/src/unitdev03/0519/close_loop_multi_1.py new file mode 100644 index 0000000..5542372 --- /dev/null +++ b/src/unitdev03/0519/close_loop_multi_1.py @@ -0,0 +1,297 @@ +#!/usr/bin/env python3 +from pymavlink import mavutil +import socket +import re +import time +import math +import threading +import json +import os +import datetime +import queue +import argparse +from select import select + +class Drone: + def __init__(self, instance_id, mavlink_port, udp_port): + self.instance_id = instance_id + self.mavlink_port = mavlink_port + self.udp_port = udp_port + self.running = True + self.connection = None + + # 創建數據緩存 + self.latest_position = None + + # 創建日誌佇列 + self.log_queue = queue.Queue() + + # 創建日誌文件目錄 + self.log_dir = "drone_logs" + if not os.path.exists(self.log_dir): + os.makedirs(self.log_dir) + # 創建一個含時間戳的日誌文件名 + timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") + self.log_file = os.path.join(self.log_dir, f"drone_{self.instance_id}_{timestamp}.log") + + # 通訊統計 + self.packet_counter = 0 + self.start_time = time.time() + self.packets_per_second = 0 + + def connect(self): + """僅建立MAVLink連接""" + print(f"Drone {self.instance_id}: 建立MAVLink連接 (Port {self.mavlink_port})...") + connection_string = f"udp:127.0.0.1:{self.mavlink_port}" + self.connection = mavutil.mavlink_connection(connection_string) + self.connection.wait_heartbeat() + print(f"Drone {self.instance_id}: MAVLink連接已建立") + + def set_guided_mode(self): + """切換到GUIDED模式""" + self.connection.mav.set_mode_send( + self.connection.target_system, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 4 # GUIDED模式ID + ) + print(f"Drone {self.instance_id}: 已切換到GUIDED模式") + + def arm(self): + """解鎖無人機""" + self.connection.mav.command_long_send( + self.connection.target_system, + self.connection.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1, 0, 0, 0, 0, 0, 0 # 1表示解鎖 + ) + print(f"Drone {self.instance_id}: 無人機已解鎖") + + def takeoff(self, altitude=3.0): + """起飛到指定高度""" + self.connection.mav.command_long_send( + self.connection.target_system, + self.connection.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, 0, 0, 0, 0, 0, 0, altitude + ) + print(f"Drone {self.instance_id}: 起飛命令已發送，目標高度{altitude}公尺") + + def mavlink_listener(self): + """事件驅動方式監聽MAVLink訊息""" + # 獲取底層socket + mavlink_socket = self.connection.port.fileno() + + while self.running: + # 使用select等待數據，無須sleep + readable, _, _ = select([mavlink_socket], [], [], 0.001) + if readable: + msg = self.connection.recv_msg() + if msg and msg.get_type() == 'LOCAL_POSITION_NED': + self.latest_position = msg + + def log_worker(self): + """事件驅動方式處理日誌寫入""" + while self.running: + if not self.log_queue.empty(): + try: + # 批量處理日誌 + batch = [] + while not self.log_queue.empty() and len(batch) < 100: + try: + entry = self.log_queue.get_nowait() + batch.append(entry) + except queue.Empty: + break + + # 寫入日誌 + if batch: + with open(self.log_file, 'a', encoding='utf-8') as f: + for entry in batch: + f.write(json.dumps(entry, ensure_ascii=False) + "\n") + except Exception as e: + print(f"Drone {self.instance_id}: 日誌寫入錯誤: {e}") + + # 每秒計算一次統計數據 + now = time.time() + if now - self.start_time >= 1.0: + self.packets_per_second = self.packet_counter + print(f"Drone {self.instance_id} - 每秒封包數: {self.packets_per_second}") + self.packet_counter = 0 + self.start_time = now + + # 使用select等待短暫時間而非sleep + select([], [], [], 0.01) # 10ms的微小等待 + + def log_packet(self, packet_data, packet_num): + """將數據加入日誌佇列""" + try: + timestamp = datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3] + log_entry = { + "timestamp": timestamp, + "packet_number": packet_num, + "data": packet_data + } + self.log_queue.put(log_entry) + except Exception as e: + print(f"Drone {self.instance_id}: 佇列添加錯誤: {e}") + + def run(self): + """使用事件驅動方式處理UDP通訊""" + # 啟動背景線程 + mavlink_thread = threading.Thread(target=self.mavlink_listener) + mavlink_thread.daemon = True + mavlink_thread.start() + + log_thread = threading.Thread(target=self.log_worker) + log_thread.daemon = True + log_thread.start() + + # 設置UDP服務器 + server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 262144) # 增大接收緩衝區 + server_socket.bind(('0.0.0.0', self.udp_port)) + server_socket.setblocking(0) # 非阻塞模式 + + print(f"Drone {self.instance_id}: UDP服務器已啟動，監聽端口{self.udp_port}") + print(f"Drone {self.instance_id}: 日誌將保存至 {self.log_file}") + + # 主事件循環 + while self.running: + # 使用select監聽UDP + readable, _, _ = select([server_socket], [], [], 0.001) + + if readable: + try: + data, _ = server_socket.recvfrom(4096) + decoded_data = data.decode('utf-8') + + # 增加計數器 + self.packet_counter += 1 + total_packets = self.packet_counter + + try: + # 解析JSON + json_data = json.loads(decoded_data) + + # 寫入日誌 + self.log_packet(json_data, total_packets) + + # 提取速度數據 + vel_match = re.search(r'"next_velocity":\[([-\d\.\,]+)\]', decoded_data) + if vel_match: + velocity_str = vel_match.group(1) + velocity = [float(x) for x in velocity_str.split(',')] + vx, vy, vz = velocity + + # 設置type_mask - 僅使用速度控制 + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE + ) + + # 直接發送速度指令，不需等待位置數據 + self.connection.mav.set_position_target_local_ned_send( + 0, + self.connection.target_system, + self.connection.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask, + 0, 0, 0, # 位置 + vx, vy, -vz, # 速度 + 0, 0, 0, # 加速度 + 0, 0 # 航向 + ) + print(f"Drone {self.instance_id}: 發送速度指令: vx={vx}, vy={vy}, vz={-vz}") + + except json.JSONDecodeError as je: + print(f"Drone {self.instance_id}: JSON解析錯誤: {je}") + self.log_packet({"raw_data": decoded_data}, total_packets) + + except Exception as e: + print(f"Drone {self.instance_id}: 處理錯誤: {e}") + + # 清理資源 + server_socket.close() + print(f"Drone {self.instance_id}: 線程結束") + +def main(): + # 解析命令行參數 + parser = argparse.ArgumentParser(description='多無人機控制程序') + parser.add_argument('--drones', type=str, default='1,2,3', + help='要控制的無人機ID，用逗號分隔 (例如: 1,2,3)') + args = parser.parse_args() + + # 所有可用的無人機配置 + all_drone_configs = [ + (1, 14550, 5003), + (2, 14560, 5013), + (3, 14570, 5023) + ] + + # 解析要使用的無人機ID + drone_ids = [int(id_str) for id_str in args.drones.split(',')] + drone_configs = [config for config in all_drone_configs if config[0] in drone_ids] + + drones = [] + threads = [] + + try: + # 創建所有無人機實例 + for instance_id, mavlink_port, udp_port in drone_configs: + drone = Drone(instance_id, mavlink_port, udp_port) + drones.append(drone) + + # 步驟1: 所有無人機連接 + for drone in drones: + drone.connect() + + # 適當延遲確保連接穩定 + time.sleep(1) + + # 步驟2: 所有無人機同時切換到GUIDED模式 + for drone in drones: + drone.set_guided_mode() + + time.sleep(1) + + # 步驟3: 所有無人機同時解鎖 + for drone in drones: + drone.arm() + + time.sleep(1) + + # 步驟4: 所有無人機同時起飛 + for drone in drones: + drone.takeoff() + + # 啟動監聽線程 + for drone in drones: + thread = threading.Thread(target=drone.run) + thread.daemon = True + threads.append(thread) + thread.start() + + print(f"已啟動 {len(drones)} 台無人機 (ID: {args.drones})，按Ctrl+C結束程序") + + # 主線程事件循環 + while True: + select([], [], [], 0.1) # 使用select代替time.sleep + + except KeyboardInterrupt: + print("正在停止所有無人機...") + for drone in drones: + drone.running = False + + for thread in threads: + thread.join(timeout=2) + + print("程序已結束") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/unitdev03/0519/enemy_status.py b/src/unitdev03/0519/enemy_status.py new file mode 100644 index 0000000..c4b96a6 --- /dev/null +++ b/src/unitdev03/0519/enemy_status.py @@ -0,0 +1,340 @@ +#!/usr/bin/env python3 + +from pymavlink import mavutil +import time +import sys +import json +import socket +import threading +import os +from datetime import datetime + +# 固定初始位置設定 (5,5,0) +INITIAL_POSITION = [5, 5, 0] + +# MAVLink 連接設定 +connection_string = 'udp:127.0.0.1:14553' + +# 網路傳輸設定 +target_ip = '140.120.31.130' +target_port = 9053 + +# 建立 UDP socket 用於傳送數據 +sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + +# 封包編號計數器 +packet_counter = 0 + +# 共享變數 - 存儲最新位置 +current_position = None +initial_position = INITIAL_POSITION.copy() +position_lock = threading.Lock() + +# 連接到無人機 +print(f"嘗試連接到 MAVLink: {connection_string}") +try: + connection = mavutil.mavlink_connection(connection_string, autoreconnect=True) + connection.wait_heartbeat() + print(f"成功連接到系統 {connection.target_system}!") +except Exception as e: + print(f"MAVLink 連接失敗: {e}") + sys.exit(1) + +print(f"使用指定初始位置: 北={initial_position[0]}, 東={initial_position[1]}, 下={initial_position[2]}") + +# 請求位置數據流 +connection.mav.request_data_stream_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_DATA_STREAM_POSITION, + 10, + 1 +) + +# 日誌記錄函數 +def start_logging(): + """創建並開啟日誌文件""" + log_dir = "logs" + if not os.path.exists(log_dir): + os.makedirs(log_dir) + + timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") + log_file = os.path.join(log_dir, f"drone_log_{timestamp}.csv") + + file = open(log_file, "w") + file.write("packet_id,timestamp,x,y,z,vx,vy,vz\n") # CSV標頭 + print(f"日誌記錄已開始: {log_file}") + return file + +# 控制函數 +def set_guided_mode(): + """設置為 GUIDED 模式""" + connection.mav.command_long_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_CMD_DO_SET_MODE, + 0, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 4, + 0, 0, 0, 0, 0 + ) + ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3) + return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED + +def arm_throttle(): + """解鎖油門""" + connection.mav.command_long_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1, 0, 0, 0, 0, 0, 0 + ) + ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3) + return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED + +def takeoff(altitude): + """起飛到指定高度""" + connection.mav.command_long_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, 0, 0, 0, 0, 0, 0, altitude + ) + ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3) + return ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED + +def set_speed_limit(speed_type, speed): + """ + 設置速度限制 + + Parameters: + ----------- + speed_type : int + 速度類型 (0=空速, 1=地速, 2=爬升速度, 3=下降速度) + speed : float + 速度值 (米/秒) + """ + connection.mav.command_long_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + 0, # 確認 + speed_type, # 參數1: 速度類型 + speed, # 參數2: 速度 (m/s) + -1, # 參數3: 油門 (-1 表示不變) + 0, 0, 0, 0 # 參數4-7: 未使用 + ) + ack = connection.recv_match(type='COMMAND_ACK', blocking=True, timeout=3) + success = ack and ack.result == mavutil.mavlink.MAV_RESULT_ACCEPTED + if success: + print(f"已設置速度限制為 {speed:.2f} m/s") + else: + print("設置速度限制失敗") + return success + +def goto_position(north, east, down): + """前往指定位置 (世界座標系)""" + # 將世界座標轉換為相對座標 + rel_north = north - initial_position[0] + rel_east = east - initial_position[1] + rel_down = down - initial_position[2] + + connection.mav.set_position_target_local_ned_send( + 0, + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + 0b0000111111111000, + rel_north, rel_east, rel_down, + 0, 0, 0, + 0, 0, 0, + 0, 0 + ) + # print(f"相對位置: 北={rel_north:.2f}, 東={rel_east:.2f}, 下={rel_down:.2f}") + +def get_current_position(): + """獲取存儲的當前位置""" + with position_lock: + if current_position is None: + return None + return current_position.copy() + +def control_interface(): + """命令行控制界面""" + print("無人機控制介面已啟動，輸入命令：") + print(" arm - 解鎖油門") + print(" takeoff [高度] - 起飛") + print(" goto [北] [東] [高度] - 前往位置 (世界座標系)") + print(" pos - 顯示當前位置 (相對座標和世界座標)") + print(" init - 顯示初始位置") + print(" exit - 退出控制") + + while True: + try: + cmd = input("命令> ").strip().split() + if not cmd: + continue + + if cmd[0] == "arm": + print("嘗試解鎖油門...") + if set_guided_mode() and arm_throttle(): + print("成功解鎖油門！") + else: + print("解鎖失敗") + + elif cmd[0] == "takeoff": + alt = float(cmd[1]) if len(cmd) > 1 else 3.0 + print(f"嘗試起飛到 {alt} 米高...") + if takeoff(alt): + print("起飛命令已發送") + else: + print("起飛失敗") + + elif cmd[0] == "goto": + if len(cmd) >= 4: + north = float(cmd[1]) + east = float(cmd[2]) + down = -float(cmd[3]) # 高度是負的下 + print(f"前往世界位置: 北:{north} 東:{east} 下:{-down} 米") + goto_position(north, east, down) + else: + print("用法: goto [北] [東] [高度]") + + elif cmd[0] == "pos": + pos = get_current_position() + if pos: + rel_x, rel_y, rel_z = pos + # 計算世界座標 + world_x = initial_position[0] + rel_x + world_y = initial_position[1] + rel_y + world_z = initial_position[2] + rel_z + # NED 轉 ENU + enu_x = rel_y + enu_y = rel_x + enu_z = -rel_z + + print("\n當前位置:") + #print(f" 相對座標 (NED): 北={rel_x:.2f}m, 東={rel_y:.2f}m, 下={rel_z:.2f}m") + print(f" 世界座標 : 北={world_x:.2f}m, 東={world_y:.2f}m, 下={-world_z:.2f}m") + #print(f" 相對座標 (ENU): 東={enu_x:.2f}m, 北={enu_y:.2f}m, 上={enu_z:.2f}m") + #print(f" 高度: {-rel_z:.2f}m") + else: + print("尚未獲取到位置信息") + + elif cmd[0] == "speed": + if len(cmd) > 1: + speed = float(cmd[1]) + set_speed_limit(1, speed) # 1 = 地速 + else: + print("用法: speed [速度m/s]") + + elif cmd[0] == "climbspeed": + if len(cmd) > 1: + speed = float(cmd[1]) + set_speed_limit(2, speed) # 2 = 爬升速度 + set_speed_limit(3, speed) # 3 = 下降速度 + else: + print("用法: climbspeed [速度m/s]") + + elif cmd[0] == "init": + print(f"初始位置 (NED): 北={initial_position[0]:.2f}m, 東={initial_position[1]:.2f}m, 下={initial_position[2]:.2f}m") + + elif cmd[0] == "exit": + print("退出控制介面") + break + + else: + print(f"未知命令: {cmd[0]}") + except Exception as e: + print(f"命令執行錯誤: {e}") + +# 數據傳輸線程函數 +def data_sending_thread(): + global packet_counter, current_position + + # 開啟日誌文件 + log_file = start_logging() + + try: + print(f"開始接收數據並傳送到 {target_ip}:{target_port}...") + while True: + msg = connection.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1) + if msg is not None: + # 更新當前位置（線程安全） + with position_lock: + current_position = [msg.x, msg.y, msg.z] + + # 增加封包編號 + packet_counter += 1 + + # 計算世界座標 + world_x = initial_position[0] + msg.x + world_y = initial_position[1] + msg.y + world_z = initial_position[2] + msg.z + + # NED 轉換為 ENU 並使用世界座標 + enu_position = { + "x": world_x, # 東 (從 NED 的 x) + "y": world_y, # 北 (從 NED 的 y) + "z": -world_z # 上 (從 NED 的 -z) + } + + enu_velocity = { + "vx": msg.vy, # 東向速度 + "vy": msg.vx, # 北向速度 + "vz": -msg.vz # 上向速度 + } + + # 創建JSON對象 (使用世界座標) + json_data = { + "packet_id": packet_counter, + "timestamp": msg.time_boot_ms, + "enemy_position": enu_position, + "enemy_velocity": enu_velocity + } + + # 寫入日誌文件 (CSV格式) + try: + log_file.write(f"{packet_counter},{msg.time_boot_ms},{enu_position['x']},{enu_position['y']},{enu_position['z']},{enu_velocity['vx']},{enu_velocity['vy']},{enu_velocity['vz']}\n") + log_file.flush() # 確保數據即時寫入文件 + except Exception as e: + print(f"寫入日誌失敗: {e}") + + json_str = json.dumps(json_data) + + try: + sock.sendto(json_str.encode('utf-8'), (target_ip, target_port)) + except Exception as e: + print(f"傳送數據失敗: {e}") + + time.sleep(0.1) + except Exception as e: + print(f"數據傳輸線程錯誤: {e}") + finally: + # 確保關閉日誌文件 + log_file.close() + print("日誌文件已關閉") + +# 創建並啟動數據發送線程 +sender_thread = threading.Thread(target=data_sending_thread) +sender_thread.daemon = True +sender_thread.start() + +# 啟動命令行控制界面 +try: + control_interface() +except KeyboardInterrupt: + print("\n程序已被用戶中斷") +finally: + if connection: + connection.mav.request_data_stream_send( + connection.target_system, + connection.target_component, + mavutil.mavlink.MAV_DATA_STREAM_ALL, + 0, 0 + ) + print("數據流已停止") + sock.close() + print("網絡連接已關閉") + print(f"總共傳送了 {packet_counter} 個封包") + print("日誌文件已儲存") \ No newline at end of file diff --git a/src/unitdev03/0519/test_transform.py b/src/unitdev03/0519/test_transform.py new file mode 100644 index 0000000..96b22be --- /dev/null +++ b/src/unitdev03/0519/test_transform.py @@ -0,0 +1,663 @@ +import rclpy +from pymavlink import mavutil +from time import sleep +import time + +import socket +import struct + +# used at fdm_switch_example_two +import threading +import queue + +import json + +def mavlink_analyzer_simple(count = 500): + # rclpy.init() + # node = rclpy.create_node('mavlink_analyzer_simple') + + print('Inintializing connection') + + connection_string="udp:127.0.0.1:14550" + connection = mavutil.mavlink_connection(connection_string) + + print('Catch messages') + msg_count = {} + + start_time = time.time() + + for _ in range(count): + msg = connection.recv_msg() + # msg = connection.recv_match(blocking=True) + if msg: + msg_type = msg.get_type() + if msg_type in msg_count: + msg_count[msg_type] += 1 + else: + msg_count[msg_type] = 1 + + if msg.get_type() == 'SERVO_OUTPUT_RAW': + print(msg) + pass + + else: + print('No message yet, 1 second for data to fill') + sleep(1) + + print('Messages Type received:') + print(msg_count) + + end_time = time.time() + print('Time elapsed: ', end_time - start_time) + + print('Closing connection') + connection.close() + + +def fdm_parser_example(data=None): + if data is None: + data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000') + # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000 + + + parse_format = 'HHI16H' + + if len(data) != struct.calcsize(parse_format): + print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format))) + exit(1) + + decoded = struct.unpack(parse_format,data) + + magic = decoded[0] + frame_rate_hz = decoded[1] + frame_count = decoded[2] + pwm = decoded[3:] + + + print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm)) + +def fdm_switch_example_one(): + + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 沒有轉換數據格式 + ''' + # make a link get fdm from SITL, as a server + sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock_s2g.bind(('', 9012)) + sock_s2g.settimeout(0.1) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19012) + + packet_count = 0 + start_time = time.time() + + while True: + try: + # 接到SITL的數據 並轉發給Gazebo + data_s2g, addr_s2g = sock_s2g.recvfrom(1024) + sock_g2s.sendto(data_s2g, server_address) + # 得到Gazebo的回應 並轉發給SITL + data_g2s, addr_g2s = sock_g2s.recvfrom(1024) + sock_s2g.sendto(data_g2s, addr_s2g) + + # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo) + try: + # 解析從 SITL 到 Gazebo 的數據 + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # 解析從 Gazebo 到 SITL 的數據 + # 注意：假設從 Gazebo 返回的數據是字串形式的 JSON + try: + g2s_data_str = data_g2s.decode('utf-8') + # 保存原始數據，適合後續分析 + with open('gazebo_response_raw.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + # 如果不是 UTF-8 格式，可能是二進制數據 + g2s_data_str = str(data_g2s) + + # 合併數據到一個 JSON 對象 + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time() + } + + # 寫入 JSON 檔案 + with open('fdm_one_data.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + except Exception as e: + print(f"JSON 處理錯誤: {e}") + + packet_count += 1 + except socket.timeout: + print(f'no value') + #print(f'address:{addr_s2g}') + #pass + + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + #print(f"Packets received in the last second: {packet_count}") + print(f'JSON Pack:{data_g2s}') + packet_count = 0 + start_time = current_time + fdm_parser_example(data_s2g) + + +def fdm_switch_example_two(): + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 有轉換數據格式 + + time_usec + servo1_raw + servo16_raw + ''' + # read info from SITL via MAVLink + connection_string="udp:127.0.0.1:14550" # uart mavlink # MATLAB + connection = mavutil.mavlink_connection(connection_string) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19012) + + data_queue = queue.Queue() + servo_out = [0] * 16 + data_queue.put(servo_out) + + def send_data_from_queue(sock, server_address, q, frame_rate_fdm=600, frame_count_fdm=1): + interval = 1.0 / frame_rate_fdm + while True: + start_time = time.time() + try: + data = q.get(timeout=0.1) + if data is None: + break + last_data = data + except queue.Empty: + data = last_data + data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out) + sock.sendto(data, server_address) + + # 新增：寫入 JSON 檔案 + data_json = { + "magic": 0x481a, + "frame_rate_hz": frame_rate_fdm, + "frame_count": frame_count_fdm, + "pwm": list(servo_out) + } + with open('fdm_data.json', 'w') as f: + json.dump(data_json, f) + + frame_count_fdm += 1 + fdm_parser_example(data) # debug + + elapsed_time = time.time() - start_time + sleep_time = interval - elapsed_time + if sleep_time > 0: + sleep(sleep_time) + + Running = True + thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue)) + thread.start() + + while Running: + msgb1 = None + msg = connection.recv_msg() + while msg : + if msg.get_type() == 'SERVO_OUTPUT_RAW': + msgb1 = msg + # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值 + msg = connection.recv_msg() + + if msgb1: + for i in range(16): + servo_out[i] = getattr(msgb1, f'servo{i+1}_raw') + data_queue.put(servo_out) + msgb1 = None + # Running = False # debug + else: + # print('No message yet, 10 ms for data to fill') + sleep(0.01) + + # Example of putting data into the queue + # data_queue.put(data) + # To stop the thread, you can put None into the queue + # data_queue.put(None) + + +def fdm_switch_example_one_with_remote_forwarding(): + ''' + 這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 + 並且將JSON數據轉發到指定的遠端IP + ''' + # make a link get fdm from SITL, as a server + sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock_s2g.bind(('', 9012)) + sock_s2g.settimeout(0.1) + + # set a target link pass JSON to Gazebo, as a client + sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + server_address = ('127.0.0.1', 19012) + + # 建立與遠端IP的socket連接 + remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + remote_address = ('140.120.31.130', 9003) # 遠端IP與埠號，您可以根據需要更改埠號 + print(f'Setting up forwarding to remote address: {remote_address}') + + packet_count = 0 + start_time = time.time() + + while True: + try: + # 接到SITL的數據 並轉發給Gazebo + data_s2g, addr_s2g = sock_s2g.recvfrom(1024) + sock_g2s.sendto(data_s2g, server_address) + + # 得到Gazebo的回應 並轉發給SITL + data_g2s, addr_g2s = sock_g2s.recvfrom(1024) + sock_s2g.sendto(data_g2s, addr_s2g) + + # 解析並保存接收到的數據到 JSON 檔案 (SITL -> Gazebo) + try: + # 解析從 SITL 到 Gazebo 的數據 + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # 解析從 Gazebo 到 SITL 的數據 + try: + g2s_data_str = data_g2s.decode('utf-8') + # 保存原始數據，適合後續分析 + with open('gazebo_response_raw.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + # 如果不是 UTF-8 格式，可能是二進制數據 + g2s_data_str = str(data_g2s) + + # 合併數據到一個 JSON 對象 + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time() + } + + # 寫入 JSON 檔案 + with open('fdm_one_data.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + # 將JSON數據轉發到遠端IP + remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) + + except Exception as e: + print(f"JSON 處理錯誤: {e}") + + packet_count += 1 + except socket.timeout: + print(f'no value') + #pass + + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + print(f'JSON Pack:{data_g2s}') + print(f'Forwarded data to remote IP: 140.120.31.130') + packet_count = 0 + start_time = current_time + fdm_parser_example(data_s2g) + + +def fdm_switch_multiple(num_systems=3): + ''' + Create multiple UDP forwarders between SITL and Gazebo using threading + Each forwarder uses its own set of ports following the pattern: + - SITL ports: 9002, 9012, 9022, ... + - Gazebo ports: 19002, 19012, 19022, ... + + Parameters: + num_systems (int): Number of forwarding pairs to create + ''' + # Flag to control thread execution + running = True + + def forwarder_thread(system_id): + sitl_port = 9002 + (system_id * 10) + gazebo_port = 19002 + (system_id * 10) + + # Create SITL socket (server) + sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sitl_socket.bind(('', sitl_port)) + sitl_socket.settimeout(0.1) + + # Create Gazebo socket (client) + gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + gazebo_socket.settimeout(0.1) + gazebo_address = ('127.0.0.1', gazebo_port) + + print(f"System {system_id}: Forwarding {sitl_port} → {gazebo_port}") + + packet_count = 0 + start_time = time.time() + last_data_s2g = None + + try: + while running: + try: + # Receive from SITL and forward to Gazebo + data_s2g, addr_s2g = sitl_socket.recvfrom(1024) + last_data_s2g = data_s2g + gazebo_socket.sendto(data_s2g, gazebo_address) + + try: + # Get response from Gazebo and send back to SITL + data_g2s, addr_g2s = gazebo_socket.recvfrom(1024) + sitl_socket.sendto(data_g2s, addr_s2g) + + # Process and store data + try: + # Parse SITL to Gazebo data + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # Try to parse Gazebo to SITL data + try: + g2s_data_str = data_g2s.decode('utf-8') + if packet_count % 50 == 0: + with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + g2s_data_str = str(data_g2s) + + # Combine data + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time(), + "system_id": system_id, + "sitl_port": sitl_port, + "gazebo_port": gazebo_port + } + + # Save to JSON file (once per second) + current_time = time.time() + elapsed_time = current_time - start_time + if elapsed_time >= 1.0: + with open(f'fdm_data_system_{system_id}.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + except Exception as e: + print(f"System {system_id} - JSON processing error: {e}") + + packet_count += 1 + + except socket.timeout: + # No response from Gazebo, continue + pass + + except socket.timeout: + # No data from SITL, continue + pass + + # Print stats periodically + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + # print(f"System {system_id} - Packets: {packet_count} in the last second") + if packet_count > 0 and last_data_s2g: + print(f'System {system_id} - JSON Pack: {data_g2s}') + fdm_parser_example(last_data_s2g) + packet_count = 0 + start_time = current_time + + except Exception as e: + print(f"System {system_id} - Error in forwarder thread: {e}") + finally: + # Clean up resources + sitl_socket.close() + gazebo_socket.close() + print(f"System {system_id} - Forwarder thread terminated") + + # Create and start threads for each system + threads = [] + for i in range(num_systems): + thread = threading.Thread(target=forwarder_thread, args=(i,)) + thread.daemon = True + threads.append(thread) + thread.start() + + # Keep the main thread running + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\nShutting down all forwarders...") + running = False + time.sleep(1) + print("Done.") + return + + +def fdm_switch_multiple_with_remote(num_systems=3, remote_ip='140.120.31.130'): + ''' + Create multiple UDP forwarders between SITL and Gazebo using threading + Also forwards the combined data to a remote IP address + + Each forwarder uses its own set of ports following the pattern: + - SITL ports: 9002, 9012, 9022, ... + - Gazebo ports: 19002, 19012, 19022, ... + - Remote ports: 9003, 9013, 9023, ... (for the remote forwarding) + + Parameters: + num_systems (int): Number of forwarding pairs to create + remote_ip (str): IP address for remote forwarding + ''' + # Flag to control thread execution + running = True + + # Add shared counters for total packets + total_packets_sent = 0 + total_packets_lock = threading.Lock() # For thread safety + + def forwarder_thread(system_id): + nonlocal total_packets_sent + sitl_port = 9002 + (system_id * 10) + gazebo_port = 19002 + (system_id * 10) + remote_port = 9003 + (system_id * 10) + + # Create SITL socket (server) + sitl_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sitl_socket.bind(('', sitl_port)) + sitl_socket.settimeout(0.1) + + # Create Gazebo socket (client) + gazebo_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + gazebo_socket.settimeout(0.1) + gazebo_address = ('127.0.0.1', gazebo_port) + + # Create remote forwarding socket + remote_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + remote_address = (remote_ip, remote_port) + + print(f"System {system_id}: SITL:{sitl_port} → Gazebo:{gazebo_port}, Remote:{remote_ip}:{remote_port}") + + packet_count = 0 + start_time = time.time() + last_data_s2g = None + forward_counter = 0 # 添加轉發計數器 + system_packets_sent = 0 # Local counter for this system + + try: + while running: + try: + # Receive from SITL and forward to Gazebo + data_s2g, addr_s2g = sitl_socket.recvfrom(1024) + last_data_s2g = data_s2g + gazebo_socket.sendto(data_s2g, gazebo_address) + + try: + # Get response from Gazebo and send back to SITL + data_g2s, addr_g2s = gazebo_socket.recvfrom(1024) + sitl_socket.sendto(data_g2s, addr_s2g) + + # Process and store data + try: + # Parse SITL to Gazebo data + decoded = struct.unpack('HHI16H', data_s2g) + s2g_json = { + "magic": decoded[0], + "frame_rate_hz": decoded[1], + "frame_count": decoded[2], + "pwm": list(decoded[3:]) + } + + # Try to parse Gazebo to SITL data + try: + g2s_data_str = data_g2s.decode('utf-8') + if packet_count % 50 == 0: + with open(f'gazebo_response_raw_{system_id}.txt', 'a') as f: + f.write(g2s_data_str + '\n') + except: + g2s_data_str = str(data_g2s) + + # Combine data + combined_data = { + "sitl_to_gazebo": s2g_json, + "gazebo_to_sitl": g2s_data_str, + "timestamp": time.time(), + "system_id": system_id, + "sitl_port": sitl_port, + "gazebo_port": gazebo_port + } + + ''' + # 每n筆才轉發一次 + forward_counter += 1 + if forward_counter >= 18: + # Forward data to remote server + remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) + #print(f"System {system_id} - Send data to remote (第{packet_count}個包)") + forward_counter = 0 # 重置計數器 + ''' + + # 使用模運算代替計數器重置 + forward_counter += 1 + if packet_count % 1 == 0: + # Forward data to remote server + remote_socket.sendto(json.dumps(combined_data).encode(), remote_address) + system_packets_sent += 1 + with total_packets_lock: + total_packets_sent += 1 + # Debug用，看每秒傳了多少筆出去 + #print(f"System {system_id} - Send data to remote (第{packet_count}個包)") + + # Save to JSON file (once per second) + current_time = time.time() + elapsed_time = current_time - start_time + if elapsed_time >= 1.0: + with open(f'fdm_data_system_{system_id}.json', 'w') as f: + json.dump(combined_data, f, indent=2) + + except Exception as e: + print(f"System {system_id} - JSON processing error: {e}") + + packet_count += 1 + + except socket.timeout: + # No response from Gazebo, continue + pass + + except socket.timeout: + # No data from SITL, continue + pass + + # Print stats periodically + current_time = time.time() + elapsed_time = current_time - start_time + + if elapsed_time >= 1.0: + print(f"System {system_id} - Packets: {packet_count} in the last second") + if packet_count > 0 and last_data_s2g: + fdm_parser_example(last_data_s2g) + packet_count = 0 + start_time = current_time + + except Exception as e: + print(f"System {system_id} - Error in forwarder thread: {e}") + finally: + # Clean up resources + sitl_socket.close() + gazebo_socket.close() + remote_socket.close() + print(f"System {system_id} - Forwarder thread terminated") + + # Create statistics display thread + def stats_display_thread(): + start_time = time.time() + while running: + time.sleep(5) # Display stats every 5 seconds + elapsed = time.time() - start_time + with total_packets_lock: + rate = total_packets_sent / elapsed if elapsed > 0 else 0 + print(f"\n===== STATISTICS =====") + print(f"Total packets sent: {total_packets_sent}") + print(f"Average rate: {rate:.2f} packets/sec") + print(f"Running time: {elapsed:.1f} seconds") + print(f"======================\n") + + # Create and start threads for each system + threads = [] + for i in range(num_systems): + thread = threading.Thread(target=forwarder_thread, args=(i,)) + thread.daemon = True + threads.append(thread) + thread.start() + + # Start stats display thread + stats_thread = threading.Thread(target=stats_display_thread) + stats_thread.daemon = True + stats_thread.start() + + # Keep the main thread running + try: + while True: + time.sleep(1) + except KeyboardInterrupt: + print("\nShutting down all forwarders...") + running = False + time.sleep(1) + print(f"Final total packets sent: {total_packets_sent}") + print("Done.") + return + + +print('Start') +#fdm_switch_example_two() +#mavlink_analyzer_simple() +#fdm_switch_example_one() +#fdm_switch_example_one_with_remote_forwarding() +#fdm_switch_multiple(1) # Create 3 forwarding pairs +fdm_switch_multiple_with_remote(3) # Create 1 forwarding pair with remote forwarding +#fdm_parser_example() \ No newline at end of file From b49a8b0314a0e7e487a2e7a3a85fac6377887e77 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:46:33 +0800 Subject: [PATCH 051/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkObject.py | 419 ++++++++---------- 1 file changed, 179 insertions(+), 240 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index d3f2214..0a219be 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,16 +1,15 @@ ''' # 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 queue stack 傳送到分析器 -# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 +# 匯流排接到訊息後透過 queue stack 傳送到橋接器 +# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流 # 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue -# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 +# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 # 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 # 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function ''' - # 基礎功能的 import import threading import queue @@ -23,44 +22,38 @@ from pymavlink import mavutil from rclpy.node import Node # 自定義的 import -from mavlinkDevice import mavlink_device +from mavlinkDevice import mavlink_device, mavlink_systems from mavlinkPublish import mavlink_publisher from theLogger import setup_logger - # ====================== 分割線 ===================== logger = setup_logger("mavlinkObject.py") -fixed_stream_analyzer_queue = queue.Queue() +fixed_stream_bridge_queue = queue.Queue() return_packet_processor_queue = queue.Queue() -converte_format_queues = [] - -# 用來記錄每個 system 的資訊 -# 資料格式 { sysid : mavlink_device object } -mavlink_systems = {} +swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer # ====================== 分割線 ===================== -class mavlink_analyzer(Node, mavlink_publisher): +class mavlink_bridge(Node, mavlink_publisher): ''' - 這個 class 就是 固定串流分析器 - 是用來接收 mavlink 訊息 並進行分析 - 這個地方是針對 fixed_stream_analyzer_queue 的資料做處理的 + 這個 class 就是 固定串流橋接器 + 是用來接收 mavlink 訊息 並進行橋接 + 這個地方是針對 fixed_stream_bridge_queue 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object 並且看是否有重複 system id 整段代碼包含兩大區塊 thread 和 node - thread 區塊內會對 fixed_stream_analyzer_queue 進行監聽 並且將收到的訊息進行處理 + thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 藉由控制 ros2 的機制再把 device object 的資訊發送出去 ps. 我限制了這個 class 只能有一個 instance - ''' _instance = None _lock = threading.Lock() # 確保多線程安全 @@ -68,7 +61,7 @@ class mavlink_analyzer(Node, mavlink_publisher): def __new__(cls, *args, **kwargs): with cls._lock: # 確保多線程環境下只有一個實例被創建 if cls._instance is None: - cls._instance = super(mavlink_analyzer, cls).__new__(cls) + cls._instance = super(mavlink_bridge, cls).__new__(cls) return cls._instance def __init__(self): @@ -84,7 +77,7 @@ class mavlink_analyzer(Node, mavlink_publisher): self.thread = threading.Thread(target=self._run_thread) self.thread.start() else: - logger.error('mavlink_analyzer instance already exists. Do not create another one.') + logger.error('mavlink_bridge instance already exists. Do not create another one.') def stop(self): self.running = False @@ -92,33 +85,45 @@ class mavlink_analyzer(Node, mavlink_publisher): # === 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 進行後處理 + logger.info('Start of mavlink_bridge._run_thread') + # 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 while self.running: - if fixed_stream_analyzer_queue.empty(): + if fixed_stream_bridge_queue.empty(): continue - msg_pack = fixed_stream_analyzer_queue.get() + msg_pack = fixed_stream_bridge_queue.get() msg = msg_pack[2] sysid = msg.get_srcSystem() + compid = msg.get_srcComponent() msg_id = msg.get_msgId() - msg_type = msg.get_type() - # 若這個 system id 還不存在 執行完整建立 device object 的流程 + # 若這個 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_device = mavlink_device() # 創建一個新的 device object + self.mavlink_systems[sysid] = this_device + this_device.socket_id = msg_pack[0] + this_device.sysid = sysid + else: + this_device = self.mavlink_systems[sysid] - this_component = device_object.mavlink_component() # 創建一個新的 component object - device_object.components[msg.get_srcComponent()] = this_component - this_component.mav_type = msg_type - + # 若該 component id 存在 則直接使用該 component object + # 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object + # 若該 component id 不存在 又不是 heartbeat 則不處理 + if compid in self.mavlink_systems[sysid].components: + this_component = self.mavlink_systems[sysid].components[compid] + elif msg_id == 0: + # 只有透過 heartbeat 可以創建一個新的 component object + this_component = this_device.mavlink_component() + this_device.components[msg.get_srcComponent()] = this_component + this_component.mav_type = msg.type + this_component.mav_autopilot = msg.autopilot + else: + continue + # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ - if msg_id == 0: # HEARTBEAT 處理 - this_component.emitParams['base_mode'] = msg.base_mode - this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg) + + if msg_id == 0: # HEARTBEAT 處理 + this_component.emitParams['heartbeat'] = msg elif msg_id == 30: # ATTITUDE 處理 this_component.emitParams['attitude'] = msg @@ -139,15 +144,15 @@ class mavlink_analyzer(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_analyzer._run_thread') + logger.info('End of mavlink_bridge._run_thread') # === Node 區塊 === def _init_node(self): - logger.info('Start of mavlink_analyzer._init_node') - super().__init__('mavlink_analyzer') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒 + logger.info('Start of mavlink_bridge._init_node') + super().__init__('mavlink_bridge') # TODO 不知道為何 這句耗時超長 可以到 5~10 秒 def emit_info(self): # 這邊將 mavlink_systems 內所有的 device object 內所有的 component 輪循過 @@ -173,16 +178,41 @@ class mavlink_object(): 每個 mavlink bus 都會有一個 mavlink_object 其中主要是 thread 做統計封包與分流 分流表的控制在三個 list 分別為 - multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的 + multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToConvert : 這個 list 是用來分流到轉格式的 + multiplexingToSwap : 這個 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 = [] + mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } + socket_num = 0 # 用來記錄目前的 socket 數量 + def __new__(cls, *args, **kwargs): + # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 + # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 + # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號 + # socket_id 從 0 開始 + + instance = super().__new__(cls) + socket_id = 0 + for k in cls.mavlinkObjects.keys(): + if k == socket_id: + socket_id += 1 + else: + break + instance.socket_id = socket_id + cls.socket_num += 1 + cls.mavlinkObjects[socket_id] = instance + return instance + + def __init__(self, socket): + self.mavlink_socket = socket + # 這邊變數是執行的時候被使用的 不要直接寫入它 + self._multiplexingList = [] + # 存放要發送的訊息的 queue 或稱 buffer + self.output_buffer = queue.Queue() + if self.socket_id >= len(swap_queues): + swap_queues.append(self.output_buffer) + else: + swap_queues[self.socket_id] = self.output_buffer # 關聯到全域變數 global mavlink_systems @@ -190,28 +220,57 @@ class mavlink_object(): # 這三個 list 用來分配不同的訊息到不同的 queue self.multiplexingToAnalysis = [ - 0, # HEARTBEAT - 24, # GPS_RAW_INT - 30, # ATTITUDE - 32, # LOCAL_POSITION_NED - 33, # GLOBAL_POSITION_INT - 74, # VFR_HUD - 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [ - 0, # HEARTBEAT + 0, # HEARTBEAT # 挺必要的項目 + # 24, # GPS_RAW_INT + # 30, # ATTITUDE + # 33, # GLOBAL_POSITION_INT + # 74, # VFR_HUD + # 147, # BATTERY_STATUS ] - self.multiplexingToConvert = [ + self.multiplexingToReturn = [] + self.multiplexingToSwap = [ + [] for _ in range(len(swap_queues)) ] + # 刷新其他 mavlink_object 的 multiplexingToSwap + for k, object in self.mavlinkObjects.items(): + if (k != self.socket_id) and (len(object.multiplexingToSwap) <= self.socket_id): + object.multiplexingToSwap.append([]) + object.updateMultiplexingList() + + logger.info('mavlink_object instance {} created'.format(self.socket_id)) + def __del__(self): - self.mavlink_socket.close() - self.stop() + # 停下自己的 thread + if self.mavlink_socket != None: + self.mavlink_socket.close() + self.stop() + + # 移除其他 mavlink_object 的 multiplexingToSwap + for k, object in self.mavlinkObjects.items(): + if (k != self.socket_id) and (len(object.multiplexingToSwap) > self.socket_id): + object.multiplexingToSwap[self.socket_id] = [] + object.updateMultiplexingList() + + # 移除自己的 swap_queues + swap_queues[self.socket_id] = None + + # 處理 class 的 instance 記錄 + self.socket_num -= 1 + self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance + + # logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替 + # print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # try: + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # except Exception as e: + # print(f"Error logging in __del__: {e}") + # if 'logger' in globals() and logger: + # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) def run(self): - if not self.socket_id: - logger.error('Please set socket id before running') - return + # TODO 檢查 socket 是否有連線 self.thread = threading.Thread(target=self._run_thread) self.running = self.updateMultiplexingList() self.thread.start() @@ -220,20 +279,24 @@ class mavlink_object(): self.running = False def _run_thread(self): + logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) 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.") 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: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' + if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...' # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 sysid = mavlinkMsg.get_srcSystem() compid = mavlinkMsg.get_srcComponent() @@ -241,19 +304,36 @@ class mavlink_object(): # 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 + # break # Debug function can put here you can see the input data from mavlink # 將訊息依照 multiplexing list 分發到不同的 queue - for i in range(len(self.multiplexingList)): - if mavlinkMsg.get_msgId() in self.multiplexingList[i]: + for i in range(len(self._multiplexingList)): + if (self._multiplexingList[i] == []): + continue + elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1): if i == 0: - fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg)) + fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) elif i == 1: return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) else: - convert_queue = converte_format_queues[i-2] - convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) + _queue = swap_queues[i-2] + # _queue.put((self.socket_id,timestamp,mavlinkMsg)) # 測試看看 也許不需要別的資訊 只需要封包 + _queue.put(mavlinkMsg) + + # 處理要送出的封包 + # 如果 有資料在 output_buffer 中則將其取出並發送 + # 沒有就略過發送 + try: + mavlinkMsg_send = self.output_buffer.get(block=False) + except queue.Empty: + mavlinkMsg_send = None + # except Exception as e: + # logger.error(f"Error getting data from output_buffer: {e}") + # mavlinkMsg_send = None + + if mavlinkMsg_send: + # self.mavlink_socket.mav.send(mavlinkMsg_send) + self.mavlink_socket.write(mavlinkMsg_send.get_msgbuf()) # 這邊會將封包寫入 socket 中 # thread 結束 logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) @@ -262,20 +342,37 @@ class mavlink_object(): ''' 更新 multiplexing list 並做簡單的檢查 ''' + # 檢查 multiplexingToAnalysis 與 multiplexingToReturn 是否有 -1 值 + check = (-1 in self.multiplexingToAnalysis) or (-1 in self.multiplexingToReturn) + if check: + logger.error('MultiplexingToAnalysis or MultiplexingToReturn NOT all type spilt. socket id : {}'.format(self.socket_id)) + return False - # 更新 multiplexing list - self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + # 檢查 multiplexingToSwap 與 swap_queues 的長度是否一致 而且 swap_queues 的長度不能為 0 + check = len(self.multiplexingToSwap) != len(swap_queues) or len(swap_queues) == 0 + if check: + logger.error('Multiplexing Queue not fit List , Please check the list. socket id : {}'.format(self.socket_id)) + return False + + # 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋 + try: + self.multiplexingToSwap[self.socket_id] = [] + except IndexError: + logger.error('Multiplexing List of self socket id should be void. socket id : {}'.format(self.socket_id)) + return False + + # 組合 multiplexing list + multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap - # 檢查 multiplexing list 格式是否有錯誤 - check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) + # 檢查 multiplexing list 格式是否有錯誤 # 全部都要是 list 每個 list 裡面都要是 int + check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in multiL_tmp) if not check: - logger.error('Multiplexing List Format Error, Please check the list') + logger.error('Multiplexing List Format Error, Please check the list. socket id : {}'.format(self.socket_id)) 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 + # 更新 multiplexing list + self._multiplexingList = multiL_tmp + return True # ====================== 分割線 ===================== @@ -284,164 +381,6 @@ class mavlink_object(): def main_node(): pass -# ====================== 分割線 ===================== -# 測試時的程式進入點 -def main_develop(args=None): - - test_item = 3 - print('test_item : ', test_item) - - 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 - - # 做一個空的 queue list 驗證 multiplexingToConvert - converte_format_queues.append(queue.Queue()) - # 啟動連線的模組 - mavlink_object1.run() - - # 運行幾秒並印出 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() - - 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_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") - - print("start emit info") - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < 200000: - 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_socket.close() - print('End of Program .Test 3') if __name__ == '__main__': - import rclpy # 這邊是為了測試而加的 - main_develop() + pass From 1ff5df87c1688a824b848a84513433e3db10a26d Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:46:50 +0800 Subject: [PATCH 052/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index 028e3ee..b328f81 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,6 +9,8 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' +from pymavlink import mavutil + import std_msgs.msg import sensor_msgs.msg import geometry_msgs.msg @@ -24,13 +26,13 @@ class mavlink_publisher(): prefix_path = 'MavLinkBus' - def create_flightMode(self, sysid, component_obj): + def create_state(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'flightMode' + target_topic = 'state' # 這邊要檢查 flight_mode 是否存在 try: - _ = component_obj.emitParams['flightMode_mode'] + _ = component_obj.emitParams['heartbeat'] except KeyError: # 這個 component id 還不存在 logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) @@ -38,15 +40,16 @@ class mavlink_publisher(): # 若存在則 建立 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] + publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] return True - def packEmit_flightMode(cls, emitParams, publisher): - msg_str = emitParams['flightMode_mode'] - msg = std_msgs.msg.String() - msg.data = msg_str + def packEmit_state(cls, emitParams, publisher): + mav_msg = emitParams['heartbeat'] + msg = mavros_msgs.msg.State() + msg.mode = mavutil.mode_string_v10(mav_msg) + msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 publisher.publish(msg) pass From 4b8efcc5f9b9cde3518a155749d195608523f7e7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:47:00 +0800 Subject: [PATCH 053/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/devRun.py | 532 ++++++++++++++++++ 1 file changed, 532 insertions(+) create mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py new file mode 100644 index 0000000..60d54b9 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -0,0 +1,532 @@ +# 基礎功能的 import +import queue +import time + +# ROS2 的 import +import rclpy + +# mavlink 的 import +from pymavlink import mavutil + +# 自定義的 import +import mavlinkObject as mo +import mavlinkDevice as md + +# ====================== 分割線 ===================== + +test_item = 51 +running_time = 30 +print('test_item : ', test_item) + +if test_item == 1: + # 測試 mavlink_object 中 updateMultiplexingList 的輸出 + print('===> Start of Program .Test ', test_item) + mavlink_object_none = mo.mavlink_object(None) + + print('=====================') + print('正常範例') + mavlink_object_none.multiplexingToAnalysis = [] + mavlink_object_none.multiplexingToReturn = [] + # mavlink_object_none.multiplexingToSwap = [[]] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + print('multiplexingList : ', mavlink_object_none._multiplexingList) + + print('=====================') + print('錯誤範例 長度不一致') + mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + + print('=====================') + print('正常範例') + mo.swap_queues.append(queue.Queue()) + mo.swap_queues.append(queue.Queue()) + mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5] + mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10] + mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + print('multiplexingList : ', mavlink_object_none._multiplexingList) + + print('=====================') + print('錯誤範例 multiplexingToAnalysis 不可以有 -1') + mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5] + ret = mavlink_object_none.updateMultiplexingList() + print('execute result : ', ret) + + print('<=== End of Program') + +elif test_item == 2: + # 測試 mavlink_object 創建時 socket_id 是否正確 + # 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試 + print('===> Start of Program .Test ', test_item) + mavlink_object_none1 = mo.mavlink_object(None) + mavlink_object_none2 = mo.mavlink_object(None) + mavlink_object_none3 = mo.mavlink_object(None) + del mavlink_object_none2 + + print(len(mo.swap_queues)) + + mavlink_object_none2 = mo.mavlink_object(None) + print(len(mo.swap_queues)) + + print(mavlink_object_none1._multiplexingList) + + print('<=== End of Program') + +elif test_item == 10: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 + # 只啟用了 mavlink_object 的功能 + print('===> Start of Program .Test ', test_item) + + # 創建一個空的通道 這個通道的 socket_id 是 0 + mavlink_object_none = mo.mavlink_object(None) + + # 創建另一個通道 + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket) + # 設定通道轉發的參數 + mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE + mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT + mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD + + # print(mavlink_object1.multiplexingToSwap) + # print(mo.swap_queues) + + # 啟動通道 + mavlink_object1.run() + + # 確認轉拋的設定有沒有錯 + print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList) + + # 運行幾秒並印出 queue 的資料 + start_time = time.time() + while time.time() - start_time < running_time: + while not mo.fixed_stream_bridge_queue.empty(): + print('fixed_stream_bridge_queue:') + t = mo.fixed_stream_bridge_queue.get() + print('from {} : {}'.format(t[0],t[2])) + while not mo.return_packet_processor_queue.empty(): + print('return_packet_processor_queue:') + t = mo.return_packet_processor_queue.get() + print('from {} : {}'.format(t[0],t[2])) + # print(t[2].get_msgbuf()) + # print(t[2].type) + + for n in range(len(mo.swap_queues)): + q = mo.swap_queues[n] + while not q.empty(): + print('swap_queues:') + t = q.get() + print('{} gets : {}'.format(n,t)) + + + # 結束程式 退出所有 thread + mavlink_object1.running = False + mavlink_object1.thread.join() + mavlink_socket.close() + print('<=== End of Program') + +elif test_item == 11: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 + # 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能 + print('===> Start of Program .Test ', test_item) + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + # 創建通道 + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object2 = mo.mavlink_object(mavlink_socket) + # mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT + # mavlink_object2.multiplexingToReturn = [] + + # 啟動連線的模組 + mavlink_object2.run() + + # 做點延遲 讓 heartbeat 先吃進來 + time.sleep(2) + print("=== connection established! ===") + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + start_time = time.time() + show_time = time.time() + compid = 1 + while time.time() - start_time < running_time: + 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("===================") + + # 結束程式 退出所有 thread + mavlink_object2.stop() + mavlink_object2.thread.join() + analyzer.stop() + + analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance + analyzer.thread.join() + mavlink_socket.close() + print('<=== End of Program') + +elif test_item == 12: + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試 mavlink object 作為交換器功能的代碼 + print('===> Start of Program .Test ', test_item) + + # 啟動連線的模組 + analyzer = mo.mavlink_bridge() + + # 初始化輸入通道 + 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] # only HEARTBEAT + + # 初始化輸出通道 + connection_string_out="udpout:127.0.0.1:14553" + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + mavlink_object_out.multiplexingToAnalysis = [0] + + # 做一個空的通道驗證 可以拿來 debug + mavlink_object_none = mo.mavlink_object(None) + + # 讓兩個通道互相傳輸 + mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all + mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all + + # 啟動通道 + mavlink_object_in.run() + mavlink_object_out.run() + + # 做點延遲 讓 heartbeat 先吃進來 + time.sleep(3) + print("=== connection established! ===") + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + # print(type(mavlink_socket_out)) + # print(type(mavlink_socket_out.mav)) + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + try: + test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) + print('none object : ', test) + except queue.Empty: + pass + + if (time.time() - show_time) >= 2: + show_time = time.time() + for sysid in analyzer.mavlink_systems: + for compid in analyzer.mavlink_systems[sysid].components: + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + print("===================") + + + + # 結束程式 退出所有 thread + mavlink_object_in.stop() + mavlink_object_in.thread.join() + mavlink_object_out.stop() + mavlink_object_out.thread.join() + mavlink_socket_in.close() + mavlink_socket_out.close() + + analyzer.stop() + print('<=== End of Program') + +elif test_item == 20: + # 這邊測試 node 生成 topic 的功能 + # 利用 空的通道 發出假的 heartbeat 封包 + print('===> Start of Program .Test ', test_item) + rclpy.init() # 注意要初始化 rclpy 才能使用 node + + from pymavlink.dialects.v20 import common as mavlink2 + + sysid = 1 + compid = 1 + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + mav = mavlink2.MAVLink(None) + mav.srcSystem = sysid + mav.srcComponent = compid + + # 這是一個 heartbeat 封包 + fake_heartbeat = mavlink2.MAVLink_heartbeat_message( + type=mavlink2.MAV_TYPE_QUADROTOR, + autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode=3, + custom_mode=0, + system_status=0, + mavlink_version=3 + ) + mavlink_bytes = fake_heartbeat.pack(mav) + fake_heartbeat_msg = mav.parse_char(mavlink_bytes) + + print(analyzer.mavlink_systems) + + mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data + time.sleep(0.1) + print(analyzer.mavlink_systems) + + # ROS2 初始化 + analyzer._init_node() + + # 創建 ROS2 topic + analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid + print("topic created") + time.sleep(5) + + # 丟出 topic + analyzer.emit_info() + + # 結束程式 + analyzer.running = False + analyzer.destroy_node() + + rclpy.shutdown() + analyzer.stop() + analyzer.thread.join() + print('<=== End of Program') + +elif test_item == 21: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + + 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_socket = mavutil.mavlink_connection(connection_string) + mavlink_object3 = mo.mavlink_object(mavlink_socket) + # 啟動通道 + mavlink_object3.run() + + + print('=== waiting for mavlink data ...') + time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + compid = 1 + sysid = 1 + start_time = time.time() + analyzer.create_flightMode(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") + + 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(1) + except KeyboardInterrupt: + break + + + # 程式結束 + 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') + +elif test_item == 22: + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + 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_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" + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + mavlink_object_out.multiplexingToAnalysis = [] + + + + # 讓兩個通道互相傳輸 + mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all + mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + + # 啟動通道 + mavlink_object_in.run() + mavlink_object_out.run() + + print('waiting for mavlink data ...') + time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + compid = 1 + sysid = 1 + show_time = time.time() + analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) + print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") + + print("start emit info") + + start_time = time.time() + show_time = time.time() + show_time2 = time.time() + + while time.time() - start_time < running_time: + if (time.time() - show_time2) >= 1: + analyzer.emit_info() # 這邊是測試 node 的運行 + # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] + fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] + sss = mavutil.mode_string_v10(fmsg) + # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) + print("目前的飛行模式 : {}".format(sss)) + show_time2 = time.time() + + # if (time.time() - show_time) >= 2: + # show_time = time.time() + # for sysid in analyzer.mavlink_systems: + # for compid in analyzer.mavlink_systems[sysid].components: + # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + # print("===================") + + + + analyzer.destroy_node() + rclpy.shutdown() + + + # 結束程式 退出所有 thread + mavlink_object_in.stop() + mavlink_object_in.thread.join() + mavlink_socket_in.close() + mavlink_object_out.stop() + mavlink_object_out.thread.join() + mavlink_socket_out.close() + analyzer.stop() + analyzer.thread.join() + + + 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 的運行 + 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 From fa7689eaf560829514eeabd4603e8c447e8f965b Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 20:47:19 +0800 Subject: [PATCH 054/146] Delete 'src/fc_network_adapter/fc_network_adapter/socketManager.py' --- .../fc_network_adapter/socketManager.py | 14 -------------- 1 file changed, 14 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/socketManager.py diff --git a/src/fc_network_adapter/fc_network_adapter/socketManager.py b/src/fc_network_adapter/fc_network_adapter/socketManager.py deleted file mode 100644 index 52e8681..0000000 --- a/src/fc_network_adapter/fc_network_adapter/socketManager.py +++ /dev/null @@ -1,14 +0,0 @@ - -''' - -透過某個地方 得到 udp 或 uart 接口 -對於每個接口 視為一個獨立的物件 - -物件對於不同的接口 是為不同類型的物件 - -每個類型的物件 創建一個獨立的執行緒 來處理資料 -關於執行緒的實作 是寫在另一個模組 - -物件之間 也可以做資料轉換/轉拋 - -''' \ No newline at end of file From d32610d700670c37b048d05e0a98c5e108d4218e Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 20 May 2025 21:37:15 +0800 Subject: [PATCH 055/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 35 ++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index a81dbb2..fbe30d8 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -4,8 +4,7 @@ from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QApplication) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal, QUrl -from PyQt6.QtGui import QColor +from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtWebEngineWidgets import QWebEngineView import math import re @@ -14,8 +13,8 @@ import sys 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 +from std_msgs.msg import Float64 +from mavros_msgs.msg import VfrHud, State class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -62,10 +61,10 @@ class DroneMonitor(Node): 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), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), 10 ), 'global': self.create_subscription( @@ -135,14 +134,17 @@ class DroneMonitor(Node): 'voltage': msg.voltage }) - def mode_callback(self, drone_id, msg): - self.signals.update_signal.emit('mode', drone_id, msg.data) + def state_callback(self, drone_id, msg): + self.signals.update_signal.emit('state', drone_id, { + 'mode': msg.mode, + 'armed': msg.armed + }) 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): @@ -236,7 +238,7 @@ class ControlStationUI(QMainWindow):
@@ -365,8 +367,9 @@ class ControlStationUI(QMainWindow): if not (panel := self.drones.get(drone_id)): return - if msg_type == 'mode': - self.update_field(panel, drone_id, 'mode', f"{data}", + if msg_type == 'state': + mode = data['mode'] + self.update_field(panel, drone_id, 'mode', f"{mode}", '#FF5555' if '返航' in data else '#55FF55') elif msg_type == 'battery': From 800617dd3c20330fe1ed3198df3fbc474291ff8c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 21 May 2025 15:19:54 +0800 Subject: [PATCH 056/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 85 +++++++++++++++++++++++++++------- 1 file changed, 68 insertions(+), 17 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index fbe30d8..9d8651a 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -240,29 +240,63 @@ class ControlStationUI(QMainWindow): @@ -421,12 +455,29 @@ class ControlStationUI(QMainWindow): 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 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 + + # 清空原有 layout + for i in reversed(range(self.info_layout.count())): + widget = self.info_layout.itemAt(i).widget() + if widget: + self.info_layout.removeWidget(widget) + widget.setParent(None) + + # 根據 key 排序後重新加入 + for sorted_id in sorted(self.drones, key=lambda x: int(''.join(filter(str.isdigit, x)) or 0)): + self.info_layout.addWidget(self.drones[sorted_id]) def spin_ros(self): try: From 417d9e8f573f865de4ff4122be9e56b51abe981a Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 26 May 2025 22:43:57 +0800 Subject: [PATCH 059/146] =?UTF-8?q?1.=20=E6=96=B0=E5=A2=9E=20serial=5Fudp?= =?UTF-8?q?=5Fbitrans.py=20=E8=A9=B2=E7=A8=8B=E5=BC=8F=E9=80=A3=E7=B5=90?= =?UTF-8?q?=20serial=20=E7=9A=84=E5=B0=81=E5=8C=85=20=E7=B6=93=E9=81=8E?= =?UTF-8?q?=E8=BD=89=E6=8F=9B=E5=BE=8C=20=E5=86=8D=E5=90=91=20udp=20?= =?UTF-8?q?=E5=8F=A3=E4=B8=9F=E5=87=BA=20(=E5=8F=8D=E4=B9=8B=E4=BA=A6?= =?UTF-8?q?=E7=84=B6)=202.=20=E5=85=B6=E9=A4=98=E6=AA=94=E6=A1=88=E5=8F=AA?= =?UTF-8?q?=E6=98=AF=20=E5=90=8D=E7=A8=B1=E8=88=87=E8=A8=BB=E8=A7=A3?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +- .../fc_network_adapter/mavlinkDevice.py | 5 + .../fc_network_adapter/mavlinkObject.py | 1 - .../fc_network_adapter/serial_udp_bitrans.py | 340 ++++++++++++++++++ src/unitdev02/unitdev02/server.py | 33 -- .../unitdev02/{client.py => unix_client.py} | 0 src/unitdev02/unitdev02/unix_server.py | 94 +++++ 7 files changed, 441 insertions(+), 35 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py delete mode 100644 src/unitdev02/unitdev02/server.py rename src/unitdev02/unitdev02/{client.py => unix_client.py} (100%) create mode 100644 src/unitdev02/unitdev02/unix_server.py diff --git a/README.md b/README.md index 9f6379f..7fd8868 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,8 @@ Python 1. pymavlink -2. +2. conda-forge 中的 pyserial-asyncio +3. ROS2 1. source ~/ros2_humble/install/setup.bash diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index 4b2b07e..4ae4c98 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -31,6 +31,11 @@ class mavlink_device(): # p_str += '=====================\n' return p_str + ''' + 寫了半天 這個功能應該是 pymalink 本來就有的 + 去找 pymavlink_util.py 的 mavfile(object) + 算了 先擺著吧 之後再看看怎麼整合 + ''' def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): # 這段檢查遺失封包 try: diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 072d861..1589150 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -345,7 +345,6 @@ class mavlink_object(): return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) else: _queue = swap_queues[i-2] - # _queue.put((self.socket_id,timestamp,mavlinkMsg)) # 測試看看 也許不需要別的資訊 只需要封包 _queue.put(mavlinkMsg) # 處理要送出的封包 diff --git a/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py b/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py new file mode 100644 index 0000000..cb4373c --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py @@ -0,0 +1,340 @@ +import asyncio +import serial_asyncio + +# 附加驗證功能 +import os +import sys +import serial +import signal + +# XBee 模組 +from xbee.frame import APIFrame + +# ========================= + +SERIAL_PORT = '/dev/ttyACM0' # serial port +SERIAL_BAUDRATE = 57600 # serial baudrate + +UDP_REMOTE_IP = '127.0.0.1' # UDP Target IP +UDP_REMOTE_PORT = 14550 # UDP Target port + +# 測試用 只會吃一次資料 +DEBUG_MODE = False + +# ========================= + +def check_serial_port(): + """檢查串口是否存在與可用""" + # 檢查設備是否存在 + if not os.path.exists(SERIAL_PORT): + print(f"錯誤：串口設備 {SERIAL_PORT} 不存在") + return False + + # 檢查是否有權限訪問設備 + try: + os.access(SERIAL_PORT, os.R_OK | os.W_OK) + except Exception as e: + print(f"錯誤：無法訪問串口設備 {SERIAL_PORT}：{str(e)}") + return False + + # 檢查是否被占用 + try: + # 嘗試打開串口 + ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE) + ser.close() # 打開成功後立即關閉 + return True + except serial.SerialException as e: + print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}") + return False + except Exception as e: + print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}") + return False + +# ========================= + +class SerialToUDP(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 + def __init__(self, udp_protocol): + self.udp_protocol = udp_protocol + self.first_data = True # 標記是否為第一次收到資料 + self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + if hasattr(self.udp_protocol, 'set_serial_transport'): + self.udp_protocol.set_serial_transport(self) + print(f"Serial connection established on {SERIAL_PORT}") + + ## ===================================== + + # Serial 收到資料，轉發到 UDP + def data_received(self, data): + # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 # debug + if DEBUG_MODE and self.has_processed: + return + + # 標記首次收到資料 + if hasattr(self.udp_protocol, 'send_udp'): + if self.first_data: + print(f"First data received from serial, forwarding to UDP: {data[:20]}...") + self.first_data = False + + # 轉發數據 + self.udp_protocol.send_udp(data) + + if DEBUG_MODE: # 測試模式用 # debug + self.has_processed = True + print("[DEBUG] SerialToUDP Mark") + + def write_to_serial(self, data): + # 在資料透過 Serial 發送之前進行處理 + processed_data = self.encapsulate_data(data) + + # 處理失敗就不發送 + if processed_data not None: + self.transport.write(processed_data) + + def encapsulate_data(self, data): + + """ + 將數據封裝為 XBee API 傳輸幀 + + 使用 XBee API 格式封裝數據: + - 傳輸請求幀 (0x10) + - 使用廣播地址 + - 添加適當的頭部和校驗和 + """ + + ## 方法一 + # 設置幀參數 + frame_id = 0x01 # 幀識別碼，用於確認 + frame_type = 0x10 # 幀類型：傳輸請求 + dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 64位目標地址 (廣播) + dest_addr16 = b'\xFF\xFE' # 16位目標地址 (未知/廣播) + broadcast_radius = 0x00 # 廣播半徑 (0 = 最大) + options = 0x00 # 傳輸選項 + + # 構建幀數據部分 + frame_data = bytearray() + frame_data.append(frame_type) # 添加幀類型 + frame_data.append(frame_id) # 添加幀 ID + frame_data.extend(dest_addr64) # 添加 64 位目標地址 + frame_data.extend(dest_addr16) # 添加 16 位目標地址 + frame_data.append(broadcast_radius) # 添加廣播半徑 + frame_data.append(options) # 添加選項 + frame_data.extend(data) # 添加實際數據內容 + + # 計算校驗和 (0xFF 減去所有字節的總和的最低 8 位) + checksum = 0xFF - (sum(frame_data) & 0xFF) + + # 構建完整的 API 幀 + # 起始分隔符 + 長度 (兩字節) + 幀數據 + 校驗和 + complete_frame = bytearray() + complete_frame.append(0x7E) # 添加起始分隔符 + complete_frame.extend(struct.pack(">H", len(frame_data))) # 添加長度 (高位優先) + complete_frame.extend(frame_data) # 添加幀數據 + complete_frame.append(checksum) # 添加校驗和 + + return bytes(complete_frame) + + ## 方法二 + # frame_id=0x01 + # frame_type = 0x10 + # dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 + # 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) + + +class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 + def __init__(self): + self.serial_transport = None + self.transport = None + self.remote_addr = None # 儲存動態獲取的遠程地址 + self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + print("UDP transport ready. Waiting for serial data before sending...") + + def set_serial_transport(self, serial_transport): + self.serial_transport = serial_transport + + ## ===================================== + + # UDP 收到資料 + def datagram_received(self, data, addr): + # 儲存對方的地址（這樣就能向同一個來源回傳數據） + # self.remote_addr = addr + # print(f"Received UDP data from {addr}, setting as remote address") + + # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 + if DEBUG_MODE and self.has_processed: + return + + if self.serial_transport: + self.serial_transport.write_to_serial(data) + + if DEBUG_MODE: # 測試模式用 + self.has_processed = True + print("[DEBUG] UDPHandler Mark") + + def send_udp(self, data): + # 發送資料到 UDP + + # if self.transport: + # # 如果有已知的回應地址，使用該地址 + # if self.remote_addr: + # self.transport.sendto(data, self.remote_addr) + # # print(f"Sending to dynamic address: {self.remote_addr}") + # else: + # # 否則使用預設地址 + # self.transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) + # print(f"Sending first UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") + + if self.transport: + # 只有第一次或地址改變時才顯示 + # if not hasattr(self, '_last_sent_addr') or self._last_sent_addr != (UDP_REMOTE_IP, UDP_REMOTE_PORT): + # print(f"Sending UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") + # self._last_sent_addr = (UDP_REMOTE_IP, UDP_REMOTE_PORT) + + # 在透過 UDP 發送數據之前進行解封裝 + decoded_data = self.decapsulate_data(data) + self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) + + def decapsulate_data(self, data): + # 這裡可以根據需要進行數據解封裝 + + ## 方法一 + try: + # 創建一個 API 幀處理器 + api_frame = APIFrame(escaped=True) + + # 嘗試解析數據 + api_frame.fill(data) + + # 如果數據不完整，直接返回原始數據 + if not api_frame.is_complete(): + return data + + # 解析幀內容 + parsed_data = api_frame.parse() + + # 提取有用數據 + if parsed_data: + frame_data = parsed_data.get('rf_data', None) + if frame_data: + return frame_data + + return data + + ## 方法二 - 手動解析 + # try: + # # XBee API 幀格式: + # # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) + + # # 檢查幀起始符 (0x7E) + # if not data or len(data) < 5 or data[0] != 0x7E: + # return data + + # # 獲取數據長度 (不包括校驗和) + # length = (data[1] << 8) + data[2] + + # # 檢查幀完整性 + # if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 + # return data + + # # 提取API標識符和數據 + # frame_type = data[3] + # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 + + # # 根據不同的幀類型進行處理 + # if frame_type == 0x90: # 例如，這是"接收數據包"類型 + # # 提取實際有效載荷 + # # 對於接收數據包，格式通常是： + # # API標識符(1) + 64位地址(8) + 16位地址(2) + 選項(1) + 數據 + # if len(frame_data) >= 12: # 確保數據長度足夠 + # payload = frame_data[11:] # 前11字節是地址和選項 + # return payload + # return data + + # 如果無法提取 則回傳 None + except Exception as e: + print(f"手動解析 XBee 數據錯誤: {e}") + return None + +async def main(): + # 先檢查串口是否可用 + if not check_serial_port(): + print("程式終止：串口檢查失敗") + return + + loop = asyncio.get_running_loop() + + # 設置終止處理 + for sig in (signal.SIGINT, signal.SIGTERM): + loop.add_signal_handler( + sig, + lambda: asyncio.create_task(shutdown(loop)) + ) + + # 建立單一 UDP 端點處理收發 + udp_handler = UDPHandler() + + # 建立 UDP 傳輸，不指定接收端口，讓系統自動分配 + try: + udp_transport, protocol = await loop.create_datagram_endpoint( + lambda: udp_handler, + local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 + ) + except Exception as e: + print(f"無法創建 UDP 端點：{str(e)}") + return + + # 獲取系統分配的本地端口 + sock = udp_transport.get_extra_info('socket') + local_addr = sock.getsockname() + print(f"UDP listening on {local_addr[0]}:{local_addr[1]}") + + # 建立 Serial 傳輸，將 UDP 處理器傳給它 + try: + serial_proto = SerialToUDP(udp_handler) + _, serial_transport = await serial_asyncio.create_serial_connection( + loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE + ) + except Exception as e: + print(f"無法建立串口連接：{str(e)}") + udp_transport.close() + return + + print(f"Waiting for data from serial port to initiate UDP communication...") + + # 保持運行 + try: + await asyncio.Future() + except asyncio.CancelledError: + pass + +async def shutdown(loop): + """關閉程序""" + print("Shutting down...") + tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()] + + for task in tasks: + task.cancel() + + await asyncio.gather(*tasks, return_exceptions=True) + loop.stop() + +if __name__ == '__main__': + try: + asyncio.run(main()) + except KeyboardInterrupt: + print("程式被使用者中斷") + except Exception as e: + print(f"程式執行錯誤：{str(e)}") \ No newline at end of file diff --git a/src/unitdev02/unitdev02/server.py b/src/unitdev02/unitdev02/server.py deleted file mode 100644 index 148edc5..0000000 --- a/src/unitdev02/unitdev02/server.py +++ /dev/null @@ -1,33 +0,0 @@ -import socket -import os - -# socket file path -SOCKET_PATH = "/tmp/unix_socket_example" - -# 若檔案存在就先刪除 -if os.path.exists(SOCKET_PATH): - os.remove(SOCKET_PATH) - -# 建立 socket -server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) - -# 綁定 socket 到檔案 -server.bind(SOCKET_PATH) -server.listen(1) - -print("🔌 Server waiting for connection...") - -# 等待 client 連線 -conn, _ = server.accept() -print("✅ Client connected.") - -while True: - data = conn.recv(1024) - if not data: - break - print("📥 Received:", data.decode()) - conn.sendall(b"Echo: " + data) - -conn.close() -server.close() -os.remove(SOCKET_PATH) diff --git a/src/unitdev02/unitdev02/client.py b/src/unitdev02/unitdev02/unix_client.py similarity index 100% rename from src/unitdev02/unitdev02/client.py rename to src/unitdev02/unitdev02/unix_client.py diff --git a/src/unitdev02/unitdev02/unix_server.py b/src/unitdev02/unitdev02/unix_server.py new file mode 100644 index 0000000..30ef524 --- /dev/null +++ b/src/unitdev02/unitdev02/unix_server.py @@ -0,0 +1,94 @@ +import socket +import os + +# # socket file path +# SOCKET_PATH = "/tmp/unix_socket_example" + +# # 若檔案存在就先刪除 +# if os.path.exists(SOCKET_PATH): +# os.remove(SOCKET_PATH) + +# # 建立 socket +# server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + +# # 綁定 socket 到檔案 +# server.bind(SOCKET_PATH) +# server.listen(1) + +# print("🔌 Server waiting for connection...") + +# # 等待 client 連線 +# conn, _ = server.accept() +# print("✅ Client connected.") + +# while True: +# data = conn.recv(1024) +# if not data: +# break +# print("📥 Received:", data.decode()) +# conn.sendall(b"Echo: " + data) + +# conn.close() +# server.close() +# os.remove(SOCKET_PATH) + + +# ===================== + +# from pymavlink import mavutil + + +# def create_unix_socket_connection(): +# # 建立一個 mavtcpin 實例 +# mav_conn = mavutil.mavtcpin("127.0.0.250:9999", source_system=1, source_component=1) + +# # 替換底層 socket +# # 關閉原有的 socket +# mav_conn.listen.close() + +# # 創建 Unix socket 並替換 +# unix_socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) +# unix_socket_path = "/tmp/unix_socket_mavlink.sock" + +# # 確保先前的 unix socket 已被移除 +# if os.path.exists(unix_socket_path): +# os.remove(unix_socket_path) + +# # 綁定並設定 Unix socket +# unix_socket.bind(unix_socket_path) +# unix_socket.listen(1) +# unix_socket.setblocking(0) +# mavutil.set_close_on_exec(unix_socket.fileno()) + +# # 替換 listen socket +# mav_conn.listen = unix_socket +# mav_conn.fd = unix_socket.fileno() + +# # mav_conn.port = unix_socket + +# return mav_conn + + +# a = create_unix_socket_connection() + +# # print(a.port) + + + + +# =============================== + +import mavunixin +mav_conn = mavunixin.mavunixin("unix:/tmp/unix_socket_mavlink.sock", source_system=1, source_component=1) + +import time +print("🔌 Server waiting for connection...") +while True: + time.sleep(1) + mavlinkMsg = mav_conn.recv_msg() + if mavlinkMsg is not None: + print("📥 Received:", mavlinkMsg) + # a.send(mavlinkMsg) + # print("📤 Server replied:", data.decode()) + else: + print("No message received.") \ No newline at end of file From 7f7753d0b4bb3d1a9d43953d6043866c69701a0c Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Tue, 3 Jun 2025 02:04:51 +0800 Subject: [PATCH 060/146] =?UTF-8?q?1.=20(=E4=B8=AD=E7=B9=BC=E7=9A=84?= =?UTF-8?q?=E5=AD=98=E6=AA=94)=20=E6=BA=96=E5=82=99=E5=B0=8D=20mavlinkObje?= =?UTF-8?q?ct.py=20=E7=9A=84=20mavlink=5Fobject=20=E9=87=8D=E6=A7=8B=202.?= =?UTF-8?q?=20=E6=96=B0=E5=A2=9E=20ringBuffer.py=20=E4=BD=9C=E7=82=BA=20qu?= =?UTF-8?q?eue=20=E5=8F=96=E4=BB=A3=203.=20=E6=96=B0=E5=A2=9E=20tests=20?= =?UTF-8?q?=E8=B3=87=E6=96=99=E5=A4=BE=E5=88=86=E9=9B=A2=E7=A8=8B=E5=BC=8F?= =?UTF-8?q?=E8=88=87=E6=B8=AC=E8=A9=A6=E6=AA=94=E6=A1=88=204.=20asyncioMan?= =?UTF-8?q?ager.py=20=E6=98=AF=E5=8F=83=E8=80=83=E7=94=A8=E7=9A=84?= =?UTF-8?q?=E7=AF=84=E4=BE=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/asyncioManager.py | 290 ++++++++++++++++++ .../fc_network_adapter/devRun.py | 99 +++++- .../fc_network_adapter/mavlinkObject.py | 2 - .../fc_network_adapter/ringBuffer.py | 231 ++++++++++++++ src/fc_network_adapter/tests/__init__.py | 0 .../tests/test_asyncioManager.py | 246 +++++++++++++++ .../tests/test_ringBuffer.py | 152 +++++++++ 7 files changed, 1015 insertions(+), 5 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/asyncioManager.py create mode 100644 src/fc_network_adapter/fc_network_adapter/ringBuffer.py create mode 100644 src/fc_network_adapter/tests/__init__.py create mode 100644 src/fc_network_adapter/tests/test_asyncioManager.py create mode 100644 src/fc_network_adapter/tests/test_ringBuffer.py diff --git a/src/fc_network_adapter/fc_network_adapter/asyncioManager.py b/src/fc_network_adapter/fc_network_adapter/asyncioManager.py new file mode 100644 index 0000000..9a9ba65 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/asyncioManager.py @@ -0,0 +1,290 @@ +import asyncio +import socket +import threading +import time +import logging +from typing import Dict, Any, Optional, List, Callable, Tuple +from pymavlink import mavutil +from dataclasses import dataclass, field +from .ringBuffer import RingBuffer + +# 設置日誌 +logger = logging.getLogger("async_io_manager") + +@dataclass +class Connection: + """表示一個 MAVLink 連接""" + socket_id: int + socket: Any # pymavlink socket 或相容物件 + buffer: RingBuffer = field(default_factory=lambda: RingBuffer(1024)) + last_activity: float = field(default_factory=time.time) + is_closed: bool = False + stats: Dict[str, int] = field(default_factory=lambda: {"rx_count": 0, "rx_bytes": 0, "errors": 0}) + +class AsyncIOManager: + """ + 異步 I/O 管理器 + 負責高效處理所有 MAVLink socket 的 I/O 操作 + """ + _instance = None + _lock = threading.Lock() + + def __new__(cls, *args, **kwargs): + with cls._lock: + if cls._instance is None: + cls._instance = super(AsyncIOManager, cls).__new__(cls) + cls._instance._initialized = False + return cls._instance + + def __init__(self): + if self._initialized: + return + + self._initialized = True + self.connections: Dict[int, Connection] = {} + self.next_socket_id = 0 + self._running = False + self._event_loop = None + self._thread = None + self.check_interval = 0.01 # 檢查間隔，單位：秒 + + def start(self): + """啟動異步 I/O 管理器""" + if self._running: + logger.warning("AsyncIOManager already running") + return + + self._running = True + self._thread = threading.Thread(target=self._run_event_loop, daemon=True) + self._thread.start() + logger.info("AsyncIOManager started") + + def _run_event_loop(self): + """在專用執行緒中執行 asyncio 事件循環""" + try: + # 創建新的事件循環 + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + self._event_loop = loop + + # 添加主要協程任務 + loop.create_task(self._process_all_connections()) + + # 運行事件循環 + loop.run_forever() + except Exception as e: + logger.error(f"Error in AsyncIOManager event loop: {e}") + finally: + if loop.is_running(): + loop.stop() + logger.info("AsyncIOManager event loop stopped") + + async def _process_all_connections(self): + """處理所有連接的主循環""" + while self._running: + for socket_id, conn in list(self.connections.items()): + if conn.is_closed: + continue + + try: + # 非阻塞模式讀取數據 + await asyncio.get_event_loop().run_in_executor( + None, self._read_from_connection, conn + ) + except Exception as e: + conn.stats["errors"] += 1 + logger.error(f"Error processing connection {socket_id}: {e}") + + # 短暫休息，讓出 CPU + await asyncio.sleep(self.check_interval) + + def _read_from_connection(self, conn: Connection): + """從連接讀取數據，此函數在執行器中運行""" + try: + # 使用 mavlink 的非阻塞讀取 + msg = conn.socket.recv_msg() + if msg: + # 收到消息，更新統計資訊 + conn.last_activity = time.time() + conn.stats["rx_count"] += 1 + conn.stats["rx_bytes"] += len(msg.get_msgbuf()) + + # 將消息放入緩衝區 + if not conn.buffer.put(msg): + # 緩衝區已滿，記錄錯誤 + logger.warning(f"Buffer full for connection {conn.socket_id}, message dropped") + except Exception as e: + conn.stats["errors"] += 1 + if "Connection refused" not in str(e): # 忽略常見的連接拒絕錯誤 + logger.error(f"Error reading from socket {conn.socket_id}: {e}") + + def stop(self): + """停止異步 I/O 管理器""" + if not self._running: + return + + self._running = False + + # 停止事件循環 + if self._event_loop: + asyncio.run_coroutine_threadsafe( + self._shutdown_loop(), self._event_loop + ) + + # 等待執行緒結束 + if self._thread and self._thread.is_alive(): + self._thread.join(timeout=2.0) + if self._thread.is_alive(): + logger.warning("AsyncIOManager thread did not terminate cleanly") + + logger.info("AsyncIOManager stopped") + + async def _shutdown_loop(self): + """關閉事件循環""" + # 關閉所有連接 + for socket_id in list(self.connections.keys()): + self.remove_connection(socket_id) + + # 停止事件循環 + loop = asyncio.get_event_loop() + loop.stop() + + def register_connection(self, socket: Any) -> int: + """ + 註冊新的連接 + + Args: + socket: pymavlink socket 或相容物件 + + Returns: + int: 分配的 socket_id + """ + socket_id = self.next_socket_id + self.next_socket_id += 1 + + # 創建新的連接 + connection = Connection(socket_id=socket_id, socket=socket) + self.connections[socket_id] = connection + + logger.info(f"Registered new connection with ID {socket_id}") + return socket_id + + def remove_connection(self, socket_id: int) -> bool: + """ + 移除連接 + + Args: + socket_id: 連接 ID + + Returns: + bool: 成功移除返回 True + """ + if socket_id in self.connections: + conn = self.connections[socket_id] + conn.is_closed = True + + try: + # 關閉原始 socket + if hasattr(conn.socket, 'close'): + conn.socket.close() + except Exception as e: + logger.error(f"Error closing socket {socket_id}: {e}") + + # 從字典中移除 + self.connections.pop(socket_id) + logger.info(f"Removed connection with ID {socket_id}") + return True + return False + + def get_buffer(self, socket_id: int) -> Optional[RingBuffer]: + """ + 獲取指定連接的緩衝區 + + Args: + socket_id: 連接 ID + + Returns: + RingBuffer 或 None（如果連接不存在） + """ + if socket_id in self.connections: + return self.connections[socket_id].buffer + return None + + def get_stats(self, socket_id: int = None) -> Dict: + """ + 獲取連接統計資訊 + + Args: + socket_id: 指定連接 ID，None 表示所有連接 + + Returns: + Dict: 統計資訊 + """ + if socket_id is not None and socket_id in self.connections: + return self.connections[socket_id].stats + + # 彙總所有連接的統計資訊 + total_stats = {"rx_count": 0, "rx_bytes": 0, "errors": 0, "connection_count": len(self.connections)} + for conn in self.connections.values(): + for key, value in conn.stats.items(): + if key in total_stats: + total_stats[key] += value + return total_stats + + def send_message(self, socket_id: int, message: Any) -> bool: + """ + 發送 MAVLink 訊息 + + Args: + socket_id: 連接 ID + message: 要發送的訊息 + + Returns: + bool: 成功發送返回 True + """ + if socket_id not in self.connections: + return False + + conn = self.connections[socket_id] + if conn.is_closed: + return False + + try: + # 使用 asyncio.run_coroutine_threadsafe 在事件循環中執行 IO 操作 + future = asyncio.run_coroutine_threadsafe( + self._send_message_async(conn, message), + self._event_loop + ) + # 等待操作完成（可選） + return future.result(timeout=1.0) + except Exception as e: + logger.error(f"Error sending message on socket {socket_id}: {e}") + return False + + async def _send_message_async(self, conn: Connection, message: Any) -> bool: + """在事件循環中異步發送訊息""" + try: + # 調用執行器進行實際發送操作 + return await asyncio.get_event_loop().run_in_executor( + None, self._send_message_sync, conn, message + ) + except Exception as e: + conn.stats["errors"] += 1 + logger.error(f"Async error sending message: {e}") + return False + + def _send_message_sync(self, conn: Connection, message: Any) -> bool: + """同步發送訊息函數，在執行器中運行""" + try: + if hasattr(message, 'get_msgbuf'): + # 如果是 MAVLink 訊息對象 + conn.socket.write(message.get_msgbuf()) + else: + # 假設是已編碼的字節 + conn.socket.write(message) + conn.last_activity = time.time() + return True + except Exception as e: + conn.stats["errors"] += 1 + logger.error(f"Error in sync message send: {e}") + return False \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 110fb61..c638659 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -2,6 +2,7 @@ import queue import time + # ROS2 的 import import rclpy @@ -14,10 +15,16 @@ import mavlinkDevice as md # ====================== 分割線 ===================== -test_item = 51 -running_time = 30 +test_item = 12 +running_time = 3000 print('test_item : ', test_item) +''' +測試項 個位數 表示分離的功能 + +測試項 1X 表示 mavlink_object 的功能 測試連線的能力 +''' + if test_item == 1: # 測試 mavlink_object 中 updateMultiplexingList 的輸出 print('===> Start of Program .Test ', test_item) @@ -140,7 +147,7 @@ elif test_item == 11: analyzer = mo.mavlink_bridge() # 創建通道 - connection_string="udp:127.0.0.1:14550" + connection_string="udp:127.0.0.1:15551" mavlink_socket = mavutil.mavlink_connection(connection_string) mavlink_object2 = mo.mavlink_object(mavlink_socket) # mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT @@ -169,6 +176,7 @@ elif test_item == 11: 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_socket.last_seq) print("===================") # 結束程式 退出所有 thread @@ -254,6 +262,91 @@ elif test_item == 12: analyzer.stop() print('<=== End of Program') +elif test_item == 13: + # 這邊測試看看能否狸貓換太子的方式 + # 把原來 pymavlink 的 mavtcpin 換成 unix socket + print('===> Start of Program .Test ', test_item) + + import os + import socket + + # 建立一個 mavtcpin 實例 + import mavunixin + + + + + # ===== 以下就是按照原本測試 12 的流程 ===== + # 啟動連線的模組 + analyzer = mo.mavlink_bridge() + + # 初始化輸入通道 + mavlink_socket_unix = mavunixin.mavunixin("unix:/tmp/unix_socket_mavlink.sock", source_system=1, source_component=1) + mavlink_object_in = mo.mavlink_object(mavlink_socket_unix) + mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT + + # 停留五秒 我要啟動另一邊 + time.sleep(5) + + # 初始化輸出通道 + connection_string_out="udpout:127.0.0.1:14553" + mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + mavlink_object_out.multiplexingToAnalysis = [0] + + # 做一個空的通道驗證 可以拿來 debug + mavlink_object_none = mo.mavlink_object(None) + + # 讓兩個通道互相傳輸 + mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all + mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all + + # 啟動通道 + mavlink_object_in.run() + mavlink_object_out.run() + + # 做點延遲 讓 heartbeat 先吃進來 + time.sleep(3) + print("=== connection established! ===") + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + # print(type(mavlink_socket_out)) + # print(type(mavlink_socket_out.mav)) + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + try: + test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) + print('none object : ', test) + except queue.Empty: + pass + + if (time.time() - show_time) >= 2: + show_time = time.time() + for sysid in analyzer.mavlink_systems: + for compid in analyzer.mavlink_systems[sysid].components: + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + print("===================") + + + + # 結束程式 退出所有 thread + mavlink_object_in.stop() + mavlink_object_in.thread.join() + mavlink_object_out.stop() + mavlink_object_out.thread.join() + mavlink_socket_unix.close() + mavlink_socket_out.close() + + analyzer.stop() + print('<=== End of Program') + elif test_item == 20: # 這邊測試 node 生成 topic 的功能 # 利用 空的通道 發出假的 heartbeat 封包 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 1589150..b55107c 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -307,7 +307,6 @@ class mavlink_object(): self.running = False def _run_thread(self): - logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) start_time = time.time() while self.running: @@ -317,7 +316,6 @@ class mavlink_object(): try: mavlinkMsg = self.mavlink_socket.recv_msg() except Exception as e: - logger.critical(f"Receiving data not mavlink format. Object Delete.") logger.critical(f"Receiving data not mavlink format. Object Delete.") print(f"[mavlinkObject.py] Error receiving mavlink data: {e}") print(mavlinkMsg) diff --git a/src/fc_network_adapter/fc_network_adapter/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/ringBuffer.py new file mode 100644 index 0000000..6728426 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/ringBuffer.py @@ -0,0 +1,231 @@ +# import array +import threading +import ctypes +from typing import Any, Optional, Tuple + +class RingBuffer: + """ + 高效能無鎖環形緩衝區，使用原子操作避免鎖定 + 用於在不同執行緒間高效傳遞資料 + """ + # 緩衝區計數器，用於自動分配 buffer_id + _buffer_counter = 0 + _counter_lock = threading.Lock() + + def __init__(self, capacity: int = 256, buffer_id: int = None): + """ + 初始化環形緩衝區 + + Args: + capacity: 緩衝區容量 (必須是 2 的次方) + buffer_id: 緩衝區 ID，如果為 None 則自動分配 + """ + # 確保容量是 2 的次方，便於使用位運算取模 + if capacity & (capacity - 1) != 0: + # 找到大於等於 capacity 的最小 2 的次方 + capacity = 1 << (capacity - 1).bit_length() + + # 分配緩衝區 ID + if buffer_id is None: + with RingBuffer._counter_lock: + buffer_id = RingBuffer._buffer_counter + RingBuffer._buffer_counter += 1 + + self.buffer_id = buffer_id + self.capacity = capacity + self.mask = capacity - 1 # 用於快速取模 + self.buffer = [None] * capacity + + # 原子計數器作為讀寫指標 + self.write_index = ctypes.c_uint64(0) + self.read_index = ctypes.c_uint64(0) + + # 用於檢測上次操作的執行緒 ID + self._last_read_thread = None + self._last_write_thread = None + + # 添加同時讀寫統計 + self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數 + self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數 + self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數 + self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數 + self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數 + + # 記錄各執行緒的操作次數 + self.thread_write_counts = {} # {thread_id: count} + self.thread_read_counts = {} # {thread_id: count} + + # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作） + self._stats_lock = threading.Lock() + + def put(self, item: Any) -> bool: + """ + 將項目存入緩衝區 + + Args: + item: 要存入的項目 + + Returns: + bool: 成功寫入返回 True，緩衝區已滿返回 False + """ + # 更新寫入統計 + self.total_write_count.value += 1 + + # 檢測上次寫入的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒寫入次數 + with self._stats_lock: + self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時寫入 + if self._last_write_thread is not None and current_thread != self._last_write_thread: + self.concurrent_write_count.value += 1 + + self._last_write_thread = current_thread + + # 原子獲取當前寫入位置 + current = self.write_index.value + next_pos = (current + 1) & self.mask + + # 檢查緩衝區是否已滿 + if next_pos == self.read_index.value: + self.overflow_count.value += 1 + return False + + # 寫入資料並原子更新寫入位置 + self.buffer[current] = item + self.write_index.value = next_pos + return True + + def get(self) -> Optional[Any]: + """ + 從緩衝區讀取項目 + + Returns: + 項目或 None（如果緩衝區為空） + """ + # 更新讀取統計 + self.total_read_count.value += 1 + + # 檢測上次讀取的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒讀取次數 + with self._stats_lock: + self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時讀取 + if self._last_read_thread is not None and current_thread != self._last_read_thread: + self.concurrent_read_count.value += 1 + + self._last_read_thread = current_thread + + # 檢查緩衝區是否為空 + if self.read_index.value == self.write_index.value: + return None + + # 原子獲取當前讀取位置並讀取資料 + current = self.read_index.value + item = self.buffer[current] + + # 原子更新讀取位置 + self.read_index.value = (current + 1) & self.mask + return item + + def get_all(self) -> list: + """ + 獲取緩衝區中的所有項目 + + Returns: + list: 所有可用項目的列表 + """ + items = [] + while True: + item = self.get() + if item is None: + break + items.append(item) + return items + + def size(self) -> int: + # 返回目前緩衝區內項目數量 + return (self.write_index.value - self.read_index.value) & self.mask + + def is_empty(self) -> bool: + # 檢查緩衝區是否為空 + return self.read_index.value == self.write_index.value + + def is_full(self) -> bool: + # 檢查緩衝區是否已滿 + return ((self.write_index.value + 1) & self.mask) == self.read_index.value + + def clear(self) -> None: + """清空緩衝區""" + self.read_index.value = self.write_index.value + + def get_stats(self) -> dict: + """ + 獲取緩衝區統計資訊 + + Returns: + dict: 包含各種統計數據的字典 + """ + with self._stats_lock: + stats = { + "buffer_id": self.buffer_id, + "capacity": self.capacity, + "current_size": self.size(), + "is_full": self.is_full(), + "is_empty": self.is_empty(), + "total_writes": self.total_write_count.value, + "total_reads": self.total_read_count.value, + "concurrent_writes": self.concurrent_write_count.value, + "concurrent_reads": self.concurrent_read_count.value, + "overflow_count": self.overflow_count.value, + "write_threads": len(self.thread_write_counts), + "read_threads": len(self.thread_read_counts), + "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value), + "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value), + "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3], + "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3], + } + return stats + + def print_stats(self) -> None: + """輸出當前緩衝區統計資訊""" + stats = self.get_stats() + + print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===") + print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}") + print(f"Total Write Operations: {stats['total_writes']}") + print(f"Total Read Operations: {stats['total_reads']}") + print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})") + print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})") + print(f"Buffer Overflow Count: {stats['overflow_count']}") + print(f"Writing Threads: {stats['write_threads']}") + print(f"Reading Threads: {stats['read_threads']}") + + print("Top Writer Threads:") + for thread_id, count in stats['top_writer_threads']: + print(f" Thread {thread_id}: {count} writes") + + print("Top Reader Threads:") + for thread_id, count in stats['top_reader_threads']: + print(f" Thread {thread_id}: {count} reads") + print("=============================\n") + + def reset_stats(self) -> None: + """重置所有統計數據""" + with self._stats_lock: + self.concurrent_write_count.value = 0 + self.concurrent_read_count.value = 0 + self.total_write_count.value = 0 + self.total_read_count.value = 0 + self.overflow_count.value = 0 + self.thread_write_counts.clear() + self.thread_read_counts.clear() + + def __str__(self) -> str: + """返回緩衝區的字符串表示""" + return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})" \ No newline at end of file diff --git a/src/fc_network_adapter/tests/__init__.py b/src/fc_network_adapter/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/tests/test_asyncioManager.py b/src/fc_network_adapter/tests/test_asyncioManager.py new file mode 100644 index 0000000..27a790a --- /dev/null +++ b/src/fc_network_adapter/tests/test_asyncioManager.py @@ -0,0 +1,246 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +import time +import threading +import socket +import random +from pymavlink import mavutil +from pymavlink.dialects.v20 import common as mavlink2 + +from fc_network_adapter.asyncioManager import AsyncIOManager, Connection + +class MockMAVLinkSocket: + """模擬 pymavlink socket 行為的類，用於測試""" + + def __init__(self, name="mock", simulate_errors=False): + self.name = name + self.simulate_errors = simulate_errors + self.is_closed = False + self.received_msgs = [] + self.seq = 0 + + def recv_msg(self): + """模擬接收 MAVLink 消息""" + if self.is_closed: + return None + + # 隨機模擬網絡延遲 + time.sleep(random.uniform(0.01, 0.05)) + + # 偶爾模擬錯誤 + if self.simulate_errors and random.random() < 0.1: + raise Exception("Simulated network error") + + # 70% 的機率返回消息，30% 返回 None (模擬無數據可讀) + if random.random() > 0.3: + msg = self._generate_mavlink_message() + self.received_msgs.append(msg) + return msg + + return None + + def _generate_mavlink_message(self): + """生成模擬 MAVLink 消息""" + # 創建 MAVLink 實例 + mav = mavlink2.MAVLink(self) + + # 設置來源系統和組件 + mav.srcSystem = random.randint(1, 5) + mav.srcComponent = random.randint(1, 5) + + # 創建 HEARTBEAT 消息 + msg = mavlink2.MAVLink_heartbeat_message( + type=mavlink2.MAV_TYPE_QUADROTOR, + autopilot=mavlink2.MAV_AUTOPILOT_GENERIC, + base_mode=0, + custom_mode=0, + system_status=0, + mavlink_version=3 + ) + + # 打包消息 + msg.pack(mav) + self.seq += 1 + return msg + + def write(self, data): + """模擬發送數據""" + if self.is_closed: + raise Exception("Socket is closed") + + # 模擬寫入操作 + time.sleep(random.uniform(0.001, 0.01)) + + # 偶爾模擬錯誤 + if self.simulate_errors and random.random() < 0.1: + raise Exception("Simulated write error") + + return len(data) + + def close(self): + """關閉連接""" + self.is_closed = True + +def create_mock_mavlink_sockets(count=3): + """創建多個模擬 MAVLink socket""" + sockets = [] + for i in range(count): + # 每三個 socket 中有一個模擬錯誤 + simulate_errors = (i % 3 == 0) + socket = MockMAVLinkSocket(f"mock_{i}", simulate_errors) + sockets.append(socket) + return sockets + +def monitor_thread(io_manager): + """監視 AsyncIOManager 狀態的執行緒""" + print("Starting monitoring thread...") + + try: + while True: + # 獲取總體統計資訊 + stats = io_manager.get_stats() + print(f"\n=== AsyncIOManager Stats ===") + print(f"Total connections: {stats.get('connection_count', 0)}") + print(f"Total messages received: {stats.get('rx_count', 0)}") + print(f"Total bytes received: {stats.get('rx_bytes', 0)}") + print(f"Total errors: {stats.get('errors', 0)}") + + # 獲取每個連接的緩衝區狀態 + for socket_id in io_manager.connections: + buffer = io_manager.get_buffer(socket_id) + conn_stats = io_manager.get_stats(socket_id) + + if buffer and not io_manager.connections[socket_id].is_closed: + print(f"Connection {socket_id}: Buffer size={buffer.size()}, " + + f"Messages={conn_stats.get('rx_count', 0)}, " + + f"Last activity={time.time() - io_manager.connections[socket_id].last_activity:.1f}s ago") + + # 每秒更新一次 + time.sleep(1.0) + except KeyboardInterrupt: + print("Monitoring thread stopped.") + +def consumer_thread(io_manager, socket_id): + """從指定連接的緩衝區消費消息""" + print(f"Starting consumer for socket {socket_id}...") + + try: + while True: + # 獲取緩衝區 + buffer = io_manager.get_buffer(socket_id) + if not buffer: + print(f"Consumer {socket_id}: Buffer not found or connection closed") + break + + # 獲取所有消息 + messages = buffer.get_all() + if messages: + print(f"Consumer {socket_id}: Received {len(messages)} messages") + for msg in messages: + # 處理每條消息 (這裡只是示例，實際應用中可能有更複雜的處理) + print(f" - Message from SysID={msg.get_srcSystem()}, CompID={msg.get_srcComponent()}, Type={msg.get_type()}") + + # 短暫休眠，避免 CPU 空轉 + time.sleep(0.2) + except Exception as e: + print(f"Consumer {socket_id} stopped: {e}") + +def send_command_thread(io_manager, socket_ids): + """定期向所有連接發送命令""" + print("Starting command sender thread...") + + # 創建一個 MAVLink 實例用於生成消息 + mav = mavlink2.MAVLink(None) + mav.srcSystem = 255 # GCS system ID + mav.srcComponent = 0 + + try: + count = 0 + while True: + # 等待 2 秒 + time.sleep(2.0) + count += 1 + + # 創建一個命令消息 + msg = mavlink2.MAVLink_command_long_message( + target_system=1, + target_component=1, + command=mavlink2.MAV_CMD_REQUEST_PROTOCOL_VERSION, + confirmation=0, + param1=0, param2=0, param3=0, + param4=0, param5=0, param6=0, param7=0 + ) + + # 向所有連接發送消息 + for socket_id in socket_ids: + if socket_id in io_manager.connections and not io_manager.connections[socket_id].is_closed: + try: + # 打包並發送消息 + result = io_manager.send_message(socket_id, msg) + print(f"Sent command to socket {socket_id}: {'Success' if result else 'Failed'}") + except Exception as e: + print(f"Error sending to socket {socket_id}: {e}") + + # 每 10 次迭代關閉一個連接 (用於演示關閉功能) + if count % 10 == 0 and socket_ids: + socket_id = socket_ids.pop(0) + print(f"Closing connection {socket_id}...") + io_manager.remove_connection(socket_id) + except Exception as e: + print(f"Command sender thread stopped: {e}") + +def main(): + print("AsyncIOManager Example") + print("======================") + + # 創建 AsyncIOManager 實例 + io_manager = AsyncIOManager() + + # 啟動 I/O 管理器 + io_manager.start() + print("AsyncIOManager started") + + # 創建模擬 MAVLink socket + mock_sockets = create_mock_mavlink_sockets(5) + socket_ids = [] + + # 註冊連接 + for i, socket in enumerate(mock_sockets): + socket_id = io_manager.register_connection(socket) + socket_ids.append(socket_id) + print(f"Registered socket {i} with ID {socket_id}") + + # 啟動監視執行緒 + monitor = threading.Thread(target=monitor_thread, args=(io_manager,)) + monitor.daemon = True + monitor.start() + + # 為每個連接啟動消費者執行緒 + consumers = [] + for socket_id in socket_ids: + consumer = threading.Thread(target=consumer_thread, args=(io_manager, socket_id)) + consumer.daemon = True + consumer.start() + consumers.append(consumer) + + # 啟動命令發送執行緒 + sender = threading.Thread(target=send_command_thread, args=(io_manager, socket_ids.copy())) + sender.daemon = True + sender.start() + + print("\nPress Ctrl+C to exit...") + try: + # 運行一段時間然後退出 + time.sleep(30) + except KeyboardInterrupt: + pass + finally: + # 清理資源 + print("\nShutting down...") + io_manager.stop() + print("AsyncIOManager stopped") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_ringBuffer.py b/src/fc_network_adapter/tests/test_ringBuffer.py new file mode 100644 index 0000000..563087b --- /dev/null +++ b/src/fc_network_adapter/tests/test_ringBuffer.py @@ -0,0 +1,152 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +import time +import threading + +from fc_network_adapter.ringBuffer import RingBuffer + + +def producer(buffer, count, interval=0.01): + """生產者：向緩衝區添加資料""" + print(f"Producer started (thread {threading.get_ident()})") + for i in range(count): + # 嘗試寫入數據，直到成功 + while not buffer.put(f"Item-{i}"): + print(f"Buffer full, producer waiting... (thread {threading.get_ident()})") + time.sleep(0.1) + + print(f"Produced: Item-{i}, buffer size: {buffer.size()}") + time.sleep(interval) # 模擬生產過程 + + print(f"Producer finished (thread {threading.get_ident()})") + +def consumer(buffer, max_items, interval=0.05): + """消費者：從緩衝區讀取資料""" + print(f"Consumer started (thread {threading.get_ident()})") + items_consumed = 0 + + while items_consumed < max_items: + # 嘗試讀取數據 + item = buffer.get() + if item: + print(f"Consumed: {item}, buffer size: {buffer.size()}") + items_consumed += 1 + else: + print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})") + + time.sleep(interval) # 模擬消費過程 + + print(f"Consumer finished (thread {threading.get_ident()})") + +def batch_consumer(buffer, interval=0.2): + """批量消費者：一次性讀取緩衝區中的所有資料""" + print(f"Batch consumer started (thread {threading.get_ident()})") + + for _ in range(5): # 執行5次批量讀取 + time.sleep(interval) # 等待緩衝區積累數據 + items = buffer.get_all() + if items: + print(f"Batch consumed {len(items)} items: {items}") + else: + print("Buffer empty for batch consumer") + + print(f"Batch consumer finished (thread {threading.get_ident()})") + +def demonstrate_multi_writer(): + """示範多個寫入執行緒同時操作緩衝區""" + print("\n=== Demonstrating Multiple Writers ===") + buffer = RingBuffer(capacity=80) + + # 創建多個生產者執行緒 + threads = [] + for i in range(3): + thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1))) + threads.append(thread) + thread.start() + + # 等待所有執行緒完成 + for thread in threads: + thread.join() + + buffer.print_stats() # 印出統計資訊 + + # 讀出所有剩餘資料 + remaining = buffer.get_all() + print(f"Remaining items in buffer after multiple writers: {remaining}") + +def demonstrate_basic_usage(): + """示範基本使用方式""" + print("\n=== Demonstrating Basic Usage ===") + # 創建緩衝區 + buffer = RingBuffer(capacity=20, buffer_id=7) + + # 檢查初始狀態 + print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 添加幾個項目 + for i in range(5): + buffer.put(f"Test-{i}") + + # 檢查狀態 + print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 讀取一個項目 + item = buffer.get() + print(f"Read item: {item}") + print(f"After reading 1 item - Content Size: {buffer.size()}") + + # 添加更多項目直到滿 + items_added = 0 + while not buffer.is_full(): + buffer.put(f"Fill-{items_added}") + items_added += 1 + + print(f"Added {items_added} more items until full") + print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 嘗試添加到已滿的緩衝區 + result = buffer.put("Overflow") + print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}") + + # 獲取所有項目 + all_items = buffer.get_all() + print(f"All items in buffer: {all_items}") + print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}") + + # 印出統計資訊 + buffer.print_stats() + +def demonstrate_producer_consumer(): + """示範生產者-消費者模式""" + print("\n=== Demonstrating Producer-Consumer Pattern ===") + buffer = RingBuffer(capacity=16) + + # 創建生產者和消費者執行緒 + producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1)) + consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2)) + batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5)) + + # 啟動執行緒 + producer_thread.start() + consumer_thread.start() + batch_thread.start() + + # 等待執行緒完成 + producer_thread.join() + consumer_thread.join() + batch_thread.join() + + # 檢查最終狀態 + print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}") + + buffer.print_stats() + +if __name__ == "__main__": + # 展示各種使用場景 + # demonstrate_basic_usage() + # demonstrate_producer_consumer() + demonstrate_multi_writer() + + print("\nAll demonstrations completed!") \ No newline at end of file From 6fab72ac97a415d2cd97176f5618c24ffb01fedd Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 9 Jun 2025 17:27:31 +0800 Subject: [PATCH 061/146] =?UTF-8?q?=E9=87=8D=E5=A4=A7=E6=9B=B4=E6=96=B0=20?= =?UTF-8?q?:=201.=20=E4=BF=AE=E6=AD=A3=E4=B8=8D=E5=90=8C=20socket=20?= =?UTF-8?q?=E8=BD=89=E5=82=B3=E9=96=93=E6=95=88=E8=83=BD=E4=BD=8E=E8=90=BD?= =?UTF-8?q?=E5=95=8F=E9=A1=8C=20=E6=96=BC=20mavlinkObject.py=20=E9=87=8D?= =?UTF-8?q?=E6=A7=8B=20mavlink=5Fobject=20=E5=A2=9E=E5=8A=A0=20async=5Fio?= =?UTF-8?q?=5Fmanager=20(=E7=89=88=E6=9C=AC=E7=9B=AE=E6=A8=99=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=E8=AA=BF=E6=95=B4=20=E5=AF=AB=E6=96=BC=E7=A8=8B?= =?UTF-8?q?=E5=BC=8F=E7=A2=BC=E8=A8=BB=E8=A7=A3=E4=B8=AD)=202.=20=E8=AA=BF?= =?UTF-8?q?=E6=95=B4=E5=90=84=E5=80=8B=E7=A8=8B=E5=BC=8F=E7=A2=BC=E7=9A=84?= =?UTF-8?q?=E6=AA=94=E6=A1=88=E4=BD=8D=E7=BD=AE=20=E8=AE=93=E6=B8=AC?= =?UTF-8?q?=E8=A9=A6=E7=94=A8=E7=A8=8B=E5=BC=8F=E8=88=87=E5=B0=88=E6=A1=88?= =?UTF-8?q?=E7=A8=8B=E5=BC=8F=E5=88=86=E9=9B=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/asyncioManager.py | 290 -------- .../fc_network_adapter/devRun.py | 624 ------------------ .../fc_network_adapter/mavlinkDevice.py | 2 +- .../fc_network_adapter/mavlinkObject.py | 619 +++++++++++------ .../fc_network_adapter/mavlinkPublish.py | 4 +- .../fc_network_adapter/test_mavlinkObject.py | 0 src/fc_network_adapter/tests/devRun.py | 448 +++++++++++++ .../tests/test_asyncioManager.py | 246 ------- .../tests/test_mavlinkObject.py | 468 +++++++++++++ 9 files changed, 1342 insertions(+), 1359 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/asyncioManager.py delete mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py create mode 100644 src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py create mode 100644 src/fc_network_adapter/tests/devRun.py delete mode 100644 src/fc_network_adapter/tests/test_asyncioManager.py create mode 100644 src/fc_network_adapter/tests/test_mavlinkObject.py diff --git a/src/fc_network_adapter/fc_network_adapter/asyncioManager.py b/src/fc_network_adapter/fc_network_adapter/asyncioManager.py deleted file mode 100644 index 9a9ba65..0000000 --- a/src/fc_network_adapter/fc_network_adapter/asyncioManager.py +++ /dev/null @@ -1,290 +0,0 @@ -import asyncio -import socket -import threading -import time -import logging -from typing import Dict, Any, Optional, List, Callable, Tuple -from pymavlink import mavutil -from dataclasses import dataclass, field -from .ringBuffer import RingBuffer - -# 設置日誌 -logger = logging.getLogger("async_io_manager") - -@dataclass -class Connection: - """表示一個 MAVLink 連接""" - socket_id: int - socket: Any # pymavlink socket 或相容物件 - buffer: RingBuffer = field(default_factory=lambda: RingBuffer(1024)) - last_activity: float = field(default_factory=time.time) - is_closed: bool = False - stats: Dict[str, int] = field(default_factory=lambda: {"rx_count": 0, "rx_bytes": 0, "errors": 0}) - -class AsyncIOManager: - """ - 異步 I/O 管理器 - 負責高效處理所有 MAVLink socket 的 I/O 操作 - """ - _instance = None - _lock = threading.Lock() - - def __new__(cls, *args, **kwargs): - with cls._lock: - if cls._instance is None: - cls._instance = super(AsyncIOManager, cls).__new__(cls) - cls._instance._initialized = False - return cls._instance - - def __init__(self): - if self._initialized: - return - - self._initialized = True - self.connections: Dict[int, Connection] = {} - self.next_socket_id = 0 - self._running = False - self._event_loop = None - self._thread = None - self.check_interval = 0.01 # 檢查間隔，單位：秒 - - def start(self): - """啟動異步 I/O 管理器""" - if self._running: - logger.warning("AsyncIOManager already running") - return - - self._running = True - self._thread = threading.Thread(target=self._run_event_loop, daemon=True) - self._thread.start() - logger.info("AsyncIOManager started") - - def _run_event_loop(self): - """在專用執行緒中執行 asyncio 事件循環""" - try: - # 創建新的事件循環 - loop = asyncio.new_event_loop() - asyncio.set_event_loop(loop) - self._event_loop = loop - - # 添加主要協程任務 - loop.create_task(self._process_all_connections()) - - # 運行事件循環 - loop.run_forever() - except Exception as e: - logger.error(f"Error in AsyncIOManager event loop: {e}") - finally: - if loop.is_running(): - loop.stop() - logger.info("AsyncIOManager event loop stopped") - - async def _process_all_connections(self): - """處理所有連接的主循環""" - while self._running: - for socket_id, conn in list(self.connections.items()): - if conn.is_closed: - continue - - try: - # 非阻塞模式讀取數據 - await asyncio.get_event_loop().run_in_executor( - None, self._read_from_connection, conn - ) - except Exception as e: - conn.stats["errors"] += 1 - logger.error(f"Error processing connection {socket_id}: {e}") - - # 短暫休息，讓出 CPU - await asyncio.sleep(self.check_interval) - - def _read_from_connection(self, conn: Connection): - """從連接讀取數據，此函數在執行器中運行""" - try: - # 使用 mavlink 的非阻塞讀取 - msg = conn.socket.recv_msg() - if msg: - # 收到消息，更新統計資訊 - conn.last_activity = time.time() - conn.stats["rx_count"] += 1 - conn.stats["rx_bytes"] += len(msg.get_msgbuf()) - - # 將消息放入緩衝區 - if not conn.buffer.put(msg): - # 緩衝區已滿，記錄錯誤 - logger.warning(f"Buffer full for connection {conn.socket_id}, message dropped") - except Exception as e: - conn.stats["errors"] += 1 - if "Connection refused" not in str(e): # 忽略常見的連接拒絕錯誤 - logger.error(f"Error reading from socket {conn.socket_id}: {e}") - - def stop(self): - """停止異步 I/O 管理器""" - if not self._running: - return - - self._running = False - - # 停止事件循環 - if self._event_loop: - asyncio.run_coroutine_threadsafe( - self._shutdown_loop(), self._event_loop - ) - - # 等待執行緒結束 - if self._thread and self._thread.is_alive(): - self._thread.join(timeout=2.0) - if self._thread.is_alive(): - logger.warning("AsyncIOManager thread did not terminate cleanly") - - logger.info("AsyncIOManager stopped") - - async def _shutdown_loop(self): - """關閉事件循環""" - # 關閉所有連接 - for socket_id in list(self.connections.keys()): - self.remove_connection(socket_id) - - # 停止事件循環 - loop = asyncio.get_event_loop() - loop.stop() - - def register_connection(self, socket: Any) -> int: - """ - 註冊新的連接 - - Args: - socket: pymavlink socket 或相容物件 - - Returns: - int: 分配的 socket_id - """ - socket_id = self.next_socket_id - self.next_socket_id += 1 - - # 創建新的連接 - connection = Connection(socket_id=socket_id, socket=socket) - self.connections[socket_id] = connection - - logger.info(f"Registered new connection with ID {socket_id}") - return socket_id - - def remove_connection(self, socket_id: int) -> bool: - """ - 移除連接 - - Args: - socket_id: 連接 ID - - Returns: - bool: 成功移除返回 True - """ - if socket_id in self.connections: - conn = self.connections[socket_id] - conn.is_closed = True - - try: - # 關閉原始 socket - if hasattr(conn.socket, 'close'): - conn.socket.close() - except Exception as e: - logger.error(f"Error closing socket {socket_id}: {e}") - - # 從字典中移除 - self.connections.pop(socket_id) - logger.info(f"Removed connection with ID {socket_id}") - return True - return False - - def get_buffer(self, socket_id: int) -> Optional[RingBuffer]: - """ - 獲取指定連接的緩衝區 - - Args: - socket_id: 連接 ID - - Returns: - RingBuffer 或 None（如果連接不存在） - """ - if socket_id in self.connections: - return self.connections[socket_id].buffer - return None - - def get_stats(self, socket_id: int = None) -> Dict: - """ - 獲取連接統計資訊 - - Args: - socket_id: 指定連接 ID，None 表示所有連接 - - Returns: - Dict: 統計資訊 - """ - if socket_id is not None and socket_id in self.connections: - return self.connections[socket_id].stats - - # 彙總所有連接的統計資訊 - total_stats = {"rx_count": 0, "rx_bytes": 0, "errors": 0, "connection_count": len(self.connections)} - for conn in self.connections.values(): - for key, value in conn.stats.items(): - if key in total_stats: - total_stats[key] += value - return total_stats - - def send_message(self, socket_id: int, message: Any) -> bool: - """ - 發送 MAVLink 訊息 - - Args: - socket_id: 連接 ID - message: 要發送的訊息 - - Returns: - bool: 成功發送返回 True - """ - if socket_id not in self.connections: - return False - - conn = self.connections[socket_id] - if conn.is_closed: - return False - - try: - # 使用 asyncio.run_coroutine_threadsafe 在事件循環中執行 IO 操作 - future = asyncio.run_coroutine_threadsafe( - self._send_message_async(conn, message), - self._event_loop - ) - # 等待操作完成（可選） - return future.result(timeout=1.0) - except Exception as e: - logger.error(f"Error sending message on socket {socket_id}: {e}") - return False - - async def _send_message_async(self, conn: Connection, message: Any) -> bool: - """在事件循環中異步發送訊息""" - try: - # 調用執行器進行實際發送操作 - return await asyncio.get_event_loop().run_in_executor( - None, self._send_message_sync, conn, message - ) - except Exception as e: - conn.stats["errors"] += 1 - logger.error(f"Async error sending message: {e}") - return False - - def _send_message_sync(self, conn: Connection, message: Any) -> bool: - """同步發送訊息函數，在執行器中運行""" - try: - if hasattr(message, 'get_msgbuf'): - # 如果是 MAVLink 訊息對象 - conn.socket.write(message.get_msgbuf()) - else: - # 假設是已編碼的字節 - conn.socket.write(message) - conn.last_activity = time.time() - return True - except Exception as e: - conn.stats["errors"] += 1 - logger.error(f"Error in sync message send: {e}") - return False \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py deleted file mode 100644 index c638659..0000000 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ /dev/null @@ -1,624 +0,0 @@ -# 基礎功能的 import -import queue -import time - - -# ROS2 的 import -import rclpy - -# mavlink 的 import -from pymavlink import mavutil - -# 自定義的 import -import mavlinkObject as mo -import mavlinkDevice as md - -# ====================== 分割線 ===================== - -test_item = 12 -running_time = 3000 -print('test_item : ', test_item) - -''' -測試項 個位數 表示分離的功能 - -測試項 1X 表示 mavlink_object 的功能 測試連線的能力 -''' - -if test_item == 1: - # 測試 mavlink_object 中 updateMultiplexingList 的輸出 - print('===> Start of Program .Test ', test_item) - mavlink_object_none = mo.mavlink_object(None) - - print('=====================') - print('正常範例') - mavlink_object_none.multiplexingToAnalysis = [] - mavlink_object_none.multiplexingToReturn = [] - # mavlink_object_none.multiplexingToSwap = [[]] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - print('multiplexingList : ', mavlink_object_none._multiplexingList) - - print('=====================') - print('錯誤範例 長度不一致') - mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - - print('=====================') - print('正常範例') - mo.swap_queues.append(queue.Queue()) - mo.swap_queues.append(queue.Queue()) - mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5] - mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10] - mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - print('multiplexingList : ', mavlink_object_none._multiplexingList) - - print('=====================') - print('錯誤範例 multiplexingToAnalysis 不可以有 -1') - mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - - print('<=== End of Program') - -elif test_item == 2: - # 測試 mavlink_object 創建時 socket_id 是否正確 - # 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試 - print('===> Start of Program .Test ', test_item) - mavlink_object_none1 = mo.mavlink_object(None) - mavlink_object_none2 = mo.mavlink_object(None) - mavlink_object_none3 = mo.mavlink_object(None) - del mavlink_object_none2 - - print(len(mo.swap_queues)) - - mavlink_object_none2 = mo.mavlink_object(None) - print(len(mo.swap_queues)) - - print(mavlink_object_none1._multiplexingList) - - print('<=== End of Program') - -elif test_item == 10: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 - # 只啟用了 mavlink_object 的功能 - print('===> Start of Program .Test ', test_item) - - # 創建一個空的通道 這個通道的 socket_id 是 0 - mavlink_object_none = mo.mavlink_object(None) - - # 創建另一個通道 - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object1 = mo.mavlink_object(mavlink_socket) - # 設定通道轉發的參數 - mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE - mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT - mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD - - # print(mavlink_object1.multiplexingToSwap) - # print(mo.swap_queues) - - # 啟動通道 - mavlink_object1.run() - - # 確認轉拋的設定有沒有錯 - print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList) - - # 運行幾秒並印出 queue 的資料 - start_time = time.time() - while time.time() - start_time < running_time: - while not mo.fixed_stream_bridge_queue.empty(): - print('fixed_stream_bridge_queue:') - t = mo.fixed_stream_bridge_queue.get() - print('from {} : {}'.format(t[0],t[2])) - while not mo.return_packet_processor_queue.empty(): - print('return_packet_processor_queue:') - t = mo.return_packet_processor_queue.get() - print('from {} : {}'.format(t[0],t[2])) - # print(t[2].get_msgbuf()) - # print(t[2].type) - - for n in range(len(mo.swap_queues)): - q = mo.swap_queues[n] - while not q.empty(): - print('swap_queues:') - t = q.get() - print('{} gets : {}'.format(n,t)) - - - # 結束程式 退出所有 thread - mavlink_object1.running = False - mavlink_object1.thread.join() - mavlink_socket.close() - print('<=== End of Program') - -elif test_item == 11: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 - # 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能 - print('===> Start of Program .Test ', test_item) - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - - # 創建通道 - connection_string="udp:127.0.0.1:15551" - mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object2 = mo.mavlink_object(mavlink_socket) - # mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT - # mavlink_object2.multiplexingToReturn = [] - - # 啟動連線的模組 - mavlink_object2.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(2) - print("=== connection established! ===") - - # 印出目前所有 mavlink_systems 的內容 - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - start_time = time.time() - show_time = time.time() - compid = 1 - while time.time() - start_time < running_time: - 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_socket.last_seq) - print("===================") - - # 結束程式 退出所有 thread - mavlink_object2.stop() - mavlink_object2.thread.join() - analyzer.stop() - - analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance - analyzer.thread.join() - mavlink_socket.close() - print('<=== End of Program') - -elif test_item == 12: - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 初始化輸入通道 - 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] # only HEARTBEAT - - # 初始化輸出通道 - connection_string_out="udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [0] - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 讓兩個通道互相傳輸 - mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all - mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all - - # 啟動通道 - mavlink_object_in.run() - mavlink_object_out.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - # print(type(mavlink_socket_out)) - # print(type(mavlink_socket_out.mav)) - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object : ', test) - except queue.Empty: - pass - - if (time.time() - show_time) >= 2: - show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - print("===================") - - - - # 結束程式 退出所有 thread - mavlink_object_in.stop() - mavlink_object_in.thread.join() - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_in.close() - mavlink_socket_out.close() - - analyzer.stop() - print('<=== End of Program') - -elif test_item == 13: - # 這邊測試看看能否狸貓換太子的方式 - # 把原來 pymavlink 的 mavtcpin 換成 unix socket - print('===> Start of Program .Test ', test_item) - - import os - import socket - - # 建立一個 mavtcpin 實例 - import mavunixin - - - - - # ===== 以下就是按照原本測試 12 的流程 ===== - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 初始化輸入通道 - mavlink_socket_unix = mavunixin.mavunixin("unix:/tmp/unix_socket_mavlink.sock", source_system=1, source_component=1) - mavlink_object_in = mo.mavlink_object(mavlink_socket_unix) - mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT - - # 停留五秒 我要啟動另一邊 - time.sleep(5) - - # 初始化輸出通道 - connection_string_out="udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [0] - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 讓兩個通道互相傳輸 - mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all - mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all - - # 啟動通道 - mavlink_object_in.run() - mavlink_object_out.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - # print(type(mavlink_socket_out)) - # print(type(mavlink_socket_out.mav)) - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object : ', test) - except queue.Empty: - pass - - if (time.time() - show_time) >= 2: - show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - print("===================") - - - - # 結束程式 退出所有 thread - mavlink_object_in.stop() - mavlink_object_in.thread.join() - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_unix.close() - mavlink_socket_out.close() - - analyzer.stop() - print('<=== End of Program') - -elif test_item == 20: - # 這邊測試 node 生成 topic 的功能 - # 利用 空的通道 發出假的 heartbeat 封包 - print('===> Start of Program .Test ', test_item) - rclpy.init() # 注意要初始化 rclpy 才能使用 node - - from pymavlink.dialects.v20 import common as mavlink2 - - sysid = 1 - compid = 1 - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - - mav = mavlink2.MAVLink(None) - mav.srcSystem = sysid - mav.srcComponent = compid - - # 這是一個 heartbeat 封包 - fake_heartbeat = mavlink2.MAVLink_heartbeat_message( - type=mavlink2.MAV_TYPE_QUADROTOR, - autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode=3, - custom_mode=0, - system_status=0, - mavlink_version=3 - ) - mavlink_bytes = fake_heartbeat.pack(mav) - fake_heartbeat_msg = mav.parse_char(mavlink_bytes) - - print(analyzer.mavlink_systems) - - mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data - time.sleep(0.1) - print(analyzer.mavlink_systems) - - # ROS2 初始化 - analyzer._init_node() - - # 創建 ROS2 topic - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid - print("topic created") - time.sleep(5) - - # 丟出 topic - analyzer.emit_info() - - # 結束程式 - analyzer.running = False - analyzer.destroy_node() - - rclpy.shutdown() - analyzer.stop() - analyzer.thread.join() - print('<=== End of Program') - -elif test_item == 21: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - - 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_socket = mavutil.mavlink_connection(connection_string) - mavlink_object3 = mo.mavlink_object(mavlink_socket) - # 啟動通道 - mavlink_object3.run() - - - print('=== waiting for mavlink data ...') - time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - compid = 1 - sysid = 1 - start_time = time.time() - analyzer.create_flightMode(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") - - 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(1) - except KeyboardInterrupt: - break - - - # 程式結束 - 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') - -elif test_item == 22: - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - 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_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" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [] - - - - # 讓兩個通道互相傳輸 - mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all - mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - - # 啟動通道 - mavlink_object_in.run() - mavlink_object_out.run() - - print('waiting for mavlink data ...') - time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - compid = 1 - sysid = 1 - show_time = time.time() - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) - print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") - - print("start emit info") - - start_time = time.time() - show_time = time.time() - show_time2 = time.time() - - while time.time() - start_time < running_time: - if (time.time() - show_time2) >= 1: - analyzer.emit_info() # 這邊是測試 node 的運行 - # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] - fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] - sss = mavutil.mode_string_v10(fmsg) - # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) - print("目前的飛行模式 : {}".format(sss)) - show_time2 = time.time() - - # if (time.time() - show_time) >= 2: - # show_time = time.time() - # for sysid in analyzer.mavlink_systems: - # for compid in analyzer.mavlink_systems[sysid].components: - # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - # print("===================") - - - - analyzer.destroy_node() - rclpy.shutdown() - - - # 結束程式 退出所有 thread - mavlink_object_in.stop() - mavlink_object_in.thread.join() - mavlink_socket_in.close() - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_out.close() - analyzer.stop() - analyzer.thread.join() - - - 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 4ae4c98..47a1c92 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -1,6 +1,6 @@ # 自定義的 import -from theLogger import setup_logger +from .theLogger import setup_logger # ====================== 分割線 ===================== diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index b55107c..d0231ed 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,8 +1,7 @@ ''' # 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 queue stack 傳送到橋接器 -# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流 -# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue +# 匯流排接到訊息後透過 ring buffer 傳送到橋接器 +# 會有兩種 RingBuffer 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 # 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 # 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 @@ -13,8 +12,11 @@ # 基礎功能的 import import threading -import queue +# import queue import time +import asyncio +from enum import Enum, auto +# from typing import Dict, List, Optional, Set, Any, Tuple # mavlink 的 import from pymavlink import mavutil @@ -23,38 +25,17 @@ from pymavlink import mavutil from rclpy.node import Node # 自定義的 import -from mavlinkDevice import mavlink_device -from mavlinkPublish import mavlink_publisher -from theLogger import setup_logger - +from .mavlinkDevice import mavlink_device, mavlink_systems +from .mavlinkPublish import mavlink_publisher +from .theLogger import setup_logger +from .ringBuffer import RingBuffer # ====================== 分割線 ===================== logger = setup_logger("mavlinkObject.py") -# 基礎功能的 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, mavlink_systems -from mavlinkPublish import mavlink_publisher -from theLogger import setup_logger - -# ====================== 分割線 ===================== - -logger = setup_logger("mavlinkObject.py") - -fixed_stream_bridge_queue = queue.Queue() -return_packet_processor_queue = queue.Queue() -swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer +stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255) +return_packet_ring = RingBuffer(capacity=256, buffer_id=254) # ====================== 分割線 ===================== @@ -75,6 +56,8 @@ class mavlink_bridge(Node, mavlink_publisher): node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 藉由控制 ros2 的機制再把 device object 的資訊發送出去 + fixed_stream_bridge_queue 置換成 stream_bridge_ring + ps. 我限制了這個 class 只能有一個 instance ''' _instance = None @@ -106,13 +89,25 @@ class mavlink_bridge(Node, mavlink_publisher): # === Thread 區塊 === def _run_thread(self): - # start_time = time.time() # debug + start_time = time.time() # debug + run_loop_count = 0 # debug logger.info('Start of mavlink_bridge._run_thread') - # 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 + # 從 Queue stream_bridge_ring 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 while self.running: - if fixed_stream_bridge_queue.empty(): + # # 這個迴圈每秒鐘被執行了幾輪? # 這整段都是 debug 用的 + # current_time = time.time() + # if (current_time - start_time) >= 1.0: + # logger.info(f'mavlink_bridge._run_thread loop count: {run_loop_count}') + # run_loop_count = 0 + # start_time = current_time + # else: + # run_loop_count += 1 + # # ========================= + + + if stream_bridge_ring.is_empty(): continue - msg_pack = fixed_stream_bridge_queue.get() + msg_pack = stream_bridge_ring.get() msg = msg_pack[2] sysid = msg.get_srcSystem() @@ -150,7 +145,7 @@ class mavlink_bridge(Node, mavlink_publisher): this_component.emitParams['flightMode'] = msg # debug # print("mav_type : ", msg.type) # debug - # print("get mode :", mavutil.mode_string_v10(msg)) # debug + print("get mode :", mavutil.mode_string_v10(msg)) # debug # print("record mode :", this_component.emitParams['flightMode_mode']) # debug elif msg_id == 30: # ATTITUDE 處理 @@ -201,24 +196,45 @@ class mavlink_bridge(Node, mavlink_publisher): # ====================== 分割線 ===================== -class mavlink_object(): +''' +本次改版的暫時記錄註解於此 +1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 + 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 + 而 multiplexingToSwap 會完全被移除代替方式下一條描述 +2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 + (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) + 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 + 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 +3. mavlink_object 需要加上 state 去管理其狀態 +4. mavlink_object 需要加上 target port 去管理寫出的目標 +5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 +6. 基於第1,2項 updateMultiplexingList 會被完全移除 +7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object +8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 + +''' + +# 定義 mavlink_object 的狀態 +class MavlinkObjectState(Enum): + INIT = auto() # 初始化狀態 + RUNNING = auto() # 運行中狀態 + ERROR = auto() # 錯誤狀態 + CLOSED = auto() # 已關閉狀態 + +class mavlink_object: ''' 每個 mavlink bus 都會有一個 mavlink_object - 其中主要是 thread 做統計封包與分流 - 分流表的控制在三個 list 分別為 - multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 - multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToSwap : 這個 list 是用來分流到轉拋串流的 - 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 直接透過 socket 寫出 ''' - mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } - socket_num = 0 # 用來記錄目前的 socket 數量 + mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } + socket_num = 0 # 用來記錄目前的 socket 數量 + def __new__(cls, *args, **kwargs): # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號 # socket_id 從 0 開始 - instance = super().__new__(cls) socket_id = 0 for k in cls.mavlinkObjects.keys(): @@ -232,180 +248,389 @@ class mavlink_object(): return instance def __init__(self, socket): + # 登入所需的 socket self.mavlink_socket = socket - # 這邊變數是執行的時候被使用的 不要直接寫入它 - self._multiplexingList = [] - # 存放要發送的訊息的 queue 或稱 buffer - self.output_buffer = queue.Queue() - if self.socket_id >= len(swap_queues): - swap_queues.append(self.output_buffer) - else: - swap_queues[self.socket_id] = self.output_buffer - + + # 存放目前是否分流到固定串流橋接器和回傳封包處理器的標誌 # 都是要做的 把這個判斷拿掉 + # self.send_to_bridge = True + # self.send_to_return = False + + # 用於主線程發送消息的緩衝區 + self.outgoing_msg_lock = threading.Lock() + self.outgoing_msgs = [] + + # 記錄訊息過濾類型 (可選) + self.bridge_msg_types = [0] # 默認只處理 HEARTBEAT + self.return_msg_types = [] + + # 目標端口 + self.target_sockets = [] + # 關聯到全域變數 global mavlink_systems self.mavlink_systems = mavlink_systems - - # 這三個 list 用來分配不同的訊息到不同的 queue - self.multiplexingToAnalysis = [ - 0, # HEARTBEAT # 挺必要的項目 - # 24, # GPS_RAW_INT - # 30, # ATTITUDE - # 33, # GLOBAL_POSITION_INT - # 74, # VFR_HUD - # 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [] - self.multiplexingToSwap = [ - [] for _ in range(len(swap_queues)) - ] - - # 刷新其他 mavlink_object 的 multiplexingToSwap - for k, object in self.mavlinkObjects.items(): - if (k != self.socket_id) and (len(object.multiplexingToSwap) <= self.socket_id): - object.multiplexingToSwap.append([]) - object.updateMultiplexingList() - - logger.info('mavlink_object instance {} created'.format(self.socket_id)) + + # 物件變數 + self.task = None # Task reference + self.dirtyDataCount = 0 # 髒資料計數器 + self.dirtyDataMax = 10 # 髒資料容許閾值 + + self.state = MavlinkObjectState.INIT + logger.info(f'mavlink_object instance {self.socket_id} created') def __del__(self): - # 停下自己的 thread - if self.mavlink_socket != None: - self.mavlink_socket.close() - self.stop() - - # 移除其他 mavlink_object 的 multiplexingToSwap - for k, object in self.mavlinkObjects.items(): - if (k != self.socket_id) and (len(object.multiplexingToSwap) > self.socket_id): - object.multiplexingToSwap[self.socket_id] = [] - object.updateMultiplexingList() - - # 移除自己的 swap_queues - swap_queues[self.socket_id] = None - + # 停止 asyncio task + self.stop() + + # 關閉 socket + if hasattr(self, 'mavlink_socket') and self.mavlink_socket: + try: + self.mavlink_socket.close() + except: + pass + # 處理 class 的 instance 記錄 - self.socket_num -= 1 - self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance - - # logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替 - # print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + if hasattr(self.__class__, 'socket_num'): + self.__class__.socket_num -= 1 + + if hasattr(self.__class__, 'mavlinkObjects') and hasattr(self, 'socket_id'): + if self.socket_id in self.__class__.mavlinkObjects: + self.__class__.mavlinkObjects.pop(self.socket_id) + + # 這段不知道怎麼了 反正會一直讓 logger ERROR 我先關掉 # try: - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # logger.info(f'mavlink_object instance {self.socket_id} deleted') # except Exception as e: # print(f"Error logging in __del__: {e}") - # if 'logger' in globals() and logger: - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) - def run(self): - # TODO 檢查 socket 是否有連線 - self.thread = threading.Thread(target=self._run_thread) - self.running = self.updateMultiplexingList() - self.thread.start() + def start(self): + """啟動 mavlink_object 處理循環""" + if self.state == MavlinkObjectState.RUNNING: + logger.warning(f"mavlink_object {self.socket_id} is already running") + return + + self.state = MavlinkObjectState.RUNNING + # 實際的啟動會由 async_io_manager 處理 def stop(self): - 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() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 - - # 處理接收到的封包 + """停止 mavlink_object 處理循環""" + if self.state in (MavlinkObjectState.CLOSED, MavlinkObjectState.ERROR): + return + + self.state = MavlinkObjectState.CLOSED + if self.task: + if not self.task.done(): + self.task.cancel() + + async def process_data(self): + """處理 mavlink 數據的主要 asyncio 協程""" + logger.info(f'Start of mavlink_object.process_data id: {self.socket_id}') + + while self.state == MavlinkObjectState.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: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...' - # 統計收到的訊息 & 透過 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) - - # break # Debug function can put here you can see the input data from mavlink - - # 將訊息依照 multiplexing list 分發到不同的 queue - for i in range(len(self._multiplexingList)): - if (self._multiplexingList[i] == []): - continue - elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1): - if i == 0: - fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) - elif i == 1: - return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) - else: - _queue = swap_queues[i-2] - _queue.put(mavlinkMsg) - - # 處理要送出的封包 - # 如果 有資料在 output_buffer 中則將其取出並發送 - # 沒有就略過發送 + logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") + self.dirtyDataCount += 1 + + if self.dirtyDataCount >= self.dirtyDataMax: + logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") + self.state = MavlinkObjectState.ERROR + # 不直接退出，嘗試容忍髒數據 + await asyncio.sleep(0.01) # 短暫暫停 + continue + try: - mavlinkMsg_send = self.output_buffer.get(block=False) - except queue.Empty: - mavlinkMsg_send = None - # except Exception as e: - # logger.error(f"Error getting data from output_buffer: {e}") - # mavlinkMsg_send = None - - if mavlinkMsg_send: - # self.mavlink_socket.mav.send(mavlinkMsg_send) - self.mavlink_socket.write(mavlinkMsg_send.get_msgbuf()) # 這邊會將封包寫入 socket 中 - - # thread 結束 - logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) - - def updateMultiplexingList(self): - ''' - 更新 multiplexing list 並做簡單的檢查 - ''' - # 檢查 multiplexingToAnalysis 與 multiplexingToReturn 是否有 -1 值 - check = (-1 in self.multiplexingToAnalysis) or (-1 in self.multiplexingToReturn) - if check: - logger.error('MultiplexingToAnalysis or MultiplexingToReturn NOT all type spilt. socket id : {}'.format(self.socket_id)) - return False - - # 檢查 multiplexingToSwap 與 swap_queues 的長度是否一致 而且 swap_queues 的長度不能為 0 - check = len(self.multiplexingToSwap) != len(swap_queues) or len(swap_queues) == 0 - if check: - logger.error('Multiplexing Queue not fit List , Please check the list. socket id : {}'.format(self.socket_id)) + if mavlinkMsg: + # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 + sysid = mavlinkMsg.get_srcSystem() + compid = mavlinkMsg.get_srcComponent() + + if sysid in self.mavlink_systems: # 只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息 + self.mavlink_systems[sysid].updateComponentPacketCount( + compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) #TODO 這邊怪怪的 好像要再檢查 + + # 分發訊息到 RingBuffer + msg_id = mavlinkMsg.get_msgId() + + # if self.send_to_bridge and (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): + if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): + stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg)) + + # if self.send_to_return and (msg_id in self.return_msg_types or -1 in self.return_msg_types): + if (msg_id in self.return_msg_types or -1 in self.return_msg_types): + return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg)) + + # 將接收到的訊息轉發給所有目標端口 + for target_port in self.target_sockets: + if target_port != self.socket_id and target_port in self.mavlinkObjects: + target_obj = self.mavlinkObjects[target_port] + if target_obj.state == MavlinkObjectState.RUNNING: + try: + target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) + except Exception as e: + logger.error(f"Error forwarding message to port {target_port}: {e}") + + with self.outgoing_msg_lock: + if self.outgoing_messages and (send_msg := self.outgoing_messages.pop(0)): + try: + self.mavlink_socket.write(send_msg) + except Exception as e: + logger.error(f"mavlink_object {self.socket_id} failed to send message: {e}") + + # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權 + await asyncio.sleep(0.001) + + except asyncio.CancelledError: + logger.info(f'mavlink_object.process_data for id {self.socket_id} was cancelled') + break + except Exception as e: + await asyncio.sleep(0.01) # 短暫暫停避免CPU過載 + + + logger.info(f'End of mavlink_object.process_data id: {self.socket_id}') + self.state = MavlinkObjectState.CLOSED + + def add_target_socket(self, target_socket_id): + """添加一個目標端口用於轉發""" + if target_socket_id not in self.target_sockets and target_socket_id != self.socket_id: + self.target_sockets.append(target_socket_id) + logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}") + return True + return False + + def remove_target_socket(self, target_socket_id): + """移除目標端口""" + if target_socket_id in self.target_sockets: + self.target_sockets.remove(target_socket_id) + logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}") + return True + return False + + def set_bridge_message_types(self, msg_types): + """設置需要分流到橋接器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.bridge_msg_types = msg_types + return True + logger.error(f"Invalid bridge message types: {msg_types}") + return False + + def set_return_message_types(self, msg_types): + """設置需要分流到回傳處理器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.return_msg_types = msg_types + return True + logger.error(f"Invalid return message types: {msg_types}") + return False + + def send_message(self, message_bytes): + """ + 從主線程向此 mavlink_object 的 socket 發送數據 + 將數據添加到簡單的列表中，由 asyncio 任務處理 + + Args: + message_bytes: 要發送的 mavlink 消息的字節數據 + + Returns: + bool: 是否成功添加消息到列表 + """ + if self.state != MavlinkObjectState.RUNNING: + logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running") return False - - # 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋 + try: - self.multiplexingToSwap[self.socket_id] = [] - except IndexError: - logger.error('Multiplexing List of self socket id should be void. socket id : {}'.format(self.socket_id)) - return False - - # 組合 multiplexing list - multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap - - # 檢查 multiplexing list 格式是否有錯誤 # 全部都要是 list 每個 list 裡面都要是 int - check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in multiL_tmp) - if not check: - logger.error('Multiplexing List Format Error, Please check the list. socket id : {}'.format(self.socket_id)) - return False # 若有錯誤則回傳 False - - # 更新 multiplexing list - self._multiplexingList = multiL_tmp + # 使用鎖保護共享資源訪問 + with self.outgoing_messages_lock: + self.outgoing_messages.append(message_bytes) + return True + except Exception as e: + logger.error(f"Error queueing message for mavlink_object {self.socket_id}: {e}") + return False + + # def enable_bridge(self, enable=True): + # """啟用或禁用橋接器分流""" + # self.send_to_bridge = enable + + # def enable_return(self, enable=True): + # """啟用或禁用回傳處理器分流""" + # self.send_to_return = enable + +class async_io_manager: + """ + 管理所有 mavlink_object 實例的 asyncio 任務 + 提供單一線程來處理所有 mavlink 通道的數據 + """ + _instance = None + _lock = threading.Lock() + + def __new__(cls, *args, **kwargs): + with cls._lock: + if cls._instance is None: + cls._instance = super(async_io_manager, cls).__new__(cls) + return cls._instance + + def __init__(self): + if not hasattr(self, 'initialized'): + self.initialized = True + self.loop = None + self.main_task = None + self.running = False + self.managed_objects = {} # socket_id: task + + def start(self): + """啟動 async_io_manager 和其管理的所有 mavlink_object""" + if self.running: + logger.warning("async_io_manager already running") + return + + self.running = True + self.thread = threading.Thread(target=self._run_event_loop) + self.thread.start() + logger.info("async_io_manager started") + + def stop(self): + """停止 async_io_manager 和其管理的所有 mavlink_object""" + if not self.running: + return + + self.running = False + + if self.loop: + # 取消所有任務 + for socket_id in list(self.managed_objects.keys()): + self.remove_mavlink_object(socket_id) + + # 停止事件循環 + if self.main_task and not self.main_task.done(): + asyncio.run_coroutine_threadsafe(self._shutdown(), self.loop) + + # 等待線程結束 + if hasattr(self, 'thread') and self.thread.is_alive(): + self.thread.join(timeout=5.0) + + logger.info("async_io_manager stopped") + + async def _shutdown(self): + """正確關閉事件循環的協程""" + tasks = [task for task in asyncio.all_tasks(self.loop) if task is not asyncio.current_task()] + + for task in tasks: + task.cancel() + + await asyncio.gather(*tasks, return_exceptions=True) + self.loop.stop() + + def _run_event_loop(self): + """在獨立線程中運行事件循環""" + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + try: + self.main_task = self.loop.create_task(self._main_task()) + self.loop.run_forever() + except Exception as e: + logger.error(f"Error in async_io_manager event loop: {e}") + finally: + self.loop.close() + self.loop = None + self.running = False + logger.info("async_io_manager event loop ended") + + async def _main_task(self): + """主任務協程，用於監視和管理其他任務""" + logger.info("async_io_manager main task started") + + try: + while self.running: + # 這邊就是一個無窮迴圈 確保 async_io_manager 物件續存 + # 每秒鐘檢查並移除已完成或已取消的任務 + await asyncio.sleep(1.0) + for socket_id in list(self.managed_objects.keys()): + task = self.managed_objects[socket_id] + + if task.done(): + try: + exc = task.exception() + if exc: + logger.error(f"Task for mavlink_object {socket_id} raised exception: {exc}") + except (asyncio.CancelledError, asyncio.InvalidStateError): + pass + + if socket_id in mavlink_object.mavlinkObjects: + print(f"this is manager, i make socket_id {socket_id} closed, he is now in {mavlink_object.mavlinkObjects[socket_id].state} state.") # debug + mavlink_object.mavlinkObjects[socket_id].state = MavlinkObjectState.CLOSED + + del self.managed_objects[socket_id] + + + + except asyncio.CancelledError: + logger.info("async_io_manager main task was cancelled") + except Exception as e: + logger.error(f"Error in async_io_manager main task: {e}") + + logger.info("async_io_manager main task ended") + + def add_mavlink_object(self, mavlink_obj): + """添加一個 mavlink_object 實例到 async_io_manager 並啟動其處理任務""" + if not self.running: + logger.error("Cannot add mavlink_object: async_io_manager is not running") + return False + + if not isinstance(mavlink_obj, mavlink_object): + logger.error(f"Invalid mavlink_object: {mavlink_obj}") + return False + + socket_id = mavlink_obj.socket_id + + if socket_id in self.managed_objects: + logger.warning(f"mavlink_object {socket_id} already managed") + return False + + # 創建並啟動任務 + if self.loop: + # task = asyncio.run_coroutine_threadsafe(mavlink_obj.process_data(), self.loop).result() + future = asyncio.run_coroutine_threadsafe(mavlink_obj.process_data(), self.loop) + + # print(f"Task created for mavlink_object {socket_id}: {future1}") # debug + # task = future1.result() + self.managed_objects[socket_id] = future + mavlink_obj.task = future + mavlink_obj.state = MavlinkObjectState.RUNNING + logger.info(f"Added mavlink_object {socket_id} to async_io_manager") + return True + + return False + + def remove_mavlink_object(self, socket_id): + """從 async_io_manager 中移除一個 mavlink_object 實例並取消其處理任務""" + if socket_id not in self.managed_objects: + logger.warning(f"mavlink_object {socket_id} not managed by async_io_manager") + return False + + # 取消任務 + task = self.managed_objects[socket_id] + if not task.done(): + task.cancel() + + # 從管理列表中移除 + del self.managed_objects[socket_id] + + # 更新 mavlink_object 狀態 + if socket_id in mavlink_object.mavlinkObjects: + mavlink_object.mavlinkObjects[socket_id].state = MavlinkObjectState.CLOSED + + logger.info(f"Removed mavlink_object {socket_id} from async_io_manager") return True + + def get_managed_objects(self): + """獲取所有被管理的對象列表""" + return list(self.managed_objects.keys()) # ====================== 分割線 ===================== -# 整合到 ros2 之後的程式進入點 -def main_node(): - pass - - if __name__ == '__main__': pass diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index 028e3ee..727e47f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,14 +9,16 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' +# ROS2 的 import import std_msgs.msg import sensor_msgs.msg import geometry_msgs.msg import mavros_msgs.msg -from theLogger import setup_logger import math +# 自定義的 import +from .theLogger import setup_logger logger = setup_logger("mavlinkPublish.py") diff --git a/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/tests/devRun.py b/src/fc_network_adapter/tests/devRun.py new file mode 100644 index 0000000..0707b51 --- /dev/null +++ b/src/fc_network_adapter/tests/devRun.py @@ -0,0 +1,448 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +# 基礎功能的 import +import queue +import time + +# ROS2 的 import +import rclpy + +# mavlink 的 import +from pymavlink import mavutil + +# 自定義的 import +from fc_network_adapter import mavlinkObject as mo +from fc_network_adapter import mavlinkDevice as md + +# ====================== 分割線 ===================== + +test_item = 21 +running_time = 30 + + +print('test_item : ', test_item) + +''' +測試項 個位數 表示分離的功能 + +測試項 1X 表示 mavlink_object 的功能 測試連線的能力 +''' + +if test_item == 10: + # 需要開啟一個 ardupilot 的模擬器 + # 測試 mavlink_object 放入 ring buffer 的應用 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + manager.add_mavlink_object(mavlink_object1) + + start_time = time.time() + while (time.time() - start_time) < running_time: + items_a = mo.stream_bridge_ring.get_all() + items_b = mo.return_packet_ring.get_all() + try: + print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}") + except IndexError: + print("stream_bridge_ring is empty") + + try: + print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}") + except IndexError: + print("return_packet_ring is empty") + time.sleep(1) + + manager.stop() + + print('<=== End of Program') + +elif test_item == 11: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + manager.add_mavlink_object(mavlink_object1) + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + + time.sleep(3) + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + if (time.time() - show_time) >= 2: + # print("mark B") + + show_time = time.time() + for sysid in analyzer.mavlink_systems: + for compid in analyzer.mavlink_systems[sysid].components: + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) + analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + print("===================") + + manager.stop() + analyzer.stop() + + print('<=== End of Program') + +elif test_item == 12: + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試 mavlink object 作為交換器功能的代碼 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) + mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) + + connection_string="udp:127.0.0.1:14561" + mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) + mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) + + # 初始化輸出通道 + connection_string="udpout:127.0.0.1:14550" + mavlink_socket_out = mavutil.mavlink_connection(connection_string) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + + manager.add_mavlink_object(mavlink_object_out) + manager.add_mavlink_object(mavlink_object_in1) + manager.add_mavlink_object(mavlink_object_in2) + + mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id) + + mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id) + + start_time = time.time() + while (time.time() - start_time) < running_time: + + time.sleep(1) + + manager.stop() + + print('<=== End of Program') + + +# elif test_item == 20: +# # 這邊測試 node 生成 topic 的功能 +# # 利用 空的通道 發出假的 heartbeat 封包 +# print('===> Start of Program .Test ', test_item) +# rclpy.init() # 注意要初始化 rclpy 才能使用 node + +# from pymavlink.dialects.v20 import common as mavlink2 + +# sysid = 1 +# compid = 1 + +# # 啟動 mavlink_bridge +# analyzer = mo.mavlink_bridge() + +# mav = mavlink2.MAVLink(None) +# mav.srcSystem = sysid +# mav.srcComponent = compid + +# # 這是一個 heartbeat 封包 +# fake_heartbeat = mavlink2.MAVLink_heartbeat_message( +# type=mavlink2.MAV_TYPE_QUADROTOR, +# autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, +# base_mode=3, +# custom_mode=0, +# system_status=0, +# mavlink_version=3 +# ) +# mavlink_bytes = fake_heartbeat.pack(mav) +# fake_heartbeat_msg = mav.parse_char(mavlink_bytes) + +# print(analyzer.mavlink_systems) + +# mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data +# time.sleep(0.1) +# print(analyzer.mavlink_systems) + +# # ROS2 初始化 +# analyzer._init_node() + +# # 創建 ROS2 topic +# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid +# print("topic created") +# time.sleep(5) + +# # 丟出 topic +# analyzer.emit_info() + +# # 結束程式 +# analyzer.running = False +# analyzer.destroy_node() + +# rclpy.shutdown() +# analyzer.stop() +# analyzer.thread.join() +# print('<=== End of Program') + +elif test_item == 21: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + + print('===> Start of Program .Test ', test_item) + # 初始化 rclpy 才能使用 node + rclpy.init() + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + # 關於 Node 的初始化 + show_time = time.time() + analyzer._init_node() # 初始化 node + print('初始化 node 完成 耗時 : ',time.time() - show_time) + + time.sleep(0.5) # 系統 Setup 完成 + + + # 創建通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object3 = mo.mavlink_object(mavlink_socket) + manager.add_mavlink_object(mavlink_object3) + + print('=== waiting for mavlink data ...') + time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + compid = 1 + sysid = 1 + start_time = time.time() + analyzer.create_flightMode(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") + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + try: + # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode']) + analyzer.emit_info() # 這邊是測試 node 的運行 + time.sleep(1) + except KeyboardInterrupt: + break + + + # 程式結束 + analyzer.destroy_node() + rclpy.shutdown() + + # 結束程式 退出所有 thread + manager.stop() + analyzer.stop() + analyzer.thread.join() + + mavlink_socket.close() + print('<=== End of Program') + +# elif test_item == 22: +# # 需要開啟一個 ardupilot 的模擬器 與 GCS +# # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 +# 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_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" +# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) +# mavlink_object_out = mo.mavlink_object(mavlink_socket_out) +# mavlink_object_out.multiplexingToAnalysis = [] + + + +# # 讓兩個通道互相傳輸 +# mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all +# mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + +# # 啟動通道 +# mavlink_object_in.run() +# mavlink_object_out.run() + +# print('waiting for mavlink data ...') +# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + +# print('目前所有的系統 : ') +# for sysid in analyzer.mavlink_systems: +# print(analyzer.mavlink_systems[sysid]) + +# compid = 1 +# sysid = 1 +# show_time = time.time() +# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) +# print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") + +# print("start emit info") + +# start_time = time.time() +# show_time = time.time() +# show_time2 = time.time() + +# while time.time() - start_time < running_time: +# if (time.time() - show_time2) >= 1: +# analyzer.emit_info() # 這邊是測試 node 的運行 +# # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] +# fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] +# sss = mavutil.mode_string_v10(fmsg) +# # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) +# print("目前的飛行模式 : {}".format(sss)) +# show_time2 = time.time() + +# # if (time.time() - show_time) >= 2: +# # show_time = time.time() +# # for sysid in analyzer.mavlink_systems: +# # for compid in analyzer.mavlink_systems[sysid].components: +# # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) +# # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) +# # print("===================") + + + +# analyzer.destroy_node() +# rclpy.shutdown() + + +# # 結束程式 退出所有 thread +# mavlink_object_in.stop() +# mavlink_object_in.thread.join() +# mavlink_socket_in.close() +# mavlink_object_out.stop() +# mavlink_object_out.thread.join() +# mavlink_socket_out.close() +# analyzer.stop() +# analyzer.thread.join() + + +# 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/tests/test_asyncioManager.py b/src/fc_network_adapter/tests/test_asyncioManager.py deleted file mode 100644 index 27a790a..0000000 --- a/src/fc_network_adapter/tests/test_asyncioManager.py +++ /dev/null @@ -1,246 +0,0 @@ -import os -import sys -sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) - -import time -import threading -import socket -import random -from pymavlink import mavutil -from pymavlink.dialects.v20 import common as mavlink2 - -from fc_network_adapter.asyncioManager import AsyncIOManager, Connection - -class MockMAVLinkSocket: - """模擬 pymavlink socket 行為的類，用於測試""" - - def __init__(self, name="mock", simulate_errors=False): - self.name = name - self.simulate_errors = simulate_errors - self.is_closed = False - self.received_msgs = [] - self.seq = 0 - - def recv_msg(self): - """模擬接收 MAVLink 消息""" - if self.is_closed: - return None - - # 隨機模擬網絡延遲 - time.sleep(random.uniform(0.01, 0.05)) - - # 偶爾模擬錯誤 - if self.simulate_errors and random.random() < 0.1: - raise Exception("Simulated network error") - - # 70% 的機率返回消息，30% 返回 None (模擬無數據可讀) - if random.random() > 0.3: - msg = self._generate_mavlink_message() - self.received_msgs.append(msg) - return msg - - return None - - def _generate_mavlink_message(self): - """生成模擬 MAVLink 消息""" - # 創建 MAVLink 實例 - mav = mavlink2.MAVLink(self) - - # 設置來源系統和組件 - mav.srcSystem = random.randint(1, 5) - mav.srcComponent = random.randint(1, 5) - - # 創建 HEARTBEAT 消息 - msg = mavlink2.MAVLink_heartbeat_message( - type=mavlink2.MAV_TYPE_QUADROTOR, - autopilot=mavlink2.MAV_AUTOPILOT_GENERIC, - base_mode=0, - custom_mode=0, - system_status=0, - mavlink_version=3 - ) - - # 打包消息 - msg.pack(mav) - self.seq += 1 - return msg - - def write(self, data): - """模擬發送數據""" - if self.is_closed: - raise Exception("Socket is closed") - - # 模擬寫入操作 - time.sleep(random.uniform(0.001, 0.01)) - - # 偶爾模擬錯誤 - if self.simulate_errors and random.random() < 0.1: - raise Exception("Simulated write error") - - return len(data) - - def close(self): - """關閉連接""" - self.is_closed = True - -def create_mock_mavlink_sockets(count=3): - """創建多個模擬 MAVLink socket""" - sockets = [] - for i in range(count): - # 每三個 socket 中有一個模擬錯誤 - simulate_errors = (i % 3 == 0) - socket = MockMAVLinkSocket(f"mock_{i}", simulate_errors) - sockets.append(socket) - return sockets - -def monitor_thread(io_manager): - """監視 AsyncIOManager 狀態的執行緒""" - print("Starting monitoring thread...") - - try: - while True: - # 獲取總體統計資訊 - stats = io_manager.get_stats() - print(f"\n=== AsyncIOManager Stats ===") - print(f"Total connections: {stats.get('connection_count', 0)}") - print(f"Total messages received: {stats.get('rx_count', 0)}") - print(f"Total bytes received: {stats.get('rx_bytes', 0)}") - print(f"Total errors: {stats.get('errors', 0)}") - - # 獲取每個連接的緩衝區狀態 - for socket_id in io_manager.connections: - buffer = io_manager.get_buffer(socket_id) - conn_stats = io_manager.get_stats(socket_id) - - if buffer and not io_manager.connections[socket_id].is_closed: - print(f"Connection {socket_id}: Buffer size={buffer.size()}, " + - f"Messages={conn_stats.get('rx_count', 0)}, " + - f"Last activity={time.time() - io_manager.connections[socket_id].last_activity:.1f}s ago") - - # 每秒更新一次 - time.sleep(1.0) - except KeyboardInterrupt: - print("Monitoring thread stopped.") - -def consumer_thread(io_manager, socket_id): - """從指定連接的緩衝區消費消息""" - print(f"Starting consumer for socket {socket_id}...") - - try: - while True: - # 獲取緩衝區 - buffer = io_manager.get_buffer(socket_id) - if not buffer: - print(f"Consumer {socket_id}: Buffer not found or connection closed") - break - - # 獲取所有消息 - messages = buffer.get_all() - if messages: - print(f"Consumer {socket_id}: Received {len(messages)} messages") - for msg in messages: - # 處理每條消息 (這裡只是示例，實際應用中可能有更複雜的處理) - print(f" - Message from SysID={msg.get_srcSystem()}, CompID={msg.get_srcComponent()}, Type={msg.get_type()}") - - # 短暫休眠，避免 CPU 空轉 - time.sleep(0.2) - except Exception as e: - print(f"Consumer {socket_id} stopped: {e}") - -def send_command_thread(io_manager, socket_ids): - """定期向所有連接發送命令""" - print("Starting command sender thread...") - - # 創建一個 MAVLink 實例用於生成消息 - mav = mavlink2.MAVLink(None) - mav.srcSystem = 255 # GCS system ID - mav.srcComponent = 0 - - try: - count = 0 - while True: - # 等待 2 秒 - time.sleep(2.0) - count += 1 - - # 創建一個命令消息 - msg = mavlink2.MAVLink_command_long_message( - target_system=1, - target_component=1, - command=mavlink2.MAV_CMD_REQUEST_PROTOCOL_VERSION, - confirmation=0, - param1=0, param2=0, param3=0, - param4=0, param5=0, param6=0, param7=0 - ) - - # 向所有連接發送消息 - for socket_id in socket_ids: - if socket_id in io_manager.connections and not io_manager.connections[socket_id].is_closed: - try: - # 打包並發送消息 - result = io_manager.send_message(socket_id, msg) - print(f"Sent command to socket {socket_id}: {'Success' if result else 'Failed'}") - except Exception as e: - print(f"Error sending to socket {socket_id}: {e}") - - # 每 10 次迭代關閉一個連接 (用於演示關閉功能) - if count % 10 == 0 and socket_ids: - socket_id = socket_ids.pop(0) - print(f"Closing connection {socket_id}...") - io_manager.remove_connection(socket_id) - except Exception as e: - print(f"Command sender thread stopped: {e}") - -def main(): - print("AsyncIOManager Example") - print("======================") - - # 創建 AsyncIOManager 實例 - io_manager = AsyncIOManager() - - # 啟動 I/O 管理器 - io_manager.start() - print("AsyncIOManager started") - - # 創建模擬 MAVLink socket - mock_sockets = create_mock_mavlink_sockets(5) - socket_ids = [] - - # 註冊連接 - for i, socket in enumerate(mock_sockets): - socket_id = io_manager.register_connection(socket) - socket_ids.append(socket_id) - print(f"Registered socket {i} with ID {socket_id}") - - # 啟動監視執行緒 - monitor = threading.Thread(target=monitor_thread, args=(io_manager,)) - monitor.daemon = True - monitor.start() - - # 為每個連接啟動消費者執行緒 - consumers = [] - for socket_id in socket_ids: - consumer = threading.Thread(target=consumer_thread, args=(io_manager, socket_id)) - consumer.daemon = True - consumer.start() - consumers.append(consumer) - - # 啟動命令發送執行緒 - sender = threading.Thread(target=send_command_thread, args=(io_manager, socket_ids.copy())) - sender.daemon = True - sender.start() - - print("\nPress Ctrl+C to exit...") - try: - # 運行一段時間然後退出 - time.sleep(30) - except KeyboardInterrupt: - pass - finally: - # 清理資源 - print("\nShutting down...") - io_manager.stop() - print("AsyncIOManager stopped") - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py new file mode 100644 index 0000000..23907ee --- /dev/null +++ b/src/fc_network_adapter/tests/test_mavlinkObject.py @@ -0,0 +1,468 @@ +#!/usr/bin/env python +""" +測試腳本，用於測試 mavlinkObject.py 中的 mavlink_object 和 async_io_manager 類別 +""" + +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + + +import unittest +import time +import threading +import socket +import asyncio +from unittest.mock import MagicMock, patch + +# 導入要測試的模組 +from fc_network_adapter.mavlinkObject import ( + mavlink_object, + async_io_manager, + MavlinkObjectState, + stream_bridge_ring, + return_packet_ring +) + + +class MockMavlinkSocket: + """模擬 Mavlink Socket 的類別，用於測試""" + + def __init__(self, test_data=None): + self.closed = False + self.test_data = test_data or [] + self.data_index = 0 + self.written_data = [] + + def recv_msg(self): + if not self.test_data or self.data_index >= len(self.test_data): + return None + + data = self.test_data[self.data_index] + self.data_index += 1 + return data + + def write(self, data): + self.written_data.append(data) + + def close(self): + self.closed = True + + +class MockMavlinkMessage: + """模擬 Mavlink 訊息的類別，用於測試""" + + def __init__(self, msg_id, sysid, compid, seq=0): + self.msg_id = msg_id + self.sysid = sysid + self.compid = compid + self.seq = seq + self.msg_buf = bytes([msg_id, sysid, compid, seq]) + + def get_msgId(self): + return self.msg_id + + def get_srcSystem(self): + return self.sysid + + def get_srcComponent(self): + return self.compid + + def get_seq(self): + return self.seq + + def get_msgbuf(self): + return self.msg_buf + + def get_type(self): + return f"MSG_ID_{self.msg_id}" + + +class TestMavlinkObject(unittest.TestCase): + """測試 mavlink_object 類別""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建測試訊息 + self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1) + self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1) + self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1) + + # 創建模擬的 socket # 假的 Mavlink Socket + self.mock_socket = MockMavlinkSocket([ + self.heartbeat_msg, + self.attitude_msg, + self.position_msg + ]) + + # 創建測試對象 + self.mavlink_obj = mavlink_object(self.mock_socket) + + def test_initialization(self): + """測試 mavlink_object 初始化是否正確""" + # print("Testing mavlink_object initialization") + self.assertEqual(self.mavlink_obj.socket_id, 0) + self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT) + self.assertEqual(len(self.mavlink_obj.target_sockets), 0) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0]) + + def test_add_remove_target_socket(self): + """測試添加和移除目標端口功能""" + # 添加目標端口 + self.assertTrue(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + self.assertEqual(self.mavlink_obj.target_sockets[0], 1) + self.assertTrue(self.mavlink_obj.add_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + self.assertIn(2, self.mavlink_obj.target_sockets) + + # 嘗試添加已存在的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 嘗試添加自己的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(0)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 移除端口 + self.assertTrue(self.mavlink_obj.remove_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + + # 嘗試移除不存在的端口 + self.assertFalse(self.mavlink_obj.remove_target_socket(2)) + + def test_set_message_types(self): + """測試設置訊息類型功能""" + # 設置橋接器訊息類型 + self.assertTrue(self.mavlink_obj.set_bridge_message_types([0, 30])) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0, 30]) + + # 設置回傳處理器訊息類型 + self.assertTrue(self.mavlink_obj.set_return_message_types([32])) + self.assertEqual(self.mavlink_obj.return_msg_types, [32]) + + # 測試無效的訊息類型 + self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid")) + self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"])) + + def test_send_message(self): + """測試 send_message 功能""" + # 創建一個新的 mavlink_object 實例 + mock_socket = MockMavlinkSocket() + mavlink_obj = mavlink_object(mock_socket) + + # 準備測試數據 + test_message1 = b"test_message_1" + test_message2 = b"test_message_2" + + # 測試初始狀態 + self.assertEqual(len(mock_socket.written_data), 0) + + # 測試非運行狀態下發送消息 + self.assertFalse(mavlink_obj.send_message(test_message1)) + self.assertEqual(len(mock_socket.written_data), 0) + + # 啟動 manager + manager = async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象到 manager + manager.add_mavlink_object(mavlink_obj) + time.sleep(0.1) # 等待對象啟動 + + # 確認對象狀態 + self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) + + # 測試發送消息 + self.assertTrue(mavlink_obj.send_message(test_message1)) + time.sleep(0.2) # 等待消息處理 + + # 確認消息已發送 + self.assertEqual(len(mock_socket.written_data), 1) + self.assertEqual(mock_socket.written_data[0], test_message1) + + # 測試連續發送多條消息 + self.assertTrue(mavlink_obj.send_message(test_message2)) + time.sleep(0.2) # 等待消息處理 + + # 確認兩條消息都已發送 + self.assertEqual(len(mock_socket.written_data), 2) + self.assertEqual(mock_socket.written_data[1], test_message2) + + # 模擬發送出錯的情況 + class ErrorWriteSocket(MockMavlinkSocket): + def write(self, data): + raise Exception("Write error") + + error_socket = ErrorWriteSocket() + error_obj = mavlink_object(error_socket) + manager.add_mavlink_object(error_obj) + time.sleep(0.1) # 等待對象啟動 + + # 發送消息到錯誤的 socket + self.assertTrue(error_obj.send_message(test_message1)) + time.sleep(0.2) # 等待消息處理 + + # 即使寫入失敗，send_message 應該也返回 True + # 因為消息已成功加入到佇列中，只是後續的實際發送失敗 + + # 停止 manager + manager.stop() + time.sleep(0.5) # 等待 manager 停止 + + # 測試對象已關閉後發送消息 + self.assertFalse(mavlink_obj.send_message(test_message1)) + self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 + +class TestAsyncIOManager(unittest.TestCase): + """測試 async_io_manager 類別""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + # 模擬 mavlink 對象 + self.mock_socket1 = MockMavlinkSocket([ + MockMavlinkMessage(msg_id=0, sysid=1, compid=1), + MockMavlinkMessage(msg_id=30, sysid=1, compid=1) + ]) + self.mock_socket2 = MockMavlinkSocket([ + MockMavlinkMessage(msg_id=0, sysid=2, compid=1) + ]) + + self.mavlink_obj1 = mavlink_object(self.mock_socket1) + self.mavlink_obj2 = mavlink_object(self.mock_socket2) + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.stop() + + def test_singleton_pattern(self): + """測試 async_io_manager 的單例模式 就是不會產生兩個 magager""" + manager1 = async_io_manager() + manager2 = async_io_manager() + self.assertIs(manager1, manager2) + + def test_start_stop(self): + """測試 async_io_manager 的啟動和停止功能""" + # 啟動管理器 + self.manager.start() + self.assertTrue(self.manager.running) + self.assertIsNotNone(self.manager.thread) + + # 再次啟動應該沒有效果 + old_thread = self.manager.thread + self.manager.start() + self.assertIs(self.manager.thread, old_thread) + + # 停止管理器 + self.manager.stop() + self.assertFalse(self.manager.running) + + # 最多等待 5 秒讓線程結束 + start_time = time.time() + while self.manager.thread.is_alive() and time.time() - start_time < 5: + time.sleep(0.1) + self.assertFalse(self.manager.thread.is_alive()) + + def test_add_remove_objects(self): + """測試添加和移除 mavlink_object""" + # 啟動管理器 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj1)) + self.assertEqual(len(self.manager.managed_objects), 1) + self.assertEqual(self.mavlink_obj1.state, MavlinkObjectState.RUNNING) + + # 添加另一個對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj2)) + self.assertEqual(len(self.manager.managed_objects), 2) + + # 檢查受管理對象列表 + managed_objects = self.manager.get_managed_objects() + self.assertEqual(len(managed_objects), 2) + self.assertIn(0, managed_objects) + self.assertIn(1, managed_objects) + + # 移除對象 + self.assertTrue(self.manager.remove_mavlink_object(0)) + self.assertEqual(len(self.manager.managed_objects), 1) + + # 嘗試移除不存在的對象 + self.assertFalse(self.manager.remove_mavlink_object(999)) + + # 停止管理器 + self.manager.stop() + + def test_data_processing(self): + """測試數據處理""" + # 創建用於轉發的 mavlink_objects + mock_socket3 = MockMavlinkSocket() + mavlink_obj3 = mavlink_object(mock_socket3) + + # 設置轉發 + self.mavlink_obj1.add_target_socket(2) # socket1 轉發到 socket3 + + # 設置訊息類型 + self.mavlink_obj1.set_bridge_message_types([0, 30]) # HEARTBEAT, ATTITUDE + # self.mavlink_obj1.enable_return(True) + self.mavlink_obj1.set_return_message_types([30]) # ATTITUDE + + # 啟動管理器並添加對象 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + self.manager.add_mavlink_object(self.mavlink_obj1) + self.manager.add_mavlink_object(mavlink_obj3) + + # 等待處理完成 + time.sleep(1.0) + + # print("testing Mark A") + + # 檢查 Ring buffer 是否有正確的數據 + self.assertEqual(stream_bridge_ring.size(), 2) # HEARTBEAT + ATTITUDE + self.assertEqual(return_packet_ring.size(), 1) # ATTITUDE + + a = stream_bridge_ring.get() + print(f"stream_bridge_ring: {a}") + + # 檢查是否正確轉發 + self.assertEqual(len(mock_socket3.written_data), 2) # HEARTBEAT + ATTITUDE + + # print("testing Mark B") + + # 停止管理器 + self.manager.stop() + + def test_error_handling(self): + """測試錯誤處理情況""" + print("=== mark A ===") + # 創建一個會引發異常的 socket + class ErrorSocket(MockMavlinkSocket): + def recv_msg(self): + raise Exception("Test exception") + + error_socket = ErrorSocket() + mavlink_obj_err = mavlink_object(error_socket) + + # 啟動管理器並添加對象 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + self.manager.add_mavlink_object(mavlink_obj_err) + + print("=== mark B ===") + + # 等待錯誤處理 + time.sleep(1.0) + + # 對象應該進入錯誤狀態，但不會崩潰 + # self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR) + + print("=== mark C ===") + + # 管理器應該仍在運行 + self.assertTrue(self.manager.running) + + + # 故意等一段時間 確認 socket 有被 manager 處理掉 + time.sleep(3) + + # 停止管理器 + self.manager.stop() + + +class TestIntegration(unittest.TestCase): + """整合測試，測試多個 mavlink_object 之間的互動""" + + def test_bidirectional_forwarding(self): + """測試雙向轉發""" + # 清空全局變數和 ring buffer + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建三個 mavlink 對象，模擬三個通道 + socket1 = MockMavlinkSocket([ + MockMavlinkMessage(msg_id=0, sysid=1, compid=1), + MockMavlinkMessage(msg_id=30, sysid=1, compid=1) + ]) + socket2 = MockMavlinkSocket([ + MockMavlinkMessage(msg_id=0, sysid=2, compid=1), + MockMavlinkMessage(msg_id=32, sysid=2, compid=1) + ]) + socket3 = MockMavlinkSocket([ + MockMavlinkMessage(msg_id=0, sysid=3, compid=1), + MockMavlinkMessage(msg_id=33, sysid=3, compid=1) + ]) + + obj1 = mavlink_object(socket1) + obj2 = mavlink_object(socket2) + obj3 = mavlink_object(socket3) + + # 設置雙向轉發 + # obj1 <-> obj2 <-> obj3 + obj1.add_target_socket(1) # obj1 -> obj2 + obj2.add_target_socket(0) # obj2 -> obj1 + obj2.add_target_socket(2) # obj2 -> obj3 + obj3.add_target_socket(1) # obj3 -> obj2 + + # 啟動 async_io_manager + manager = async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加所有 mavlink_object + manager.add_mavlink_object(obj1) + manager.add_mavlink_object(obj2) + manager.add_mavlink_object(obj3) + + # 等待處理所有訊息 + time.sleep(1.5) + + # 檢查轉發結果 + # socket1 應該收到 socket2 和 socket3 的訊息 + self.assertEqual(len(socket1.written_data), 4) # 2 from obj2, 2 from obj3 via obj2 + + # socket2 應該收到 socket1 和 socket3 的訊息 + self.assertEqual(len(socket2.written_data), 4) # 2 from obj1, 2 from obj3 + + # socket3 應該收到 socket1 和 socket2 的訊息 + self.assertEqual(len(socket3.written_data), 4) # 2 from obj1 via obj2, 2 from obj2 + + # 檢查 ring buffer 的數據 + # 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] + # 應該有 3 個 HEARTBEAT 訊息 + self.assertEqual(stream_bridge_ring.size(), 3) # 3 HEARTBEAT + + # 停止管理器 + manager.stop() + + + + +if __name__ == "__main__": + unittest.main(defaultTest="TestMavlinkObject.test_send_message") From 2c4fc4583ee7c99a86e46eebda39366dbadd0323 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 9 Jun 2025 18:34:13 +0800 Subject: [PATCH 062/146] =?UTF-8?q?Hotfix=20:=20mavlink=5Fobject=20?= =?UTF-8?q?=E5=85=A7=20process=5Fdata=20=E7=9A=84=20try=20=E8=BF=B4?= =?UTF-8?q?=E5=9C=88=E5=88=AA=E6=B8=9B=20(try=20=E5=9A=B4=E9=87=8D?= =?UTF-8?q?=E5=BD=B1=E9=9F=BF=E6=95=88=E8=83=BD)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mavlinkObject.py | 33 +++++++++++-------- src/fc_network_adapter/tests/devRun.py | 6 ++-- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index d0231ed..dadb58f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -331,18 +331,18 @@ class mavlink_object: try: # 處理接收到的封包 mavlinkMsg = self.mavlink_socket.recv_msg() - except Exception as e: - logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") - self.dirtyDataCount += 1 + # except Exception as e: + # logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") + # self.dirtyDataCount += 1 - if self.dirtyDataCount >= self.dirtyDataMax: - logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") - self.state = MavlinkObjectState.ERROR - # 不直接退出，嘗試容忍髒數據 - await asyncio.sleep(0.01) # 短暫暫停 - continue + # if self.dirtyDataCount >= self.dirtyDataMax: + # logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") + # self.state = MavlinkObjectState.ERROR + # # 不直接退出，嘗試容忍髒數據 + # await asyncio.sleep(0.01) # 短暫暫停 + # continue - try: + # try: if mavlinkMsg: # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 sysid = mavlinkMsg.get_srcSystem() @@ -374,7 +374,7 @@ class mavlink_object: logger.error(f"Error forwarding message to port {target_port}: {e}") with self.outgoing_msg_lock: - if self.outgoing_messages and (send_msg := self.outgoing_messages.pop(0)): + if self.outgoing_msgs and (send_msg := self.outgoing_msgs.pop(0)): try: self.mavlink_socket.write(send_msg) except Exception as e: @@ -387,6 +387,13 @@ class mavlink_object: logger.info(f'mavlink_object.process_data for id {self.socket_id} was cancelled') break except Exception as e: + logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") + self.dirtyDataCount += 1 + + if self.dirtyDataCount >= self.dirtyDataMax: + logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") + self.state = MavlinkObjectState.ERROR + # 不直接退出，嘗試容忍髒數據 await asyncio.sleep(0.01) # 短暫暫停避免CPU過載 @@ -442,8 +449,8 @@ class mavlink_object: try: # 使用鎖保護共享資源訪問 - with self.outgoing_messages_lock: - self.outgoing_messages.append(message_bytes) + with self.outgoing_msgs_lock: + self.outgoing_msgs.append(message_bytes) return True except Exception as e: logger.error(f"Error queueing message for mavlink_object {self.socket_id}: {e}") diff --git a/src/fc_network_adapter/tests/devRun.py b/src/fc_network_adapter/tests/devRun.py index 0707b51..48f9e2b 100644 --- a/src/fc_network_adapter/tests/devRun.py +++ b/src/fc_network_adapter/tests/devRun.py @@ -18,8 +18,8 @@ from fc_network_adapter import mavlinkDevice as md # ====================== 分割線 ===================== -test_item = 21 -running_time = 30 +test_item = 12 +running_time = 10000 print('test_item : ', test_item) @@ -147,6 +147,8 @@ elif test_item == 12: manager.add_mavlink_object(mavlink_object_in1) manager.add_mavlink_object(mavlink_object_in2) + time.sleep(1) # 等待通道啟動 + mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id) mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id) From 303dfaf04a1e7a00c048d7520d1e289019db3ae0 Mon Sep 17 00:00:00 2001 From: wenchun Date: Mon, 9 Jun 2025 18:46:16 +0800 Subject: [PATCH 063/146] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20devRun.py=20?= =?UTF-8?q?=E6=B8=AC=E8=A9=A6=E6=AA=94=E6=A1=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/fc_network_adapter/tests/devRun.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/fc_network_adapter/tests/devRun.py b/src/fc_network_adapter/tests/devRun.py index 0707b51..1544ebf 100644 --- a/src/fc_network_adapter/tests/devRun.py +++ b/src/fc_network_adapter/tests/devRun.py @@ -18,8 +18,8 @@ from fc_network_adapter import mavlinkDevice as md # ====================== 分割線 ===================== -test_item = 21 -running_time = 30 +test_item = 12 +running_time = 500 print('test_item : ', test_item) @@ -130,16 +130,16 @@ elif test_item == 12: time.sleep(0.5) # 等待事件循環啟動 # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" + connection_string="udp:127.0.0.1:14554" mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) - connection_string="udp:127.0.0.1:14561" + connection_string="udp:127.0.0.1:14551" mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) # 初始化輸出通道 - connection_string="udpout:127.0.0.1:14550" + connection_string="udpout:127.0.0.1:14553" mavlink_socket_out = mavutil.mavlink_connection(connection_string) mavlink_object_out = mo.mavlink_object(mavlink_socket_out) @@ -445,4 +445,4 @@ elif test_item == 21: # analyzer.thread.join() # mavlink_socket3.close() -# print('<=== End of Program') \ No newline at end of file +# print('<=== End of Program') From 68af81ac682be58b301db37df45828a15d42cf91 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Fri, 13 Jun 2025 18:45:16 +0800 Subject: [PATCH 064/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 580 +++++++++++++++++++++++++++------ 1 file changed, 489 insertions(+), 91 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 9d8651a..6636fa3 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -3,18 +3,21 @@ import rclpy from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, - QSizePolicy, QApplication) + QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, + QHeaderView, QPushButton, QCheckBox) from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal 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 from std_msgs.msg import Float64 -from mavros_msgs.msg import VfrHud, State +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +import asyncio +from functools import partial class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -26,6 +29,10 @@ class DroneMonitor(Node): self.drone_topics = {} self.lock = Lock() + self.arm_clients = {} + self.takeoff_clients = {} + self.selected_drones = set() + # 主题检测定时器 self.create_timer(1.0, self.scan_topics) @@ -43,11 +50,22 @@ class DroneMonitor(Node): for drone_id in found_drones: if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone_subs(drone_id) + self.setup_drone(drone_id) - def setup_drone_subs(self, drone_id): + def setup_drone(self, drone_id): base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + subs = { 'attitude': self.create_subscription( Imu, @@ -61,12 +79,6 @@ class DroneMonitor(Node): lambda msg, did=drone_id: self.battery_callback(did, msg), 10 ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), 'global': self.create_subscription( NavSatFix, f'{base_topic}/global_position/global', @@ -91,6 +103,24 @@ class DroneMonitor(Node): lambda msg, did=drone_id: self.local_vel_callback(did, msg), 10 ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_callback(did, msg), + 10 + ), 'vfr_hud': self.create_subscription( VfrHud, f'{base_topic}/vfr_hud', @@ -102,6 +132,46 @@ class DroneMonitor(Node): setattr(self, f'drone_{drone_id}_subs', subs) self.signals.update_signal.emit('new_drone', drone_id, None) + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + 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) @@ -116,7 +186,7 @@ class DroneMonitor(Node): return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) - # 回调函数 + # callbacks def attitude_callback(self, drone_id, msg): if hasattr(msg, 'orientation'): roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) @@ -176,13 +246,22 @@ class DroneMonitor(Node): 'climb': msg.climb }) + def loss_rate_callback(self, drone_id, msg): + self.signals.update_signal.emit('loss_rate', drone_id, { + 'loss_rate': msg.data + }) + + def ping_callback(self, drone_id, msg): + self.signals.update_signal.emit('ping', drone_id, { + 'ping': msg.data + }) + class ControlStationUI(QMainWindow): def __init__(self): super().__init__() self.setWindowTitle('GCS') self.resize(1400, 900) - self.drones = {} - + # 初始化ROS2 rclpy.init() self.monitor = DroneMonitor() @@ -193,7 +272,26 @@ class ControlStationUI(QMainWindow): self.executor.add_node(self.monitor) # 初始化UI + self.drones = {} + self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", + "Roll", "Pitch", "Yaw", "丟包", "延遲"] + self.info_type_map = { + "mode": 0, + "armed": 1, + "battery": 2, + "altitude": 3, + "local": 4, + "velocity": 5, + "groundspeed": 6, + "heading": 7, + "roll": 8, + "pitch": 9, + "yaw": 10, + "loss_rate": 11, + "ping": 12 + } self.drone_positions = {} + self.drone_headings = {} self.map_loaded = False self.init_ui() @@ -203,23 +301,95 @@ class ControlStationUI(QMainWindow): self.timer.start(50) def init_ui(self): + + # 只呼叫一次 main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左側 TabWidget + self.left_tab = QTabWidget() + + # — 分頁 1：Drone Panel + self.drone_panel_container = QWidget() + self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) + self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + self.drone_panel_layout.setSpacing(0) + self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) + + # Wrap drone panel in scroll area + scroll = QScrollArea() + scroll.setWidget(self.drone_panel_container) + scroll.setWidgetResizable(True) + self.left_tab.addTab(scroll, "無人機") + + # 底部控制按鈕區域 + bottom_control = QWidget() + bottom_layout = QHBoxLayout(bottom_control) + + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + select_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) - # 左侧信息面板 - 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) + arm_all_btn = QPushButton("批次解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + arm_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) - left_layout.addWidget(scroll_area) + takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + takeoff_all_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + bottom_layout.addWidget(select_all_btn) + bottom_layout.addWidget(arm_all_btn) + bottom_layout.addWidget(takeoff_all_btn) + bottom_layout.addStretch() + + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + # 一開始只有一欄 + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + # 只有第一欄自動收縮到內容寬度，其它欄不動 + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + # 右侧地图区域 self.map_view = QWebEngineView() @@ -305,7 +475,17 @@ class ControlStationUI(QMainWindow): self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) - main_splitter.addWidget(left_panel) + # Create left container and its layout + left_container = QWidget() + left_layout = QVBoxLayout(left_container) + left_layout.setContentsMargins(0, 0, 0, 0) + + # Add tab widget and bottom control to left container + left_layout.addWidget(self.left_tab) + left_layout.addWidget(bottom_control) + + # Add widgets to splitter + main_splitter.addWidget(left_container) main_splitter.addWidget(self.map_view) main_splitter.setSizes([400, 1000]) @@ -344,6 +524,14 @@ class ControlStationUI(QMainWindow): header_layout = QHBoxLayout(header) header_layout.setContentsMargins(0, 0, 0, 0) + # 在標題列加入勾選框 + checkbox = QCheckBox() + checkbox.setObjectName(f"{drone_id}_checkbox") + checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + checkbox.stateChanged.connect(lambda state: self.handle_drone_selection(drone_id, state)) + + header_layout.insertWidget(0, checkbox) # 插入到最前面 + # ID显示 id_label = QLabel(drone_id) id_label.setStyleSheet(""" @@ -365,14 +553,79 @@ class ControlStationUI(QMainWindow): # 数据字段（带标签） self.create_data_row(layout, "模式:", f"{drone_id}_mode", "--") - self.create_data_row(layout, "電壓:", f"{drone_id}_battery", "--") - self.create_data_row(layout, "GPS:", f"{drone_id}_gps", "--") + self.create_data_row(layout, "ARM:", f"{drone_id}_armed", "--") + self.create_data_row(layout, "電池:", f"{drone_id}_battery", "--") 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", "--") + self.create_data_row(layout, "地速:", f"{drone_id}_groundspeed", "--") + self.create_data_row(layout, "航向:", f"{drone_id}_heading", "--") + + # Add control buttons + control_widget = QWidget() + control_layout = QHBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + + arm_btn = QPushButton("解鎖") + arm_btn.setObjectName(f"{drone_id}_arm_btn") + arm_btn.clicked.connect(lambda: self.handle_arm(drone_id)) + arm_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + } + QPushButton:hover { + background-color: #555; + } + """) + + takeoff_btn = QPushButton("起飛") + takeoff_btn.setObjectName(f"{drone_id}_takeoff_btn") + takeoff_btn.clicked.connect(lambda: self.handle_takeoff(drone_id)) + takeoff_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + } + QPushButton:hover { + background-color: #555; + } + """) + + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addStretch() + + layout.addWidget(control_widget) return panel + def handle_arm(self, drone_id): + loop = asyncio.get_event_loop() + arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state + future = self.monitor.arm_drone(drone_id, arm_state) + loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) + + def handle_takeoff(self, drone_id): + loop = asyncio.get_event_loop() + future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) + + async def handle_service_response(self, future, action): + try: + result = await future + if result: + self.statusBar().showMessage(f"{action} 成功", 3000) + else: + self.statusBar().showMessage(f"{action} 失敗", 3000) + except Exception as e: + self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) + def create_data_row(self, layout, title, object_name, default): row = QWidget() hbox = QHBoxLayout(row) @@ -394,90 +647,235 @@ class ControlStationUI(QMainWindow): 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 == 'state': - mode = data['mode'] - self.update_field(panel, drone_id, 'mode', f"{mode}", - '#FF5555' if '返航' in data else '#55FF55') - + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', False) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + arm_text = "ARMED" if armed else "DISARMED" + arm_color = '#55FF55' if armed else '#FF5555' + + # 更新無人機面板欄位 + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + + # 更新總覽頁面欄位 + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + 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') - + voltage = 14.8 + + # 使用標準電壓判斷電池節數 + cells_by_max = voltage / 4.2 + cells_by_min = voltage / 3.7 + + # 檢查是否為合理的電池組 + is_valid_cells = abs(round(cells_by_max) - cells_by_min) < 0.5 + num_cells = round(cells_by_max) + + if not is_valid_cells or num_cells < 2: + # 電壓異常，顯示錯誤 + text = f"{voltage:.1f}V (電壓異常!)" + voltage_color = '#FF0000' # 紅色 + else: + # 正常電壓計算 + cell_voltage = voltage / num_cells + cell_max = 4.2 + cell_min = 3.7 + + # 計算電量百分比 + percentage = max(0, min(100, ((cell_voltage - cell_min) / (cell_max - cell_min)) * 100)) + + # 根據百分比設置顏色 + if percentage < 20: + voltage_color = '#FF6464' # 紅色 (低電量) + elif percentage < 40: + voltage_color = '#FFA500' # 橘色 (中低電量) + else: + voltage_color = '#FFFFFF' # 白色 (正常電量) + + # 顯示總電壓、電池節數、單節電壓和百分比 + text = f"{voltage:.1f}V ({cell_voltage:.2f}V / {percentage:.0f}%)" + + self.update_field(panel, drone_id, 'battery', text, voltage_color) + self.update_overview_table(drone_id, 'battery', text) + elif msg_type == 'gps': - lat, lon = data['lat'], data['lon'] + lat, lon = data.get('lat', 0), data.get('lon', 0) self.drone_positions[drone_id] = (lat, lon) - text = (f"緯度: {lat:.6f}°\n" - f"經度: {lon:.6f}°") + text = f"{lat:.6f}°, {lon:.6f}°" self.update_field(panel, drone_id, 'gps', text) + self.update_overview_table(drone_id, 'gps', text) elif msg_type == 'altitude': - text = (f"{data['altitude']:.1f} m") + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" self.update_field(panel, drone_id, 'altitude', text) - + self.update_overview_table(drone_id, 'altitude', text) + elif msg_type == 'local_pose': - text = (f"({data['x']:.1f}, {data['y']:.1f}, {data['z']:.1f})") + text = f"{data['x']:.1f}, {data['y']:.1f}" self.update_field(panel, drone_id, 'local', text) - + self.update_overview_table(drone_id, 'local', text) + elif msg_type == 'hud': - heading = data['heading'] - text = (f"地速: {data['groundspeed']:.1f} m/s\n" - 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) - ''' + heading = data.get('heading', 0) + self.drone_headings[drone_id] = heading + groundspeed = data.get('groundspeed', 0) + heading_text = f"{heading:.1f}°" + groundspeed_text = f"{groundspeed:.1f} m/s" + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'heading', heading_text) + self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + + elif msg_type == 'loss_rate': + loss_rate = data.get('loss_rate', 0) + text = f"{loss_rate:.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + ping = data.get('ping', 0) + text = f"{ping:.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', 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) - + text = f"{data['vx']:.1f}, {data['vy']:.1f}" + self.update_overview_table(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) - ''' + roll = data.get('roll', 0) + pitch = data.get('pitch', 0) + yaw = data.get('yaw', 0) + roll_text = f"{roll:.1f}°" + pitch_text = f"{pitch:.1f}°" + yaw_text = f"{yaw:.1f}°" + self.update_overview_table(drone_id, 'roll', roll_text) + self.update_overview_table(drone_id, 'pitch', pitch_text) + self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 如果地圖已經載入，則更新地圖顯示無人機位置 + if self.map_loaded and drone_id in self.drone_positions and drone_id in self.drone_headings: + lat, lon = self.drone_positions[drone_id] + heading = self.drone_headings[drone_id] + js = f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});" + self.map_view.page().runJavaScript(js) + + + def handle_drone_selection(self, drone_id, state): + """處理個別無人機勾選狀態變化""" + if state == Qt.CheckState.Checked.value: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + def handle_select_all(self): + """處理全選按鈕 - 再次點擊時取消全選""" + # 檢查是否所有無人機都已被選中 + all_selected = all( + self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox").isChecked() + for drone_id in self.drones + ) + + # 如果全部已選中，則取消全選；否則全選 + new_state = not all_selected + + # 更新所有勾選框狀態 + for drone_id in self.drones: + checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") + if checkbox: + checkbox.setChecked(new_state) + + def handle_arm_selected(self): + """處理批次解鎖""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) + + def handle_takeoff_selected(self): + """處理批次起飛""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.takeoff_drone(drone_id, 10.0) + loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) + def update_field(self, panel, drone_id, field, text, color=None): + """Update a specific field in the panel.""" 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_overview_table(self, drone_id=None, field=None, value=None): + # Ensure the widget is available + if not hasattr(self, 'overview_table') or self.overview_table is None: + return + + # Update a specific cell in the overview table + if drone_id and field and value: + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) # Get row from field mapping + + if row == -1: + return # Invalid field, exit + + item = self.overview_table.item(row, col) + if not item: + item = QTableWidgetItem() + self.overview_table.setItem(row, col, item) + + item.setText(value) # Update the cell's text + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) # Center the text + + # If no specific update, refresh entire table + if drone_id is None: + cols = 1 + len(self.drones) + self.overview_table.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.overview_table.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(row, col, val_item) + 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 - - # 清空原有 layout - for i in reversed(range(self.info_layout.count())): - widget = self.info_layout.itemAt(i).widget() - if widget: - self.info_layout.removeWidget(widget) - widget.setParent(None) - - # 根據 key 排序後重新加入 - for sorted_id in sorted(self.drones, key=lambda x: int(''.join(filter(str.isdigit, x)) or 0)): - self.info_layout.addWidget(self.drones[sorted_id]) + """只這一個 add_drone，不要再重複定義。""" + if drone_id in self.drones: + return + panel = self.create_drone_panel(drone_id) + self.drones[drone_id] = panel + + # 重新排序並顯示所有 panel + # 先清空 + while self.drone_panel_layout.count(): + w = self.drone_panel_layout.takeAt(0).widget() + if w: + w.setParent(None) + # 再依照數字排序加入 + for did in sorted(self.drones, key=lambda x: int(x[1:])): + self.drone_panel_layout.addWidget(self.drones[did]) + + # 更新總覽表 + self.update_overview_table() def spin_ros(self): try: @@ -495,4 +893,4 @@ if __name__ == '__main__': app = QApplication(sys.argv) station = ControlStationUI() station.show() - app.exec(app.exec()) \ No newline at end of file + app.exec() \ No newline at end of file From a55b76534fe22896c1772cdf7299ba8fd6ae0df6 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:43:36 +0800 Subject: [PATCH 065/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/devRun.py | 104 +++++++----------- 1 file changed, 41 insertions(+), 63 deletions(-) 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") From 6bbf2c450fb32ff48bc10abfa67a7ef02fd86c92 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:43:54 +0800 Subject: [PATCH 066/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkObject.py | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 0a219be..88046b8 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -20,6 +20,7 @@ from pymavlink import mavutil # ROS2 的 import from rclpy.node import Node +import std_msgs.msg # 自定義的 import from mavlinkDevice import mavlink_device, mavlink_systems @@ -117,6 +118,8 @@ class mavlink_bridge(Node, mavlink_publisher): this_device.components[msg.get_srcComponent()] = this_component this_component.mav_type = msg.type this_component.mav_autopilot = msg.autopilot + this_component.sysid = sysid # ★ 新增 + this_component.compid = compid else: continue @@ -124,6 +127,7 @@ class mavlink_bridge(Node, mavlink_publisher): if msg_id == 0: # HEARTBEAT 處理 this_component.emitParams['heartbeat'] = msg + self.ensure_all_publishers(sysid, this_component) elif msg_id == 30: # ATTITUDE 處理 this_component.emitParams['attitude'] = msg @@ -137,6 +141,12 @@ class mavlink_bridge(Node, mavlink_publisher): elif msg_id == 74: # VFR_HUD 處理 this_component.emitParams['vfr_hud'] = msg + elif msg_id == 111: #TIME_SYNC + round_trip_time = std_msgs.msg.Float64() + round_trip_time.data = (int(time.time() * 1e9) - msg.ts1) / 1e6 + if(round_trip_time.data < 1e6): + this_component.emitParams['ping'] = round_trip_time + elif msg_id == 147: # BATTERY_STATUS 處理 this_component.emitParams['battery'] = msg @@ -281,7 +291,6 @@ class mavlink_object(): def _run_thread(self): logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) - start_time = time.time() while self.running: timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 @@ -300,9 +309,20 @@ class mavlink_object(): # 統計收到的訊息 & 透過 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 類型 + device = self.mavlink_systems[sysid] mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) + + comp = device.components.get(compid) + if comp and comp.loss_rate_publisher: + if timestamp - comp.last_loss_publish_time >= 1.0: + loss_rate = comp.loss_rate # loss_rate 是在 updateComponentPacketCount 中計算並儲存的欄位 + msg = std_msgs.msg.Float64() + msg.data = float(loss_rate) + comp.loss_rate_publisher.publish(msg) + comp.last_loss_publish_time = timestamp # break # Debug function can put here you can see the input data from mavlink From 55b77f592046fa960af39df40ec80d8bdbd8058b Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 30 Jun 2025 19:44:04 +0800 Subject: [PATCH 067/146] Upload files to 'src/fc_network_adapter/fc_network_adapter' --- .../fc_network_adapter/mavlinkPublish.py | 104 ++++++++++-------- 1 file changed, 56 insertions(+), 48 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index b328f81..a0ce087 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -19,26 +19,43 @@ import mavros_msgs.msg from theLogger import setup_logger import math - logger = setup_logger("mavlinkPublish.py") class mavlink_publisher(): prefix_path = 'MavLinkBus' + TOPIC_CREATORS = ( + "create_state", + "create_attitude", + "create_local_position_pose", + "create_local_position_velocity", + "create_global_global", + "create_global_rel", + "create_vfr_hud", + "create_battery", + "create_ping" + ) + + def ensure_all_publishers(self, sysid: int, component): + """若還沒替這個 component 建立過 publisher，則一次補齊。""" + if getattr(component, "_pub_ready", False): + return # 已做過就跳過 + for fn in self.TOPIC_CREATORS: + getattr(self, fn)(sysid, component) + # 發布丟包率 + if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None: + target_topic = 'packet_loss_rate' + topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) + component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10) + component.loss_rate_topic_name = topic_name + + component._pub_ready = True # 打個旗標以免重複 + logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid) + def create_state(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 target_topic = 'state' - - # 這邊要檢查 flight_mode 是否存在 - try: - _ = component_obj.emitParams['heartbeat'] - 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(mavros_msgs.msg.State, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] @@ -46,6 +63,8 @@ class mavlink_publisher(): return True def packEmit_state(cls, emitParams, publisher): + if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 + return mav_msg = emitParams['heartbeat'] msg = mavros_msgs.msg.State() msg.mode = mavutil.mode_string_v10(mav_msg) @@ -63,18 +82,13 @@ class mavlink_publisher(): def create_attitude(self, sysid, component_obj): target_topic = 'attitude' - - try: - _ = component_obj.emitParams['attitude'] - 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.Imu, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] def packEmit_attitude(self, emitParams, publisher): + if 'attitude' not in emitParams: # ← 沒資料就直接返回 + return 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) @@ -90,16 +104,13 @@ class mavlink_publisher(): 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): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Point() msg.x = mav_msg.x @@ -110,16 +121,13 @@ class mavlink_publisher(): 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): + if 'local_position' not in emitParams: + return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Vector3() msg.x = mav_msg.vx @@ -130,16 +138,13 @@ class mavlink_publisher(): 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): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = sensor_msgs.msg.NavSatFix() msg.latitude = mav_msg.lat/1e7 @@ -150,16 +155,13 @@ class mavlink_publisher(): 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): + if 'global_position' not in emitParams: + return mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() msg.data = float(mav_msg.relative_alt/1e3) @@ -168,16 +170,13 @@ class mavlink_publisher(): 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): + if 'vfr_hud' not in emitParams: + return mav_msg = emitParams['vfr_hud'] msg = mavros_msgs.msg.VfrHud() msg.airspeed = mav_msg.airspeed @@ -191,21 +190,30 @@ class mavlink_publisher(): 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): + if 'battery' not in emitParams: + return mav_msg = emitParams['battery'] msg = sensor_msgs.msg.BatteryState() msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass + + def create_ping(self, sysid, component_obj): + target_topic = 'ping' + 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_ping] + def packEmit_ping(cls, emitParams, publisher): + if 'ping' not in emitParams: + return + mav_msg = emitParams['ping'] + publisher.publish(mav_msg) + pass # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file From 2783e4ba9314377c21d2727227861329a2c9664f Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Jul 2025 17:04:49 +0800 Subject: [PATCH 068/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/mavlink.py | 100 +++++++++++++++++++++-------- 1 file changed, 72 insertions(+), 28 deletions(-) diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index b047a0e..9bb6329 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -21,13 +21,15 @@ class MAVLinkWorker(QThread): param_signal = pyqtSignal(str, int) kbps_signal = pyqtSignal(str, float) - def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): + def __init__(self, connection_string="udp:127.0.0.1:14560 ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'): super().__init__() - self.namespaces = ['UAV1', 'UAV2', 'UAV3'] - self.connection = mavutil.mavlink_connection(usb, baud=57600) + self.sysids = [1, 5, 10] + self.namespaces = {} + self.namespaces = [f"UAV{sysid}" for sysid in self.sysids] + + self.connection = mavutil.mavlink_connection(connection_string, baud=115200) self.connection.wait_heartbeat() - for sysid in self.namespaces: - sysid = int(sysid.replace('UAV', '')) + for sysid in self.sysids: self.set_sr_params(sysid) self.running = True @@ -42,8 +44,8 @@ class MAVLinkWorker(QThread): self.total_bytes_received = {} self.throughput_KBps = {} - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") + for sysid in self.sysids: + self.request_param(sysid, "SR1_EXTRA1") self.connection.mav.timesync_send( 0, #tc1 int(time.time() * 1e9) #ts1 @@ -52,7 +54,7 @@ class MAVLinkWorker(QThread): def run(self): while self.running: try: - msg = self.connection.recv_msg() + msg = self.connection.recv_msg() # Debugging line to see received messages current_time = time.time() if not msg: continue @@ -64,7 +66,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 @@ -162,7 +163,7 @@ class MAVLinkWorker(QThread): elif msg_type == 'PARAM_VALUE': param_name = "SR1_EXTRA1" if msg.param_id == param_name: - self.param_signal.emit(namespace, msg.param_value) + self.param_signal.emit(namespace, int(msg.param_value)) except Exception as e: print(f"Error reading message: {e}") @@ -171,8 +172,11 @@ class MAVLinkWorker(QThread): self.running = False self.connection.close() + def get_sysid(self, namespace): + return int(namespace.replace('UAV', '')) + def set_mode(self, namespace, mode): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) if mode == 'STABILIZE': mode = 0 elif mode == 'AUTO': @@ -188,7 +192,7 @@ class MAVLinkWorker(QThread): ) def arm(self, namespace, arm): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -199,7 +203,7 @@ class MAVLinkWorker(QThread): ) def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -210,7 +214,7 @@ class MAVLinkWorker(QThread): ) def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -221,7 +225,7 @@ class MAVLinkWorker(QThread): ) def set_local_position(self, namespace, x, y, z): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.set_position_target_local_ned_send( 0, sysid, 1, # time_boot_ms, sysid, compid mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame @@ -233,7 +237,7 @@ class MAVLinkWorker(QThread): ) def reboot_drone(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # target_component (一般為1) @@ -244,7 +248,7 @@ class MAVLinkWorker(QThread): ) def set_param(self, namespace, param_name, value): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) try: self.connection.mav.param_set_send( sysid, @@ -256,8 +260,7 @@ class MAVLinkWorker(QThread): 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', '')) + def request_param(self, sysid, param_name): try: self.connection.mav.param_request_read_send( sysid, # 系統 ID @@ -268,43 +271,57 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - ''' + def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 2, 4] + freqs = [0, 1, 4] params = { "SR1_ADSB": freqs[0], "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], - "SR1_EXTRA2": freqs[2], + "SR1_EXTRA2": freqs[1], "SR1_EXTRA3": freqs[1], - "SR1_POSITION": freqs[1], + "SR1_POSITION": freqs[2], "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 訊息頻率 """ + params = { + "SERIAL1_BAUD": 38 + } + 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] + freqs = [0, 3, 3] params = { "SR1_ADSB": freqs[0], - "SR1_EXT_STAT": freqs[0], + "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], "SR1_EXTRA2": freqs[2], - "SR1_EXTRA3": freqs[0], + "SR1_EXTRA3": freqs[1], "SR1_POSITION": freqs[1], "SR1_RAW_SENS": freqs[1], - "SR1_RC_CHAN": freqs[0] + "SR1_RC_CHAN": freqs[1] } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) + ''' + def init_drone(self, namespace): + sysid = self.get_sysid(namespace) + self.set_local_position(sysid, 0, 0, 0) class DroneControlApp(QMainWindow): def __init__(self): super().__init__() self.workers = MAVLinkWorker() + self.sysids = self.workers.sysids self.namespaces = self.workers.namespaces self.initUI() @@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow): self.rebootButton.clicked.connect(self.reboot_drone) main_layout.addWidget(self.rebootButton) + mission_layout = QHBoxLayout() + self.initButton = QPushButton("重設位置") + self.initButton.clicked.connect(self.init_drone) + self.startButton = QPushButton("開始任務") + self.startButton.clicked.connect(self.start_mission) + mission_layout.addWidget(self.initButton) + mission_layout.addWidget(self.startButton) + main_layout.addLayout(mission_layout) + # 設置主佈局 central_widget = QWidget(self) central_widget.setLayout(main_layout) @@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow): except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + def init_drone(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.init_drone(namespace) + print(f"重設位置：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}") + + def start_mission(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.start_mission(namespace) + print(f"開始任務：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}") + def main(): app = QtWidgets.QApplication(sys.argv) window = DroneControlApp() From 47a681e27d8ec34c77d6a8b069ceb16a347569ac Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 16 Jul 2025 17:05:29 +0800 Subject: [PATCH 069/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/gui.py | 765 ++++++++++++++++++++++++++------- 1 file changed, 621 insertions(+), 144 deletions(-) diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 6636fa3..5bc7213 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -4,7 +4,7 @@ from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, - QHeaderView, QPushButton, QCheckBox) + QHeaderView, QPushButton, QCheckBox, QLineEdit) from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtWebEngineWidgets import QWebEngineView import math @@ -18,6 +18,9 @@ from mavros_msgs.msg import State, VfrHud from mavros_msgs.srv import CommandBool, CommandTOL, SetMode import asyncio from functools import partial +import threading +import websockets +import json class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -31,14 +34,99 @@ class DroneMonitor(Node): self.arm_clients = {} self.takeoff_clients = {} + self.setpoint_pubs = {} self.selected_drones = set() + self.latest_data = {} + + # 啟動 WebSocket client 執行緒 + threading.Thread(target=self.start_ws_client, daemon=True).start() # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + + def start_ws_client(self): + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + retry_count = 0 + max_retries = 5 + base_delay = 1.0 + local = "ws://0.0.0.0:8765" # 本地 WebSocket 地址 + remote = "ws://140.120.31.123:8765" + while retry_count < max_retries: + try: + async with websockets.connect(local) as websocket: + print("WebSocket connected") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + try: + data = json.loads(message) + print(f"📡 Received: {data}") + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket JSON decode error: {e}") + except Exception as e: + print(f"WebSocket message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print("WebSocket connection closed") + break + except Exception as e: + retry_count += 1 + delay = base_delay * (2 ** min(retry_count, 4)) # 指數退避 + print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})") + await asyncio.sleep(delay) + print("WebSocket client stopped after maximum retries") + + + def process_websocket_message(self, data): + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + # 這裡假設 battery 是百分比 + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': round(pos.get('lat', 0), 6), + 'y': round(pos.get('lon', 0), 6), + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + def scan_topics(self): topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+)/(\w+)') + drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') found_drones = set() for topic_name, _ in topics: @@ -55,7 +143,6 @@ class DroneMonitor(Node): def setup_drone(self, drone_id): base_topic = f'/MavLinkBus/{drone_id}' - # Add service clients self.arm_clients[drone_id] = self.create_client( CommandBool, @@ -65,7 +152,14 @@ class DroneMonitor(Node): CommandTOL, f'{base_topic}/cmd/takeoff' ) - + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + subs = { 'attitude': self.create_subscription( Imu, @@ -130,7 +224,6 @@ class DroneMonitor(Node): } setattr(self, f'drone_{drone_id}_subs', subs) - self.signals.update_signal.emit('new_drone', drone_id, None) async def arm_drone(self, drone_id, arm): if drone_id not in self.arm_clients: @@ -171,7 +264,20 @@ class DroneMonitor(Node): except Exception as e: self.get_logger().error(f'Takeoff service call failed: {e}') return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + self.setpoint_pubs[drone_id].publish(msg) + return True + 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) @@ -190,71 +296,71 @@ class DroneMonitor(Node): 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, { + self.latest_data[(drone_id, 'attitude')] = { '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, { + self.latest_data[(drone_id, 'battery')] = { 'voltage': msg.voltage - }) + } def state_callback(self, drone_id, msg): - self.signals.update_signal.emit('state', drone_id, { + self.latest_data[(drone_id, 'state')] = { 'mode': msg.mode, 'armed': msg.armed - }) + } def gps_callback(self, drone_id, msg): - self.signals.update_signal.emit('gps', drone_id, { + self.latest_data[(drone_id, 'gps')] = { '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, { + self.latest_data[(drone_id, 'velocity')] = { 'vx': msg.x, 'vy': msg.y, 'vz': msg.z - }) + } def altitude_callback(self, drone_id, msg): - self.signals.update_signal.emit('altitude', drone_id, { + self.latest_data[(drone_id, 'altitude')] = { 'altitude': msg.data - }) + } def local_pose_callback(self, drone_id, msg): - self.signals.update_signal.emit('local_pose', drone_id, { + self.latest_data[(drone_id, 'local_pose')] = { 'x': msg.x, 'y': msg.y, 'z': msg.z - }) + } def hud_callback(self, drone_id, msg): - self.signals.update_signal.emit('hud', drone_id, { + self.latest_data[(drone_id, 'hud')] = { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, 'throttle': msg.throttle, 'alt': msg.altitude, 'climb': msg.climb - }) + } def loss_rate_callback(self, drone_id, msg): - self.signals.update_signal.emit('loss_rate', drone_id, { + self.latest_data[(drone_id, 'loss_rate')] = { 'loss_rate': msg.data - }) + } def ping_callback(self, drone_id, msg): - self.signals.update_signal.emit('ping', drone_id, { + self.latest_data[(drone_id, 'ping')] = { 'ping': msg.data - }) + } class ControlStationUI(QMainWindow): def __init__(self): @@ -271,8 +377,14 @@ class ControlStationUI(QMainWindow): self.executor = rclpy.executors.SingleThreadedExecutor() self.executor.add_node(self.monitor) + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(10) + # 初始化UI self.drones = {} + self.socket_groups = {} # 新增：用於儲存 socket 分組 self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", "Roll", "Pitch", "Yaw", "丟包", "延遲"] self.info_type_map = { @@ -293,12 +405,15 @@ class ControlStationUI(QMainWindow): self.drone_positions = {} self.drone_headings = {} self.map_loaded = False + # 添加地圖更新節流定時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次地圖 + # 添加待更新的無人機位置緩存 + self.pending_map_updates = {} + self.init_ui() - # 定时处理ROS事件 - self.timer = QTimer() - self.timer.timeout.connect(self.spin_ros) - self.timer.start(50) def init_ui(self): @@ -323,7 +438,7 @@ class ControlStationUI(QMainWindow): # 底部控制按鈕區域 bottom_control = QWidget() - bottom_layout = QHBoxLayout(bottom_control) + bottom_layout = QVBoxLayout(bottom_control) select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) @@ -367,10 +482,82 @@ class ControlStationUI(QMainWindow): QPushButton:hover { background-color: #555; } """) - bottom_layout.addWidget(select_all_btn) - bottom_layout.addWidget(arm_all_btn) - bottom_layout.addWidget(takeoff_all_btn) - bottom_layout.addStretch() + # Add coordinate inputs + coord_widget = QWidget() + coord_layout = QHBoxLayout(coord_widget) + coord_layout.setContentsMargins(0, 0, 0, 0) + + self.x_input = QLineEdit() + self.y_input = QLineEdit() + self.z_input = QLineEdit() + + for input_field in [self.x_input, self.y_input, self.z_input]: + input_field.setFixedWidth(60) + input_field.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) + + coord_layout.addWidget(QLabel("X:")) + coord_layout.addWidget(self.x_input) + coord_layout.addWidget(QLabel("Y:")) + coord_layout.addWidget(self.y_input) + coord_layout.addWidget(QLabel("Z:")) + coord_layout.addWidget(self.z_input) + + # Add buttons to control layout + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.clicked.connect(self.handle_setpoint_selected) + setpoint_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + + # 上方按鈕區域 + upper_buttons = QHBoxLayout() + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + arm_all_btn = QPushButton("批次解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: + btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 5px 10px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + upper_buttons.addWidget(btn) + upper_buttons.addStretch() + + # 下方座標輸入區域 + lower_control = QHBoxLayout() + lower_control.addWidget(coord_widget) + lower_control.addWidget(setpoint_btn) + lower_control.addStretch() + + + # 將兩個區域加入底部控制區 + bottom_layout.addLayout(upper_buttons) + bottom_layout.addLayout(lower_control) # — 分頁 2：Overview Table self.overview_table = QTableWidget() @@ -408,9 +595,9 @@ class ControlStationUI(QMainWindow):
+
+ +
+ @@ -1167,18 +1260,16 @@ class ControlStationUI(QMainWindow): elif msg_type == 'battery': voltage = data.get('voltage', 16) - # 使用標準電壓判斷電池節數 - cell_max = 4.2 + # 判斷電池節數 cells = round(voltage / 3.95) - max = cell_max * cells # 計算電量百分比 - percentage = voltage / max * 100 + percentage = (voltage / cells - 3.7) / 0.5 * 100 # 根據百分比設置顏色 if percentage < 20: voltage_color = '#FF6464' # 紅色 (低電量) - elif percentage < 40: + elif percentage < 50: voltage_color = '#FFA500' # 橘色 (中低電量) else: voltage_color = '#FFFFFF' # 白色 (正常電量) From 4388ebd9ede6d0098f0403b0edb3168e0cc223c0 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 14 Aug 2025 17:17:04 +0800 Subject: [PATCH 075/146] =?UTF-8?q?item51&54=20=E9=80=A3=E6=8E=A5=E5=A4=9A?= =?UTF-8?q?=E5=80=8Bport?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/devRun.py | 763 +----------------- 1 file changed, 42 insertions(+), 721 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py index 133e56d..78abb8f 100644 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ b/src/fc_network_adapter/fc_network_adapter/devRun.py @@ -18,452 +18,7 @@ test_item = 51 running_time =10000 print('test_item : ', test_item) -if test_item == 1: - # 測試 mavlink_object 中 updateMultiplexingList 的輸出 - print('===> Start of Program .Test ', test_item) - mavlink_object_none = mo.mavlink_object(None) - - print('=====================') - print('正常範例') - mavlink_object_none.multiplexingToAnalysis = [] - mavlink_object_none.multiplexingToReturn = [] - # mavlink_object_none.multiplexingToSwap = [[]] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - print('multiplexingList : ', mavlink_object_none._multiplexingList) - - print('=====================') - print('錯誤範例 長度不一致') - mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - - print('=====================') - print('正常範例') - mo.swap_queues.append(queue.Queue()) - mo.swap_queues.append(queue.Queue()) - mavlink_object_none.multiplexingToAnalysis = [0, 1, 2, 3, 4, 5] - mavlink_object_none.multiplexingToReturn = [6, 7, 8, 9, 10] - mavlink_object_none.multiplexingToSwap = [[-1], [11,12,13], [14,15,16],] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - print('multiplexingList : ', mavlink_object_none._multiplexingList) - - print('=====================') - print('錯誤範例 multiplexingToAnalysis 不可以有 -1') - mavlink_object_none.multiplexingToAnalysis = [-1, 1, 2, 3, 4, 5] - ret = mavlink_object_none.updateMultiplexingList() - print('execute result : ', ret) - - print('<=== End of Program') - -elif test_item == 2: - # 測試 mavlink_object 創建時 socket_id 是否正確 - # 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試 - print('===> Start of Program .Test ', test_item) - mavlink_object_none1 = mo.mavlink_object(None) - mavlink_object_none2 = mo.mavlink_object(None) - mavlink_object_none3 = mo.mavlink_object(None) - del mavlink_object_none2 - - print(len(mo.swap_queues)) - - mavlink_object_none2 = mo.mavlink_object(None) - print(len(mo.swap_queues)) - - print(mavlink_object_none1._multiplexingList) - - print('<=== End of Program') - -elif test_item == 10: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 - # 只啟用了 mavlink_object 的功能 - print('===> Start of Program .Test ', test_item) - - # 創建一個空的通道 這個通道的 socket_id 是 0 - mavlink_object_none = mo.mavlink_object(None) - - # 創建另一個通道 - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object1 = mo.mavlink_object(mavlink_socket) - # 設定通道轉發的參數 - mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE - mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT - mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD - - # print(mavlink_object1.multiplexingToSwap) - # print(mo.swap_queues) - - # 啟動通道 - mavlink_object1.run() - - # 確認轉拋的設定有沒有錯 - print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList) - - # 運行幾秒並印出 queue 的資料 - start_time = time.time() - while time.time() - start_time < running_time: - while not mo.fixed_stream_bridge_queue.empty(): - print('fixed_stream_bridge_queue:') - t = mo.fixed_stream_bridge_queue.get() - print('from {} : {}'.format(t[0],t[2])) - while not mo.return_packet_processor_queue.empty(): - print('return_packet_processor_queue:') - t = mo.return_packet_processor_queue.get() - print('from {} : {}'.format(t[0],t[2])) - # print(t[2].get_msgbuf()) - # print(t[2].type) - - for n in range(len(mo.swap_queues)): - q = mo.swap_queues[n] - while not q.empty(): - print('swap_queues:') - t = q.get() - print('{} gets : {}'.format(n,t)) - - - # 結束程式 退出所有 thread - mavlink_object1.running = False - mavlink_object1.thread.join() - mavlink_socket.close() - print('<=== End of Program') - -elif test_item == 11: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 - # 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能 - print('===> Start of Program .Test ', test_item) - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - - # 創建通道 - connection_string="udp:127.0.0.1:14550" - mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object2 = mo.mavlink_object(mavlink_socket) - # mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT - # mavlink_object2.multiplexingToReturn = [] - - # 啟動連線的模組 - mavlink_object2.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(2) - print("=== connection established! ===") - - # 印出目前所有 mavlink_systems 的內容 - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - start_time = time.time() - show_time = time.time() - compid = 1 - while time.time() - start_time < running_time: - 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("===================") - - # 結束程式 退出所有 thread - mavlink_object2.stop() - mavlink_object2.thread.join() - analyzer.stop() - - analyzer = mo.mavlink_bridge() # 這邊是測試是否只有一個 instance - analyzer.thread.join() - mavlink_socket.close() - print('<=== End of Program') - -elif test_item == 12: - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 初始化輸入通道 - connection_string_in="udp:127.0.0.1:14552" - mavlink_socket_in = mavutil.mavlink_connection(connection_string_in) - mavlink_object_in = mo.mavlink_object(mavlink_socket_in) - mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT - - # 初始化輸出通道 - connection_string_out="udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [0] - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 讓兩個通道互相傳輸 - mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all - mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - # mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all - - # 啟動通道 - mavlink_object_in.run() - mavlink_object_out.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - # print(type(mavlink_socket_out)) - # print(type(mavlink_socket_out.mav)) - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object : ', test) - except queue.Empty: - pass - - if (time.time() - show_time) >= 2: - show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - print("===================") - - - - # 結束程式 退出所有 thread - mavlink_object_in.stop() - mavlink_object_in.thread.join() - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_in.close() - mavlink_socket_out.close() - - analyzer.stop() - print('<=== End of Program') - -elif test_item == 20: - # 這邊測試 node 生成 topic 的功能 - # 利用 空的通道 發出假的 heartbeat 封包 - print('===> Start of Program .Test ', test_item) - rclpy.init() # 注意要初始化 rclpy 才能使用 node - - from pymavlink.dialects.v20 import common as mavlink2 - - sysid = 1 - compid = 1 - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - - mav = mavlink2.MAVLink(None) - mav.srcSystem = sysid - mav.srcComponent = compid - - # 這是一個 heartbeat 封包 - fake_heartbeat = mavlink2.MAVLink_heartbeat_message( - type=mavlink2.MAV_TYPE_QUADROTOR, - autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode=3, - custom_mode=0, - system_status=0, - mavlink_version=3 - ) - mavlink_bytes = fake_heartbeat.pack(mav) - fake_heartbeat_msg = mav.parse_char(mavlink_bytes) - - print(analyzer.mavlink_systems) - - mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data - time.sleep(0.1) - print(analyzer.mavlink_systems) - - # ROS2 初始化 - analyzer._init_node() - - # 創建 ROS2 topic - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid - print("topic created") - time.sleep(5) - - # 丟出 topic - analyzer.emit_info() - - # 結束程式 - analyzer.running = False - analyzer.destroy_node() - - rclpy.shutdown() - analyzer.stop() - analyzer.thread.join() - print('<=== End of Program') - -elif test_item == 21: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - - 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_socket = mavutil.mavlink_connection(connection_string) - mavlink_object3 = mo.mavlink_object(mavlink_socket) - # 啟動通道 - mavlink_object3.run() - - - print('=== waiting for mavlink data ...') - time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - compid = 1 - sysid = 1 - start_time = time.time() - analyzer.create_flightMode(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") - - 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(1) - except KeyboardInterrupt: - break - - - # 程式結束 - 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') - -elif test_item == 22: - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - 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_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" - mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [] - - - - # 讓兩個通道互相傳輸 - mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all - mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - - # 啟動通道 - mavlink_object_in.run() - mavlink_object_out.run() - - print('waiting for mavlink data ...') - time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - compid = 1 - sysid = 1 - show_time = time.time() - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) - print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") - - print("start emit info") - - start_time = time.time() - show_time = time.time() - show_time2 = time.time() - - while time.time() - start_time < running_time: - if (time.time() - show_time2) >= 1: - analyzer.emit_info() # 這邊是測試 node 的運行 - # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] - fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] - sss = mavutil.mode_string_v10(fmsg) - # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) - print("目前的飛行模式 : {}".format(sss)) - show_time2 = time.time() - - # if (time.time() - show_time) >= 2: - # show_time = time.time() - # for sysid in analyzer.mavlink_systems: - # for compid in analyzer.mavlink_systems[sysid].components: - # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - # print("===================") - - - - analyzer.destroy_node() - rclpy.shutdown() - - - # 結束程式 退出所有 thread - mavlink_object_in.stop() - mavlink_object_in.thread.join() - mavlink_socket_in.close() - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_out.close() - analyzer.stop() - analyzer.thread.join() - - - print('<=== End of Program') - -elif test_item == 51: +if test_item == 51: # 晉凱的測試項目 - 修改為支援多UDP連接 print('===> Start of Program .Test ', test_item) rclpy.init() # 注意要初始化 rclpy 才能使用 node @@ -476,18 +31,18 @@ elif test_item == 51: print('初始化 node 完成 耗時 : ',time.time() - show_time) # === 修改：支援多個UDP連接 === - udp_ports = [14551, 14552, 14553] # 可以根據需要調整端口 + ports = ["udp:127.0.0.1:14550", + "udp:127.0.0.1:14570", + "/dev/ttyUSB0", + "/dev/ttyUSB1"] # 可以根據需要調整端口 mavlink_sockets = [] mavlink_objects = [] - print(f"設定連接到 UDP 端口: {udp_ports}") - # 循環創建多個連接 - for i, port in enumerate(udp_ports): + for i, port in enumerate(ports): try: print(f"連接到 UDP:{port}") - connection_string = f"udp:127.0.0.1:{port}" - mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_socket = mavutil.mavlink_connection(port) mavlink_object = mo.mavlink_object(mavlink_socket) # 設定通道流動（所有連接使用相同參數） @@ -529,13 +84,6 @@ elif test_item == 51: print(f"為系統 {sysid}, 組件 {compid} 創建topics...") 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]) topics_created += 1 print(f"系統 {sysid}_{compid} topics創建完成") @@ -563,7 +111,7 @@ elif test_item == 51: loop_count += 1 if loop_count % 10 == 0: # 每5秒顯示一次（0.5s * 10） print(f"\n=== 狀態更新 (第 {loop_count} 次循環) ===") - print(f"連接的UDP端口: {udp_ports[:len(mavlink_sockets)]}") + #print(f"連接的端口: {udp_ports[:len(mavlink_sockets)]}") print(f"檢測到的系統數: {len(analyzer.mavlink_systems)}") for sysid in analyzer.mavlink_systems: @@ -626,7 +174,7 @@ elif test_item == 51: # 清理所有mavlink objects for i, mavlink_obj in enumerate(mavlink_objects): try: - print(f"停止 mavlink_object {i+1} (UDP:{udp_ports[i]}, socket_id: {mavlink_obj.socket_id})") + print(f"停止 mavlink_object {i+1} (UDP:{ports[i]}, socket_id: {mavlink_obj.socket_id})") mavlink_obj.stop() mavlink_obj.thread.join() print(f"mavlink_object {i+1} 已停止") @@ -636,10 +184,10 @@ elif test_item == 51: # 關閉所有mavlink sockets for i, mavlink_sock in enumerate(mavlink_sockets): try: - print(f"關閉 UDP 連接 {udp_ports[i]}") + print(f"關閉 UDP 連接 {ports[i]}") mavlink_sock.close() except Exception as e: - print(f"關閉 UDP 連接 {udp_ports[i]} 失敗: {e}") + print(f"關閉 UDP 連接 {ports[i]} 失敗: {e}") # 清理analyzer try: @@ -653,221 +201,6 @@ elif test_item == 51: print(f"清理完成，共處理了 {len(mavlink_sockets)} 個 UDP 連接") print('<=== End of Program') -elif test_item == 52: - # 文鈞的測試項目 - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 共用的輸出連接 (連接到GCS) - common_out = "udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(common_out) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - mavlink_object_out.multiplexingToAnalysis = [0] - - # 設定多台無人機的輸入連接 - drone_inputs = [ - #"udp:127.0.0.1:14550", # 無人機1 - #"udp:127.0.0.1:14550", # 無人機2 - "udp:127.0.0.1:14550", # 無人機3 - "udp:127.0.0.1:14554" # 無人機4 - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, input_conn in enumerate(drone_inputs): - print(f"連接無人機 {i+1} 輸入: {input_conn}") - mavlink_in = mavutil.mavlink_connection(input_conn) - obj_in = mo.mavlink_object(mavlink_in) - obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定轉發關係 - obj_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # 所有訊息 - mavlink_object_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # 所有訊息 - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 啟動所有通道 - for obj in mavlink_objects_in: - obj.run() - mavlink_object_out.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object : ', test) - except queue.Empty: - pass - - if (time.time() - show_time) >= 8: - show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - print("===================") - - # 結束程式 退出所有 thread - for i, obj in enumerate(mavlink_objects_in): - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - mavlink_object_out.stop() - mavlink_object_out.thread.join() - mavlink_socket_out.close() - - analyzer.stop() - print('<=== End of Program') - -elif test_item == 53: - # 文鈞的測試項目 - 雙輸出版本 - # 需要開啟一個 ardupilot 的模擬器 與 兩個 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼，支援雙輸出 - print('===> Start of Program .Test ', test_item) - - # 啟動連線的模組 - analyzer = mo.mavlink_bridge() - - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14560", # GCS 1 - "udpout:127.0.0.1:14570" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定多台無人機的輸入連接 - drone_inputs = [ - "udp:127.0.0.1:14551", # 無人機3 - "udp:127.0.0.1:14552" # 無人機4 - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, input_conn in enumerate(drone_inputs): - print(f"連接無人機 {i+1} 輸入: {input_conn}") - mavlink_in = mavutil.mavlink_connection(input_conn) - obj_in = mo.mavlink_object(mavlink_in) - obj_in.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定無人機到所有GCS的轉發關係 - for obj_out in mavlink_objects_out: - obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ] # 無人機→所有GCS - print(f"設定無人機 {i+1} (socket_id: {obj_in.socket_id}) → GCS (socket_id: {obj_out.socket_id}) 轉發") - - # 設定GCS到所有無人機的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有無人機 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 無人機 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - # 啟動所有輸入通道 - for obj in mavlink_objects_in: - obj.run() - - # 啟動所有輸出通道 - for obj in mavlink_objects_out: - obj.run() - - # 做點延遲 讓 heartbeat 先吃進來 - time.sleep(3) - print("=== connection established! ===") - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - # 顯示轉發設定摘要 - print("\n=== 轉發設定摘要 ===") - print(f"無人機數量: {len(mavlink_objects_in)}") - print(f"GCS數量: {len(mavlink_objects_out)}") - print("轉發規則:") - print(" - 每台無人機的所有訊息 → 所有GCS") - print(" - 每個GCS的所有訊息 → 所有無人機") - print("===================\n") - - start_time = time.time() - show_time = time.time() - - while time.time() - start_time < running_time: - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object 收到訊息: ', test) - except queue.Empty: - pass - - if (time.time() - show_time) >= 5: # 改為每5秒顯示一次，更即時監控 - show_time = time.time() - print(f"\n=== 系統狀態報告 (運行時間: {int(time.time() - start_time)}秒) ===") - - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - msg_count = analyzer.mavlink_systems[sysid].components[compid].msg_count - print(f"系統ID: {sysid}, 元件ID: {compid}, 收到訊息數量: {msg_count}") - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - - # 顯示各通道的狀態 - print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)") - print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)") - print("===================\n") - - # 結束程式 退出所有 thread - print("正在關閉所有連接...") - - # 關閉輸入通道 - for i, obj in enumerate(mavlink_objects_in): - print(f"關閉無人機 {i+1} 輸入通道") - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - # 關閉輸出通道 - for i, obj in enumerate(mavlink_objects_out): - print(f"關閉 GCS {i+1} 輸出通道") - obj.stop() - obj.thread.join() - mavlink_sockets_out[i].close() - - analyzer.stop() - print('<=== End of Program') - - elif test_item == 54: # 文鈞的測試項目 - 5輸入2輸出版本 + 結合test51的ROS2功能 # 加入詳細調試信息 @@ -915,19 +248,43 @@ elif test_item == 54: print("ROS2 bridge 初始化完成") + # 雙輸出連接設定 (連接到兩個不同的GCS) + gcs_outputs = [ + "udpout:127.0.0.1:14500", # GCS 1 + "udpout:127.0.0.1:14600" # GCS 2 + ] + + # 建立輸出連接物件 + mavlink_objects_out = [] + mavlink_sockets_out = [] + # 設定5個輸入連接（修改為實際測試可用的端口） device_inputs = [ - "udp:127.0.0.1:14551", # 無人機1 (UDP) - "udp:127.0.0.1:14552", # 無人機2 (UDP) - "udp:127.0.0.1:14553", # 無人機3 (UDP) - "udp:127.0.0.1:14554", # 無人機4 (UDP) - "udp:127.0.0.1:14555", # 無人機5 (UDP) + "udp:127.0.0.1:14550", # 無人機1 (UDP) + "udp:127.0.0.1:14570", # 無人機2 (UDP) + "/dev/ttyUSB0", # 無人機3 (UDP) + "/dev/ttyUSB1", # 無人機4 (UDP) + "/dev/ttyUSB2", # 無人機5 (UDP) ] # 建立輸入連接 mavlink_objects_in = [] mavlink_sockets_in = [] + for i, output_conn in enumerate(gcs_outputs): + print(f"建立 GCS {i+1} 輸出連接: {output_conn}") + mavlink_out = mavutil.mavlink_connection(output_conn) + obj_out = mo.mavlink_object(mavlink_out) + obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 + mavlink_objects_out.append(obj_out) + mavlink_sockets_out.append(mavlink_out) + + # 設定GCS到所有設備的轉發關係 + for i, obj_out in enumerate(mavlink_objects_out): + for j, obj_in in enumerate(mavlink_objects_in): + obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 + print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") + for i, input_conn in enumerate(device_inputs): print(f"連接設備 {i+1} 輸入: {input_conn}") try: @@ -954,30 +311,6 @@ elif test_item == 54: print(f"跳過設備 {i+1}") continue - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14560", # GCS 1 - "udpout:127.0.0.1:14570" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定GCS到所有設備的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - # 做一個空的通道驗證 可以拿來 debug mavlink_object_none = mo.mavlink_object(None) @@ -1056,7 +389,6 @@ elif test_item == 54: try: while not mo.fixed_stream_bridge_queue.empty(): msg = mo.fixed_stream_bridge_queue.get(block=False) - print(f'[DEBUG] fixed_stream_bridge_queue: from socket {msg[0]}: {msg[2].get_type()}') debug_counters['analysis_messages'] += 1 message_count += 1 except queue.Empty: @@ -1066,7 +398,6 @@ elif test_item == 54: try: while not mo.return_packet_processor_queue.empty(): msg = mo.return_packet_processor_queue.get(block=False) - print(f'[DEBUG] return_packet_processor_queue: from socket {msg[0]}: {msg[2].get_type()}') debug_counters['return_messages'] += 1 message_count += 1 except queue.Empty: @@ -1083,8 +414,6 @@ elif test_item == 54: except Exception as e: print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}") - if timesync_sent > 0: - print(f"[DEBUG] 發送 TIMESYNC 到 {timesync_sent} 個設備") last_timesync = now # ROS2 發布 (來自test51新版本) @@ -1098,10 +427,9 @@ elif test_item == 54: # 狀態報告 (更頻繁的調試輸出) if (time.time() - show_time) >= 2: # 每2秒顯示一次狀態 show_time = time.time() - print(f"\n=== 調試狀態報告 (運行時間: {int(now - time.time() + running_time)}秒) ===") + print(f"\n=== 調試狀態報告===") # 顯示消息統計 - print(f"消息計數器: {debug_counters}") print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}") # 重置調試計數器 @@ -1115,14 +443,7 @@ elif test_item == 54: for compid in system.components: component = system.components[compid] msg_count = component.msg_count - print(f" 元件 {compid}: 收到訊息數量={msg_count}") - # 顯示 emitParams 內容 - if hasattr(component, 'emitParams') and component.emitParams: - print(f" emitParams keys: {list(component.emitParams.keys())}") - else: - print(" emitParams: 空") - system.resetComponentPacketCount(compid) else: print("目前沒有檢測到任何MAVLink系統") @@ -1184,4 +505,4 @@ elif test_item == 54: # 關閉分析器 bridge.stop() bridge.thread.join() - print('<=== End of Program') + print('<=== End of Program') \ No newline at end of file From 7af138b02a22198aaa02d0fd882f3dabb954b58e Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 12 Nov 2025 20:26:49 +0800 Subject: [PATCH 076/146] =?UTF-8?q?1.=20=E8=AA=BF=E6=95=B4=E6=AA=94?= =?UTF-8?q?=E6=A1=88=E7=B5=90=E6=A7=8B=20=E8=AE=8A=E5=8B=95=20import=20?= =?UTF-8?q?=E7=9A=84=E8=B7=AF=E5=BE=91=E8=88=87=E6=96=B9=E6=B3=95=202.=20?= =?UTF-8?q?=E6=96=B0=E5=A2=9E=20mainOrchestrator.py=20=E4=BD=9C=E7=82=BA?= =?UTF-8?q?=E6=8E=A5=E4=B8=8B=E4=BE=86=E9=96=8B=E7=99=BC=E4=BB=8B=E9=9D=A2?= =?UTF-8?q?=E5=8C=96=E7=B5=B1=E5=90=88=E5=B7=A5=E5=85=B7=E7=9A=84=E4=B8=BB?= =?UTF-8?q?=E8=A6=81=E6=AA=94=E6=A1=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 11 +- .../fc_network_adapter/mainOrchestrator.py | 262 +++++++++++ .../fc_network_adapter/mavlinkDevice.py | 6 +- .../fc_network_adapter/mavlinkObject.py | 15 +- .../fc_network_adapter/mavlinkPublish.py | 6 +- .../fc_network_adapter/test_mavlinkObject.py | 0 .../fc_network_adapter/utils/__init__.py | 7 + .../{ => utils}/ringBuffer.py | 0 .../{ => utils}/theLogger.py | 2 +- src/fc_network_adapter/setup.py | 1 + .../tests/{devRun.py => demo_integration.py} | 6 +- .../tests/demo_ringBuffer.py | 152 +++++++ .../tests/test_mavlinkObject.py | 3 +- .../tests/test_ringBuffer.py | 418 ++++++++++++------ 14 files changed, 735 insertions(+), 154 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py delete mode 100644 src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/__init__.py rename src/fc_network_adapter/fc_network_adapter/{ => utils}/ringBuffer.py (100%) rename src/fc_network_adapter/fc_network_adapter/{ => utils}/theLogger.py (93%) rename src/fc_network_adapter/tests/{devRun.py => demo_integration.py} (98%) create mode 100644 src/fc_network_adapter/tests/demo_ringBuffer.py diff --git a/README.md b/README.md index 7fd8868..8e81de0 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,15 @@ Package 簡述 2. fc_network_adapter 建立、維護與飛控韌體的連接 構築 mavlink 封包 - 同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 處理無線模組的通訊格式 (XBee) + --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- + +N. logs 是執行時期的記錄檔 + +=== +請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式 + +例如 在 ~/AirTrapMine/src/ 資料夾下 +> python -m fc_network_adapter.fc_network_adapter.mainOrchestrator +> python -m fc_network_adapter.tests.test_ringBuffer diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py new file mode 100644 index 0000000..805f1eb --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -0,0 +1,262 @@ + +''' + +主要調配流程的程式 + +這個檔案包含 Terminal Utility Layer (TUL) 作為人機互動介面，並調用 mavlinkDevice 和 mavlinkObject 來處理 MAVLink 通訊和物件管理。 + +''' + + +import os +import time +import sys + +import curses +import threading +import queue +import signal + +# 自定義的 import +from . import mavlinkObject as mo +from . import mavlinkDevice as md +from .utils import RingBuffer, setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +class MenuNode: + def __init__(self, name, desc="", action=None, children=None): + self.name = name + self.desc = desc + self.action = action # 可以是函式或特殊字串 + self.children = children or [] # 子選單列表 + +class ControlPanel: + def __init__(self): + pass + + def menu_tree(self): + """建立多層選單結構""" + return MenuNode("Main Menu", children=[ + MenuNode("MavLink Object Control", children=[ + MenuNode("New+", "新增一個連結口", "ADD_MAV_OBJECT"), + MenuNode("Remove", "移除某個連結口", "REMOVE_MAV_OBJECT"), + MenuNode("ListAll", "顯示連結口列表", "LIST_MAV_OBJECT"), + MenuNode("Inspection", "顯示連結口資訊", "INSPECT_MAV_OBJECT"), + ]), + MenuNode("參數設定", children=[ + MenuNode("Speed", children=[ + MenuNode("Increase Speed", "增加速度", "ADJUST_SPEED"), + MenuNode("Decrease Speed", "減少速度", "ADJUST_SPEED"), + ]), + MenuNode("輸入文字", "鍵盤輸入模式", "INPUT_TEXT"), + ]), + MenuNode("資訊查看", children=[ + MenuNode("顯示狀態", "查看當前狀態", "SHOW_STATUS"), + MenuNode("顯示輸入", "查看用戶輸入", "SHOW_INPUT"), + ]), + MenuNode("返回上層", "回到上一層選單", "BACK"), + MenuNode("關閉面板", "關閉控制面板", "QUIT"), + ]) + + def panel_thread(self, cmd_q: queue.Queue, stop_evt: threading.Event): + stdscr = None + + def cleanup(): + """清理 curses 狀態""" + if stdscr: + stdscr.keypad(False) + curses.nocbreak() + curses.echo() + curses.endwin() + + def draw_menu(screen): + nonlocal stdscr + stdscr = screen + + curses.curs_set(0) + stdscr.nodelay(False) # 阻塞讀鍵 + stdscr.keypad(True) + + # 選單導航狀態 + menu_stack = [self.menu_tree()] # 選單堆疊 + idx_stack = [0] # 索引堆疊 + + while not stop_evt.is_set(): + # 檢查是否需要退出 + if stop_evt.is_set(): + break + + current_menu = menu_stack[-1] + current_idx = idx_stack[-1] + + stdscr.clear() + + stdscr.border() + stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f"mavlink bridge state : ") + stdscr.addstr(2, 2, f"object manager state : ") + stdscr.addstr(3, 2, f"Node state : ") + + # # Header - 顯示選單路徑 + # path = " → ".join([menu.name for menu in menu_stack]) + # stdscr.addstr(0, 2, f"控制面板: {path}", curses.A_BOLD) + # stdscr.addstr(1, 2, f"狀態: {state.status} | 速度: {state.speed} | 計數: {state.counter}") + # if state.user_input: + # stdscr.addstr(2, 2, f"輸入: {state.user_input[:50]}...") + + # 顯示當前選單項目 + start_line = 5 + for i, child in enumerate(current_menu.children): + marker = "➤ " if i == current_idx else " " + line = f"{marker}{child.name:15s} – {child.desc}" + attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL + stdscr.addstr(start_line + i, 4, line, attr) + + # 操作說明 + help_line = start_line + len(current_menu.children) + 2 + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 ←→調參數 q退出", curses.A_DIM) + + stdscr.refresh() + + # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt + stdscr.timeout(100) # 100ms timeout + ch = stdscr.getch() + + if ch == -1: # timeout，繼續檢查 stop_evt + continue + + # 處理按鍵 + if ch in (curses.KEY_UP, ord('k')): + idx_stack[-1] = (current_idx - 1) % len(current_menu.children) + + elif ch in (curses.KEY_DOWN, ord('j')): + idx_stack[-1] = (current_idx + 1) % len(current_menu.children) + + elif ch == curses.KEY_LEFT: + # 返回上層或調整參數 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + else: + # 在根選單，檢查是否為調整參數 + selected = current_menu.children[current_idx] + if selected.action == "ADJUST_SPEED": + state.speed = max(1, state.speed - 1) + + elif ch == curses.KEY_RIGHT: + # 調整參數 + selected = current_menu.children[current_idx] + if selected.action == "ADJUST_SPEED": + state.speed = min(10, state.speed + 1) + + elif ch in (curses.KEY_ENTER, 10, 13): + selected = current_menu.children[current_idx] + + # 處理不同類型的動作 + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif selected.action == "BACK": + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "QUIT": + break + + elif selected.action == "INPUT_TEXT": + # 進入輸入模式 + result = input_dialog(stdscr, "請輸入文字: ") + if result is not None: + cmd_q.put(lambda: state.set_user_input(result)) + + elif selected.action == "SHOW_STATUS": + # 顯示狀態訊息 + stdscr.clear() + stdscr.addstr(5, 2, f"當前狀態: {state.status}") + stdscr.addstr(6, 2, f"速度設定: {state.speed}") + stdscr.addstr(7, 2, f"計數器: {state.counter}") + stdscr.addstr(9, 2, "按任意鍵返回...") + stdscr.refresh() + stdscr.getch() + + elif selected.action == "SHOW_INPUT": + # 顯示用戶輸入 + stdscr.clear() + stdscr.addstr(5, 2, f"用戶輸入內容:") + stdscr.addstr(6, 2, f"{state.user_input}") + stdscr.addstr(8, 2, "按任意鍵返回...") + stdscr.refresh() + stdscr.getch() + + elif callable(selected.action): + # 執行函式 + cmd_q.put(selected.action) + + elif ch in (ord('q'), 27): + break + + try: + curses.wrapper(draw_menu) + except KeyboardInterrupt: + pass + finally: + cleanup() + +def main(): + logger.warning(f"Hello this is mainOrchestrator.py") + pp = ControlPanel() + + cmd_q = queue.Queue() + stop_evt = threading.Event() + panel_thread_obj = None + + def signal_handler(signum, frame): + """處理 Ctrl+C 信號""" + print("\n收到中斷信號，正在安全退出...") + stop_evt.set() + if panel_thread_obj and panel_thread_obj.is_alive(): + panel_thread_obj.join(timeout=2) + sys.exit(0) + + # 註冊信號處理器 + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + # 啟動控制面板（改為非 daemon） + panel_thread_obj = threading.Thread(target=pp.panel_thread, args=(cmd_q, stop_evt)) + panel_thread_obj.start() + + print("多層選單控制面板啟動。Ctrl+C 結束程式。") + + try: + while not stop_evt.is_set(): + # 取出面板丟過來的「動作」 + try: + fn = cmd_q.get_nowait() + fn() # 執行對應動作 + except queue.Empty: + pass + + # # 模擬你的長跑邏輯 + # if state.status == "running": + # # 依 speed 前進 + # state.counter += state.speed + + time.sleep(0.1) + except KeyboardInterrupt: + print("\n收到 Ctrl+C，準備結束...") + finally: + stop_evt.set() + if panel_thread_obj.is_alive(): + panel_thread_obj.join(timeout=2) + pass + + +if __name__ == "__main__": + main() + + diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py index 47a1c92..2d7ea06 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py @@ -1,10 +1,12 @@ +import os + # 自定義的 import -from .theLogger import setup_logger +from .utils import setup_logger # ====================== 分割線 ===================== -logger = setup_logger("mavlinkDevice.py") +logger = setup_logger(os.path.basename(__file__)) # 用來記錄每個 system 的資訊 # 資料格式 { sysid : mavlink_device object } diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index dadb58f..a132e97 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -12,6 +12,7 @@ # 基礎功能的 import import threading +import os # import queue import time import asyncio @@ -27,12 +28,12 @@ from rclpy.node import Node # 自定義的 import from .mavlinkDevice import mavlink_device, mavlink_systems from .mavlinkPublish import mavlink_publisher -from .theLogger import setup_logger -from .ringBuffer import RingBuffer +from .utils import RingBuffer, setup_logger + # ====================== 分割線 ===================== -logger = setup_logger("mavlinkObject.py") +logger = setup_logger(os.path.basename(__file__)) stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255) return_packet_ring = RingBuffer(capacity=256, buffer_id=254) @@ -328,7 +329,7 @@ class mavlink_object: while self.state == MavlinkObjectState.RUNNING: timestamp = time.time() - try: + try: #TODO 這邊的錯誤處理要再想想看怎麼做比較好 # 處理接收到的封包 mavlinkMsg = self.mavlink_socket.recv_msg() # except Exception as e: @@ -368,7 +369,7 @@ class mavlink_object: if target_port != self.socket_id and target_port in self.mavlinkObjects: target_obj = self.mavlinkObjects[target_port] if target_obj.state == MavlinkObjectState.RUNNING: - try: + try: # TODO 藉由 if 的檢測 確定目標端口是開啟狀態後 再進行寫出 之後刪掉 try except target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) except Exception as e: logger.error(f"Error forwarding message to port {target_port}: {e}") @@ -449,7 +450,7 @@ class mavlink_object: try: # 使用鎖保護共享資源訪問 - with self.outgoing_msgs_lock: + with self.outgoing_msg_lock: self.outgoing_msgs.append(message_bytes) return True except Exception as e: @@ -559,7 +560,7 @@ class async_io_manager: task = self.managed_objects[socket_id] if task.done(): - try: + try: # TODO 這邊的錯誤處理要再想想看怎麼做比較好 exc = task.exception() if exc: logger.error(f"Task for mavlink_object {socket_id} raised exception: {exc}") diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index 727e47f..5dc3ca1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,6 +9,8 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' +import os + # ROS2 的 import import std_msgs.msg import sensor_msgs.msg @@ -18,9 +20,9 @@ import mavros_msgs.msg import math # 自定義的 import -from .theLogger import setup_logger +from .utils import setup_logger -logger = setup_logger("mavlinkPublish.py") +logger = setup_logger(os.path.basename(__file__)) class mavlink_publisher(): diff --git a/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py new file mode 100644 index 0000000..921faf4 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py @@ -0,0 +1,7 @@ +""" +共用工具模組 +""" +from .ringBuffer import RingBuffer +from .theLogger import setup_logger + +__all__ = ['RingBuffer', 'setup_logger'] \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py similarity index 100% rename from src/fc_network_adapter/fc_network_adapter/ringBuffer.py rename to src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py diff --git a/src/fc_network_adapter/fc_network_adapter/theLogger.py b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py similarity index 93% rename from src/fc_network_adapter/fc_network_adapter/theLogger.py rename to src/fc_network_adapter/fc_network_adapter/utils/theLogger.py index c4dccfe..dce2ce7 100644 --- a/src/fc_network_adapter/fc_network_adapter/theLogger.py +++ b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py @@ -5,7 +5,7 @@ from logging.handlers import TimedRotatingFileHandler # 全域 Logger 實例 _global_logger = None -def setup_logger(name: str, log_dir: str = "log", level=logging.DEBUG) -> logging.Logger: +def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger: global _global_logger if _global_logger is None: diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py index 33414cb..b28ac96 100644 --- a/src/fc_network_adapter/setup.py +++ b/src/fc_network_adapter/setup.py @@ -20,6 +20,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main', ], }, ) diff --git a/src/fc_network_adapter/tests/devRun.py b/src/fc_network_adapter/tests/demo_integration.py similarity index 98% rename from src/fc_network_adapter/tests/devRun.py rename to src/fc_network_adapter/tests/demo_integration.py index 48f9e2b..b621e73 100644 --- a/src/fc_network_adapter/tests/devRun.py +++ b/src/fc_network_adapter/tests/demo_integration.py @@ -13,12 +13,12 @@ import rclpy from pymavlink import mavutil # 自定義的 import -from fc_network_adapter import mavlinkObject as mo -from fc_network_adapter import mavlinkDevice as md +from ..fc_network_adapter import mavlinkObject as mo +from ..fc_network_adapter import mavlinkDevice as md # ====================== 分割線 ===================== -test_item = 12 +test_item = 10 running_time = 10000 diff --git a/src/fc_network_adapter/tests/demo_ringBuffer.py b/src/fc_network_adapter/tests/demo_ringBuffer.py new file mode 100644 index 0000000..a01ed73 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_ringBuffer.py @@ -0,0 +1,152 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +import time +import threading + +from ..fc_network_adapter.utils import RingBuffer + + +def producer(buffer, count, interval=0.01): + """生產者：向緩衝區添加資料""" + print(f"Producer started (thread {threading.get_ident()})") + for i in range(count): + # 嘗試寫入數據，直到成功 + while not buffer.put(f"Item-{i}"): + print(f"Buffer full, producer waiting... (thread {threading.get_ident()})") + time.sleep(0.1) + + print(f"Produced: Item-{i}, buffer size: {buffer.size()}") + time.sleep(interval) # 模擬生產過程 + + print(f"Producer finished (thread {threading.get_ident()})") + +def consumer(buffer, max_items, interval=0.05): + """消費者：從緩衝區讀取資料""" + print(f"Consumer started (thread {threading.get_ident()})") + items_consumed = 0 + + while items_consumed < max_items: + # 嘗試讀取數據 + item = buffer.get() + if item: + print(f"Consumed: {item}, buffer size: {buffer.size()}") + items_consumed += 1 + else: + print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})") + + time.sleep(interval) # 模擬消費過程 + + print(f"Consumer finished (thread {threading.get_ident()})") + +def batch_consumer(buffer, interval=0.2): + """批量消費者：一次性讀取緩衝區中的所有資料""" + print(f"Batch consumer started (thread {threading.get_ident()})") + + for _ in range(5): # 執行5次批量讀取 + time.sleep(interval) # 等待緩衝區積累數據 + items = buffer.get_all() + if items: + print(f"Batch consumed {len(items)} items: {items}") + else: + print("Buffer empty for batch consumer") + + print(f"Batch consumer finished (thread {threading.get_ident()})") + +def demonstrate_multi_writer(): + """示範多個寫入執行緒同時操作緩衝區""" + print("\n=== Demonstrating Multiple Writers ===") + buffer = RingBuffer(capacity=80) + + # 創建多個生產者執行緒 + threads = [] + for i in range(3): + thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1))) + threads.append(thread) + thread.start() + + # 等待所有執行緒完成 + for thread in threads: + thread.join() + + buffer.print_stats() # 印出統計資訊 + + # 讀出所有剩餘資料 + remaining = buffer.get_all() + print(f"Remaining items in buffer after multiple writers: {remaining}") + +def demonstrate_basic_usage(): + """示範基本使用方式""" + print("\n=== Demonstrating Basic Usage ===") + # 創建緩衝區 + buffer = RingBuffer(capacity=20, buffer_id=7) + + # 檢查初始狀態 + print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 添加幾個項目 + for i in range(5): + buffer.put(f"Test-{i}") + + # 檢查狀態 + print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 讀取一個項目 + item = buffer.get() + print(f"Read item: {item}") + print(f"After reading 1 item - Content Size: {buffer.size()}") + + # 添加更多項目直到滿 + items_added = 0 + while not buffer.is_full(): + buffer.put(f"Fill-{items_added}") + items_added += 1 + + print(f"Added {items_added} more items until full") + print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 嘗試添加到已滿的緩衝區 + result = buffer.put("Overflow") + print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}") + + # 獲取所有項目 + all_items = buffer.get_all() + print(f"All items in buffer: {all_items}") + print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}") + + # 印出統計資訊 + buffer.print_stats() + +def demonstrate_producer_consumer(): + """示範生產者-消費者模式""" + print("\n=== Demonstrating Producer-Consumer Pattern ===") + buffer = RingBuffer(capacity=16) + + # 創建生產者和消費者執行緒 + producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1)) + consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2)) + batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5)) + + # 啟動執行緒 + producer_thread.start() + consumer_thread.start() + batch_thread.start() + + # 等待執行緒完成 + producer_thread.join() + consumer_thread.join() + batch_thread.join() + + # 檢查最終狀態 + print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}") + + buffer.print_stats() + +if __name__ == "__main__": + # 展示各種使用場景 + # demonstrate_basic_usage() + # demonstrate_producer_consumer() + demonstrate_multi_writer() + + print("\nAll demonstrations completed!") \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py index 23907ee..29744fc 100644 --- a/src/fc_network_adapter/tests/test_mavlinkObject.py +++ b/src/fc_network_adapter/tests/test_mavlinkObject.py @@ -16,7 +16,7 @@ import asyncio from unittest.mock import MagicMock, patch # 導入要測試的模組 -from fc_network_adapter.mavlinkObject import ( +from ..fc_network_adapter.mavlinkObject import ( mavlink_object, async_io_manager, MavlinkObjectState, @@ -466,3 +466,4 @@ class TestIntegration(unittest.TestCase): if __name__ == "__main__": unittest.main(defaultTest="TestMavlinkObject.test_send_message") + # unittest.main(defaultTest="TestAsyncIOManager") diff --git a/src/fc_network_adapter/tests/test_ringBuffer.py b/src/fc_network_adapter/tests/test_ringBuffer.py index 563087b..287a057 100644 --- a/src/fc_network_adapter/tests/test_ringBuffer.py +++ b/src/fc_network_adapter/tests/test_ringBuffer.py @@ -1,152 +1,296 @@ -import os -import sys -sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) +#!/usr/bin/env python +""" +測試 RingBuffer 類別的功能 +""" -import time +import unittest import threading +import time +from concurrent.futures import ThreadPoolExecutor -from fc_network_adapter.ringBuffer import RingBuffer - - -def producer(buffer, count, interval=0.01): - """生產者：向緩衝區添加資料""" - print(f"Producer started (thread {threading.get_ident()})") - for i in range(count): - # 嘗試寫入數據，直到成功 - while not buffer.put(f"Item-{i}"): - print(f"Buffer full, producer waiting... (thread {threading.get_ident()})") - time.sleep(0.1) - - print(f"Produced: Item-{i}, buffer size: {buffer.size()}") - time.sleep(interval) # 模擬生產過程 - - print(f"Producer finished (thread {threading.get_ident()})") +from ..fc_network_adapter.utils import RingBuffer -def consumer(buffer, max_items, interval=0.05): - """消費者：從緩衝區讀取資料""" - print(f"Consumer started (thread {threading.get_ident()})") - items_consumed = 0 - - while items_consumed < max_items: - # 嘗試讀取數據 - item = buffer.get() - if item: - print(f"Consumed: {item}, buffer size: {buffer.size()}") - items_consumed += 1 - else: - print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})") - - time.sleep(interval) # 模擬消費過程 - - print(f"Consumer finished (thread {threading.get_ident()})") +class TestRingBuffer(unittest.TestCase): + """測試 RingBuffer 基本功能""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=8) + + def test_initialization(self): + """測試初始化""" + self.assertEqual(self.buffer.capacity, 8) + self.assertTrue(self.buffer.is_empty()) + self.assertFalse(self.buffer.is_full()) + self.assertEqual(self.buffer.size(), 0) + + def test_put_get_basic(self): + """測試基本的放入和取出""" + # 測試放入 + self.assertTrue(self.buffer.put("item1")) + self.assertTrue(self.buffer.put("item2")) + self.assertEqual(self.buffer.size(), 2) + self.assertFalse(self.buffer.is_empty()) + + # 測試取出 + item1 = self.buffer.get() + self.assertEqual(item1, "item1") + self.assertEqual(self.buffer.size(), 1) + + item2 = self.buffer.get() + self.assertEqual(item2, "item2") + self.assertTrue(self.buffer.is_empty()) + + # 空緩衝區取出應返回 None + self.assertIsNone(self.buffer.get()) + + def test_capacity_overflow(self): + """測試緩衝區容量限制""" + # 填滿緩衝區 (容量-1，因為需要預留一個位置) + for i in range(7): # 8-1=7 + self.assertTrue(self.buffer.put(f"item{i}")) + + self.assertTrue(self.buffer.is_full()) + + # 嘗試再放入一個應該失敗 + self.assertFalse(self.buffer.put("overflow")) + self.assertEqual(self.buffer.overflow_count.value, 1) + + def test_get_all(self): + """測試取出所有項目""" + items = ["a", "b", "c", "d"] + for item in items: + self.buffer.put(item) + + all_items = self.buffer.get_all() + self.assertEqual(all_items, items) + self.assertTrue(self.buffer.is_empty()) + + def test_clear(self): + """測試清空緩衝區""" + for i in range(5): + self.buffer.put(f"item{i}") + + self.buffer.clear() + self.assertTrue(self.buffer.is_empty()) + self.assertEqual(self.buffer.size(), 0) -def batch_consumer(buffer, interval=0.2): - """批量消費者：一次性讀取緩衝區中的所有資料""" - print(f"Batch consumer started (thread {threading.get_ident()})") - - for _ in range(5): # 執行5次批量讀取 - time.sleep(interval) # 等待緩衝區積累數據 - items = buffer.get_all() - if items: - print(f"Batch consumed {len(items)} items: {items}") - else: - print("Buffer empty for batch consumer") - - print(f"Batch consumer finished (thread {threading.get_ident()})") -def demonstrate_multi_writer(): - """示範多個寫入執行緒同時操作緩衝區""" - print("\n=== Demonstrating Multiple Writers ===") - buffer = RingBuffer(capacity=80) - - # 創建多個生產者執行緒 - threads = [] - for i in range(3): - thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1))) - threads.append(thread) - thread.start() - - # 等待所有執行緒完成 - for thread in threads: - thread.join() +class TestRingBufferThreadSafety(unittest.TestCase): + """測試 RingBuffer 的線程安全性""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=256) + self.results = [] + self.write_count = 1000 + self.read_count = 1000 + + def test_concurrent_producers_consumers(self): + """測試多生產者多消費者場景""" + self.results = [] + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + + def producer(producer_id, count): + """生產者函數""" + for i in range(count): + item = f"producer_{producer_id}_item_{i}" + while not self.buffer.put(item): + time.sleep(0.001) # 緩衝區滿時稍微等待 + + def consumer(consumer_id, count): + """消費者函數""" + items = [] + for _ in range(count): + item = None + while item is None: + item = self.buffer.get() + if item is None: + time.sleep(0.001) # 緩衝區空時稍微等待 + items.append(item) + self.results.extend(items) + + # 創建多個生產者和消費者 + with ThreadPoolExecutor(max_workers=8) as executor: + # 2 個生產者，每個寫入 500 個項目 + producer_futures = [ + executor.submit(producer, 0, 500), + executor.submit(producer, 1, 500) + ] + + # 2 個消費者，每個讀取 500 個項目 + consumer_futures = [ + executor.submit(consumer, 0, 500), + executor.submit(consumer, 1, 500) + ] + + # 等待所有任務完成 + for future in producer_futures + consumer_futures: + future.result() + + # 驗證結果 + self.assertEqual(len(self.results), 1000) + self.assertTrue(self.buffer.is_empty()) + + # 檢查統計數據 + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 1000) + self.assertGreater(stats['total_reads'], 1000) # 包含失敗的讀取嘗試 + self.assertGreater(stats['write_threads'], 1) + self.assertGreater(stats['read_threads'], 1) + + def test_high_throughput(self): + """測試高吞吐量場景""" + items_per_thread = 10000 + num_threads = 4 + + def writer(): + for i in range(items_per_thread): + while not self.buffer.put(i): + pass # 忙等待 + + def reader(): + items = [] + for _ in range(items_per_thread): + item = None + while item is None: + item = self.buffer.get() + items.append(item) + self.results.extend(items) + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=num_threads * 2) as executor: + # 啟動寫入線程 + writer_futures = [executor.submit(writer) for _ in range(num_threads)] + + # 啟動讀取線程 + reader_futures = [executor.submit(reader) for _ in range(num_threads)] + + # 等待完成 + for future in writer_futures + reader_futures: + future.result() + + end_time = time.time() + + # 驗證結果 + total_items = items_per_thread * num_threads + self.assertEqual(len(self.results), total_items) + + # 性能統計 + duration = end_time - start_time + throughput = total_items / duration + + print(f"\nHigh Throughput Test Results:") + print(f"Total items: {total_items}") + print(f"Duration: {duration:.2f}s") + print(f"Throughput: {throughput:.0f} items/sec") + + # 顯示詳細統計 + self.buffer.print_stats() - buffer.print_stats() # 印出統計資訊 - # 讀出所有剩餘資料 - remaining = buffer.get_all() - print(f"Remaining items in buffer after multiple writers: {remaining}") +class TestRingBufferStatistics(unittest.TestCase): + """測試 RingBuffer 的統計功能""" + + def test_statistics_tracking(self): + """測試統計數據追蹤""" + buffer = RingBuffer(capacity=16) + + # 寫入一些數據 + for i in range(10): + buffer.put(f"item{i}") + + # 讀取一些數據 + for _ in range(5): + buffer.get() + + stats = buffer.get_stats() + + # 驗證基本統計 + self.assertEqual(stats['total_writes'], 10) + self.assertEqual(stats['total_reads'], 5) + self.assertEqual(stats['current_size'], 5) + self.assertEqual(stats['write_threads'], 1) + self.assertEqual(stats['read_threads'], 1) + + def test_reset_statistics(self): + """測試重置統計數據""" + buffer = RingBuffer(capacity=16) + + # 產生一些活動 + for i in range(5): + buffer.put(f"item{i}") + for _ in range(3): + buffer.get() + + # 重置統計 + buffer.reset_stats() + + stats = buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + self.assertEqual(stats['total_reads'], 0) + self.assertEqual(stats['concurrent_writes'], 0) + self.assertEqual(stats['concurrent_reads'], 0) + self.assertEqual(stats['overflow_count'], 0) -def demonstrate_basic_usage(): - """示範基本使用方式""" - print("\n=== Demonstrating Basic Usage ===") - # 創建緩衝區 - buffer = RingBuffer(capacity=20, buffer_id=7) - - # 檢查初始狀態 - print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") - - # 添加幾個項目 - for i in range(5): - buffer.put(f"Test-{i}") - - # 檢查狀態 - print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") - - # 讀取一個項目 - item = buffer.get() - print(f"Read item: {item}") - print(f"After reading 1 item - Content Size: {buffer.size()}") - - # 添加更多項目直到滿 - items_added = 0 - while not buffer.is_full(): - buffer.put(f"Fill-{items_added}") - items_added += 1 - - print(f"Added {items_added} more items until full") - print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") - - # 嘗試添加到已滿的緩衝區 - result = buffer.put("Overflow") - print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}") - - # 獲取所有項目 - all_items = buffer.get_all() - print(f"All items in buffer: {all_items}") - print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}") - # 印出統計資訊 +def benchmark_ringbuffer(): + """RingBuffer 性能基準測試""" + print("\n=== RingBuffer Performance Benchmark ===") + + buffer = RingBuffer(capacity=1024) + num_operations = 100000 + + # 單線程性能測試 + start_time = time.time() + for i in range(num_operations): + buffer.put(i) + for _ in range(num_operations): + buffer.get() + end_time = time.time() + + single_thread_time = end_time - start_time + throughput = (num_operations * 2) / single_thread_time + + print(f"Single Thread: {throughput:.0f} ops/sec") + + # 多線程性能測試 + buffer = RingBuffer(capacity=1024) + + def producer(): + for i in range(num_operations // 2): + while not buffer.put(i): + pass + + def consumer(): + for _ in range(num_operations // 2): + while buffer.get() is None: + pass + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=2) as executor: + future1 = executor.submit(producer) + future2 = executor.submit(consumer) + future1.result() + future2.result() + + end_time = time.time() + + multi_thread_time = end_time - start_time + throughput = num_operations / multi_thread_time + + print(f"Multi Thread: {throughput:.0f} ops/sec") + print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x") + buffer.print_stats() -def demonstrate_producer_consumer(): - """示範生產者-消費者模式""" - print("\n=== Demonstrating Producer-Consumer Pattern ===") - buffer = RingBuffer(capacity=16) - - # 創建生產者和消費者執行緒 - producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1)) - consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2)) - batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5)) - - # 啟動執行緒 - producer_thread.start() - consumer_thread.start() - batch_thread.start() - - # 等待執行緒完成 - producer_thread.join() - consumer_thread.join() - batch_thread.join() - - # 檢查最終狀態 - print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}") - - buffer.print_stats() if __name__ == "__main__": - # 展示各種使用場景 - # demonstrate_basic_usage() - # demonstrate_producer_consumer() - demonstrate_multi_writer() + # 運行單元測試 + unittest.main(argv=[''], exit=False, verbosity=2) - print("\nAll demonstrations completed!") \ No newline at end of file + # 運行性能基準測試 + benchmark_ringbuffer() \ No newline at end of file From 6a71e4530f2d4b3f780880de851a4cf222fb49f2 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 1 Dec 2025 14:53:55 +0800 Subject: [PATCH 077/146] =?UTF-8?q?1.=20=E5=A4=A7=E9=87=8F=E6=B7=BB?= =?UTF-8?q?=E5=A2=9E=E7=B5=82=E7=AB=AF=E6=A9=9F=E4=BB=8B=E9=9D=A2=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=202.=20=E5=84=AA=E5=8C=96=20mavlink=5Fobject=20?= =?UTF-8?q?=E8=88=87=20manager=20=E7=9A=84=E6=B5=81=E7=A8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + .../fc_network_adapter/mainOrchestrator.py | 752 ++++++++++++--- .../fc_network_adapter/mavlinkObject.py | 889 ++++++++++-------- .../fc_network_adapter/mavlinkVehicleView.py | 435 +++++++++ .../tests/demo_integration.py | 256 +---- .../tests/demo_mavlinkVehicleView.py | 331 +++++++ .../tests/test_mavlinkObject.py | 507 +++++----- 7 files changed, 2209 insertions(+), 962 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py create mode 100644 src/fc_network_adapter/tests/demo_mavlinkVehicleView.py diff --git a/README.md b/README.md index 8e81de0..490d84b 100644 --- a/README.md +++ b/README.md @@ -44,4 +44,5 @@ N. logs 是執行時期的記錄檔 例如 在 ~/AirTrapMine/src/ 資料夾下 > python -m fc_network_adapter.fc_network_adapter.mainOrchestrator > python -m fc_network_adapter.tests.test_ringBuffer +> python -m fc_network_adapter.tests.demo_integration diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 805f1eb..f15bad6 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -7,7 +7,6 @@ ''' - import os import time import sys @@ -17,13 +16,45 @@ import threading import queue import signal +from pymavlink import mavutil + # 自定義的 import from . import mavlinkObject as mo -from . import mavlinkDevice as md from .utils import RingBuffer, setup_logger + + logger = setup_logger(os.path.basename(__file__)) +class PanelState: + def __init__(self): + self.panel_status = "Idle" + termination_start_time = None + self.mavlink_bridge_state = "Stopped" + self.object_manager_state = "Stopped" + self.socket_object_list = [] + self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 + + # 這邊是儲存關於 socket object 的資料 + self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 + self.socket_info_single = {"socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", + "target_sockets": "", "primary_socket_id": "", "InfoReady": False} # 暫存單一 socket 的資訊 + + def intoSTART(self): + self.panel_status = "Running" + + def intoTERMINATION(self): + self.termination_start_time = time.time() + self.panel_status = "Terminating" + + def intoENGINEER(self): + self.panel_status = "Engineer" + + def intoSTOPPED(self): + self.panel_status = "Stopped" + + def set_user_input(self, text): + self.user_input = text class MenuNode: def __init__(self, name, desc="", action=None, children=None): @@ -36,33 +67,233 @@ class ControlPanel: def __init__(self): pass + def input_dialog(stdscr, prompt="請輸入文字: "): + """顯示輸入對話框""" + height, width = stdscr.getmaxyx() + + # 建立輸入視窗 + dialog_height = 5 + dialog_width = min(60, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + # 建立視窗邊框 + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(1, 2, prompt) + dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消") + dialog_win.refresh() + + # 輸入區域 + input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2) + input_win.keypad(True) + + curses.echo() + curses.curs_set(1) + + user_input = "" + + while True: + input_win.clear() + input_win.addstr(0, 0, user_input[-dialog_width+8:]) # 顯示輸入內容（滾動） + input_win.refresh() + + ch = input_win.getch() + + if ch == 27: # ESC + user_input = None + break + elif ch in (curses.KEY_ENTER, 10, 13): # Enter + break + elif ch in (curses.KEY_BACKSPACE, 127, 8): # Backspace + user_input = user_input[:-1] + elif 32 <= ch <= 126: # 可打印字符 + user_input += chr(ch) + + curses.noecho() + curses.curs_set(0) + + # 清理視窗 + del input_win + del dialog_win + stdscr.clear() + stdscr.refresh() + + return user_input + + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=5): + """動態創建 mavlink_object 列表選單（支持分頁）""" + children = [] + + if not state.socket_object_list: + children.append(MenuNode("(空)", "目前沒有連結口", None)) + else: + total_items = len(state.socket_object_list) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + for socket_id in state.socket_object_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), + MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), + MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), + MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), + MenuNode("Remove Target", "移除轉發目標(工程)", "MAVOBJ_REMOVE_TARGET"), + MenuNode("返回", "回到列表", "BACK"), + ]) + # 將 socket_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.socket_id = socket_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", "---", None)) + if page > 0: + prev_node = MenuNode("◀ 上一頁", f"第 {page}/{total_pages} 頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("下一頁 ▶", f"第 {page + 2}/{total_pages} 頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("返回", "回到上層選單", "BACK")) + menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_object_info(self, stdscr, socket_id, state: PanelState): + """顯示物件詳細資訊的對話框""" + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) + + while not state.socket_info_single.get('InfoReady', False): + time.sleep(0.05) # 等待資訊準備好 + + # 這裡顯示基本資訊 + dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") + dialog_win.addstr(3, 2, f"Socket status : 運行中") + # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) + dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") + show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) + dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") + show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) + dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}") + dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}") + show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', ''))) + dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}") + + state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False): + """選擇目標 socket 的對話框""" + height, width = stdscr.getmaxyx() + dialog_height = min(15, len(state.socket_object_list) + 5) + dialog_width = min(50, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.keypad(True) + + title = "選擇要移除的目標" if remove_mode else "選擇轉發目標" + available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id] + + if not available_sockets: + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + dialog_win.addstr(2, 2, "沒有可用的目標") + dialog_win.addstr(4, 2, "按任意鍵返回...") + dialog_win.refresh() + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + selected_idx = 0 + + while True: + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + + for i, socket_id in enumerate(available_sockets): + marker = "➤" if i == selected_idx else " " + attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL + dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr) + + dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消") + dialog_win.refresh() + + ch = dialog_win.getch() + + if ch in (curses.KEY_UP, ord('k')): + selected_idx = (selected_idx - 1) % len(available_sockets) + elif ch in (curses.KEY_DOWN, ord('j')): + selected_idx = (selected_idx + 1) % len(available_sockets) + elif ch in (curses.KEY_ENTER, 10, 13): + result = available_sockets[selected_idx] + del dialog_win + stdscr.clear() + stdscr.refresh() + return result + elif ch == 27: # ESC + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + def menu_tree(self): """建立多層選單結構""" return MenuNode("Main Menu", children=[ - MenuNode("MavLink Object Control", children=[ - MenuNode("New+", "新增一個連結口", "ADD_MAV_OBJECT"), - MenuNode("Remove", "移除某個連結口", "REMOVE_MAV_OBJECT"), - MenuNode("ListAll", "顯示連結口列表", "LIST_MAV_OBJECT"), - MenuNode("Inspection", "顯示連結口資訊", "INSPECT_MAV_OBJECT"), - ]), - MenuNode("參數設定", children=[ - MenuNode("Speed", children=[ - MenuNode("Increase Speed", "增加速度", "ADJUST_SPEED"), - MenuNode("Decrease Speed", "減少速度", "ADJUST_SPEED"), + MenuNode("MavLink Object", "控制 MavLink 物件", children=[ + MenuNode("New+", children=[ + MenuNode("UDP InBound", children=[ + MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"), + ]), + MenuNode("UDP OutBound", children=[ + MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), + ]), + ]), + MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), + ]), + MenuNode("Engineer Mode", "工程模式", children=[ + MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), #TODO: 尚未實作 + MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), #TODO: 尚未實作 + ]), + MenuNode("Shutdown", "關閉整個系統", children=[ + MenuNode("Return", "繼續運行", "BACK"), + MenuNode("Confirm", "關閉系統", "QUIT"), ]), - MenuNode("輸入文字", "鍵盤輸入模式", "INPUT_TEXT"), - ]), - MenuNode("資訊查看", children=[ - MenuNode("顯示狀態", "查看當前狀態", "SHOW_STATUS"), - MenuNode("顯示輸入", "查看用戶輸入", "SHOW_INPUT"), - ]), - MenuNode("返回上層", "回到上一層選單", "BACK"), - MenuNode("關閉面板", "關閉控制面板", "QUIT"), ]) - def panel_thread(self, cmd_q: queue.Queue, stop_evt: threading.Event): + def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event): stdscr = None - + def cleanup(): """清理 curses 狀態""" if stdscr: @@ -70,7 +301,13 @@ class ControlPanel: curses.nocbreak() curses.echo() curses.endwin() - + + def panel_shutdown(): + # 先關閉所有模組 再關閉面板 + cmd_q.put("SHUTDOWN_BRIDGE") + cmd_q.put("SHUTDOWN_MANAGER") + + def draw_menu(screen): nonlocal stdscr stdscr = screen @@ -82,6 +319,8 @@ class ControlPanel: # 選單導航狀態 menu_stack = [self.menu_tree()] # 選單堆疊 idx_stack = [0] # 索引堆疊 + + state.intoSTART() # 設定狀態為運行中 while not stop_evt.is_set(): # 檢查是否需要退出 @@ -91,35 +330,92 @@ class ControlPanel: current_menu = menu_stack[-1] current_idx = idx_stack[-1] + # 獲取終端機尺寸 + height, width = stdscr.getmaxyx() + # 簡單暴力的限制視窗的大小 + if height < 20 or width < 60: + logger.error("Terminal size too small for Control Panel.") + break + stdscr.clear() stdscr.border() stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) - stdscr.addstr(1, 2, f"mavlink bridge state : ") - stdscr.addstr(2, 2, f"object manager state : ") - stdscr.addstr(3, 2, f"Node state : ") + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"mavlink Bridge State : {state.mavlink_bridge_state}") + stdscr.addstr(3, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") - # # Header - 顯示選單路徑 - # path = " → ".join([menu.name for menu in menu_stack]) - # stdscr.addstr(0, 2, f"控制面板: {path}", curses.A_BOLD) - # stdscr.addstr(1, 2, f"狀態: {state.status} | 速度: {state.speed} | 計數: {state.counter}") - # if state.user_input: - # stdscr.addstr(2, 2, f"輸入: {state.user_input[:50]}...") + # # 更新模組狀態顯示 + # stdscr.addstr(2, 25, f"{state.mavlink_bridge_state}") + # stdscr.addstr(3, 25, f"{state.object_manager_state}") + # stdscr.addstr(4, 25, f"{len(state.socket_object_list)} ") # 顯示當前選單項目 - start_line = 5 + start_line = 6 for i, child in enumerate(current_menu.children): marker = "➤ " if i == current_idx else " " - line = f"{marker}{child.name:15s} – {child.desc}" + # 動態顯示已輸入的值 + desc = child.desc + if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]: + desc = f"{child.desc} [{state.udp_info_temp['IP']}]" + elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: + desc = f"{child.desc} [{state.udp_info_temp['Port']}]" + + line = f"{marker}{child.name:15s} – {desc}" attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL stdscr.addstr(start_line + i, 4, line, attr) + # 顯示訊息區域 + # info_start_line = start_line + len(current_menu.children) + 1 + info_start_line = height - 8 + max_msg_lines = 5 # 最多顯示 5 行訊息 + current_time = time.time() + + # 清理過時的訊息 + state.panel_info_msg_list = [ + (msg, timestamp) for msg, timestamp in state.panel_info_msg_list + if current_time - timestamp < 2.0 #秒數 + ] + + # 只顯示最新的 max_msg_lines 條訊息 + display_msgs = state.panel_info_msg_list[-max_msg_lines:] + + for i, msg_data in enumerate(display_msgs): + if info_start_line + i >= help_line - 1: # 避免超出邊界 + break + msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data + # 截斷過長的訊息 + max_msg_width = width - 6 + if len(msg) > max_msg_width: + msg = msg[:max_msg_width-3] + "..." + + stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD) + + + # 操作說明 - help_line = start_line + len(current_menu.children) + 2 - stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 ←→調參數 q退出", curses.A_DIM) + # help_line = start_line + len(current_menu.children) + 2 + help_line = height - 2 + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層 q退出", curses.A_DIM) stdscr.refresh() + # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 + # 驗證 bridge 跟 manager 狀態 兩者都停止後 就進入 STOPPED 狀態並跳出迴圈 + # 超過幾秒沒有反應就強制關閉 + if state.panel_status == "Terminating": + if time.time() - state.termination_start_time > 3: + logger.warning("Control Panel forced shutdown after timeout.") + state.intoSTOPPED() + stop_evt.set() + continue + time.sleep(0.1) + if state.mavlink_bridge_state == "Stopped" and state.object_manager_state == "Stopped": + state.intoSTOPPED() + stop_evt.set() + continue + # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt stdscr.timeout(100) # 100ms timeout ch = stdscr.getch() @@ -133,24 +429,28 @@ class ControlPanel: elif ch in (curses.KEY_DOWN, ord('j')): idx_stack[-1] = (current_idx + 1) % len(current_menu.children) + + elif ch == (ord('O')): + # 直接進入工程模式 + state.intoENGINEER() elif ch == curses.KEY_LEFT: - # 返回上層或調整參數 + # 返回上層 if len(menu_stack) > 1: menu_stack.pop() idx_stack.pop() - else: - # 在根選單，檢查是否為調整參數 - selected = current_menu.children[current_idx] - if selected.action == "ADJUST_SPEED": - state.speed = max(1, state.speed - 1) - + elif ch == curses.KEY_RIGHT: - # 調整參數 + # 進入下層 (但不執行動作) selected = current_menu.children[current_idx] - if selected.action == "ADJUST_SPEED": - state.speed = min(10, state.speed + 1) - + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif ch in (ord('q'), 27): + state.intoTERMINATION() + panel_shutdown() + elif ch in (curses.KEY_ENTER, 10, 13): selected = current_menu.children[current_idx] @@ -165,96 +465,320 @@ class ControlPanel: idx_stack.pop() elif selected.action == "QUIT": - break - - elif selected.action == "INPUT_TEXT": - # 進入輸入模式 - result = input_dialog(stdscr, "請輸入文字: ") + state.intoTERMINATION() + panel_shutdown() + + elif selected.action == "TEXT_UDP_IP": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ") if result is not None: - cmd_q.put(lambda: state.set_user_input(result)) - - elif selected.action == "SHOW_STATUS": - # 顯示狀態訊息 - stdscr.clear() - stdscr.addstr(5, 2, f"當前狀態: {state.status}") - stdscr.addstr(6, 2, f"速度設定: {state.speed}") - stdscr.addstr(7, 2, f"計數器: {state.counter}") - stdscr.addstr(9, 2, "按任意鍵返回...") - stdscr.refresh() - stdscr.getch() - - elif selected.action == "SHOW_INPUT": - # 顯示用戶輸入 - stdscr.clear() - stdscr.addstr(5, 2, f"用戶輸入內容:") - stdscr.addstr(6, 2, f"{state.user_input}") - stdscr.addstr(8, 2, "按任意鍵返回...") - stdscr.refresh() - stdscr.getch() - + state.udp_info_temp["IP"] = result + + elif selected.action == "TEXT_UDP_PORT": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ") + if result is not None: + state.udp_info_temp["Port"] = result + + elif selected.action == "CREATE_UDP_INBOUND": + cmd_q.put("CREATE_UDP_INBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "CREATE_UDP_OUTBOUND": + cmd_q.put("CREATE_UDP_OUTBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_MAV_OBJECT": + # 動態生成 mavlink_object 列表選單 + object_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(object_list_menu) + idx_stack.append(0) + + elif selected.action == "PREV_PAGE": + # 上一頁 + if hasattr(selected, 'page'): + menu_stack.pop() + idx_stack.pop() + object_list_menu = self.create_object_list_menu(state, page=selected.page) + menu_stack.append(object_list_menu) + idx_stack.append(0) + + elif selected.action == "NEXT_PAGE": + # 下一頁 + if hasattr(selected, 'page'): + menu_stack.pop() + idx_stack.pop() + object_list_menu = self.create_object_list_menu(state, page=selected.page) + menu_stack.append(object_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_MAV_OBJECT": + # 顯示物件詳細資訊 + if hasattr(selected, 'socket_id'): + cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id)) + self.show_object_info(stdscr, selected.socket_id, state) + + elif selected.action == "REMOVE_MAV_OBJECT": + # 移除物件 + if hasattr(selected, 'socket_id'): + cmd_q.put(("REMOVE_OBJECT", selected.socket_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 + menu_stack.pop() + idx_stack.pop() + # # 刷新列表頁面 + # if len(menu_stack) > 1: + # current_page = menu_stack[-1].current_page if hasattr(menu_stack[-1], 'current_page') else 0 + # menu_stack.pop() + # idx_stack.pop() + # time.sleep(0.1) # 等待物件被移除 + # object_list_menu = self.create_object_list_menu(state, page=current_page) + # menu_stack.append(object_list_menu) + # idx_stack.append(0) + + elif selected.action == "MAVOBJ_MAKE_LINK": + # 建立轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_MAKE_LINK", selected.socket_id, target_id)) + + elif selected.action == "MAVOBJ_ADD_TARGET": + # 添加目標端口 + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + + elif selected.action == "MAVOBJ_REMOVE_TARGET": + # 移除目標端口 + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) + if target_id is not None: + cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) + + elif selected.action == "STOP_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_MANAGER") + + elif selected.action == "STOP_BRIDGE": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_BRIDGE") elif callable(selected.action): # 執行函式 cmd_q.put(selected.action) - elif ch in (ord('q'), 27): - break - try: curses.wrapper(draw_menu) except KeyboardInterrupt: pass finally: + stop_evt.set() cleanup() + + +class Orchestrator: + def __init__(self, stop_sig): + self.stop_evt = stop_sig + + # === 1) 面板部分的準備 === + self.cmd_q = queue.Queue() + self.panelState = PanelState() # 面板的狀態儲存 + self.cPanel = ControlPanel() # 面板的功能 + self.panel_thread = None + + # === 2) async_io_manager & mavlink_bridge 部分的準備 === + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + self.manager = mo.async_io_manager() + self.bridge = mo.mavlink_bridge() + + def engageWholeSystem(self): + """啟動整個系統""" + # === 1) 面板部分的啟動 === + self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) + self.panel_thread.start() + + # === 2) async_io_manager & mavlink_bridge 部分的啟動 === + self.manager.start() + self.bridge.start() + + def mainLoop(self): + logger.info("Main orchestrator started <-") + try: + while not self.stop_evt.is_set(): + + # A. 更新模組狀態 + if self.manager.running: + self.panelState.object_manager_state = 'Running' + else: + self.panelState.object_manager_state = 'Stopped' + + socketid_list = self.manager.get_managed_objects() + self.panelState.socket_object_list = socketid_list + + if self.bridge.running: + self.panelState.mavlink_bridge_state = 'Running' + else: + self.panelState.mavlink_bridge_state = 'Stopped' + + # 取出面板丟過來的「動作」 + try: + cmd = self.cmd_q.get_nowait() + if callable(cmd): + cmd() # 執行對應動作 + elif isinstance(cmd, tuple): + # 處理多參數命令 + action = cmd[0] + if action == "REMOVE_OBJECT": + socket_id = cmd[1] + # 先移除所有跟他關聯的 target sockets + for s_id in mo.mavlink_object.mavlinkObjects: + s_obj = mo.mavlink_object.mavlinkObjects[s_id] + if socket_id in s_obj.target_sockets: + self.remove_target_from_object(s_id, socket_id) + # 再移除該物件 + self.delete_mavlink_object(socket_id) + elif action == "MAVOBJ_MAKE_LINK": + source_id, target_id = cmd[1], cmd[2] + self.add_target_to_object(source_id, target_id) + self.add_target_to_object(target_id, source_id) # 雙向連結 + elif action == "MAVOBJ_ADD_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.add_target_to_object(source_id, target_id) + elif action == "MAVOBJ_REMOVE_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.remove_target_from_object(source_id, target_id) + elif action == "INSPECT_MAV_OBJECT": + socket_id = cmd[1] + mav_obj = self.manager.managed_objects.get(socket_id, None) + if mav_obj: + self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type + # self.panelState.socket_info_single["socket_state"] = "Running" if mav_obj.running + self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types + self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types + self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id + self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets + self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 + + elif cmd == "CREATE_UDP_INBOUND": + self.panelState.udp_info_temp["direction"] = "inbound" + self.create_udp_object() + elif cmd == "CREATE_UDP_OUTBOUND": + self.panelState.udp_info_temp["direction"] = "outbound" + self.create_udp_object() + elif cmd == "SHUTDOWN_BRIDGE": + self.bridge.stop() + elif cmd == "SHUTDOWN_MANAGER": + self.manager.shutdown() + except queue.Empty: + pass + except Exception as e: + logger.error(f"Error processing command: {e}") + + + time.sleep(0.1) + + except KeyboardInterrupt: + pass + except Exception as e: + logger.error(f"Unexpected error in main loop: {e}") + finally: + logger.info("Main orchestrator END!") + + # 關閉 mavlink_bridge (裡面有一個執行緒) + self.bridge.stop() + + # 關閉 async_io_manager (裡面有一個執行緒) + self.manager.shutdown() + + # 關閉面板執行緒 + if self.panel_thread.is_alive(): + self.panel_thread.join(timeout=2) + + def create_udp_object(self): + if self.panelState.udp_info_temp["direction"] == "inbound": + connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + elif self.panelState.udp_info_temp["direction"] == "outbound": + connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + + try: + mavlink_socket = mavutil.mavlink_connection(connection_string) + except Exception as e: + self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) + return + mavlink_object = mo.mavlink_object(mavlink_socket) + mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + self.manager.add_mavlink_object(mavlink_object) + self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) + + def delete_mavlink_object(self, socket_id): + """移除指定的 mavlink_object""" + self.manager.remove_mavlink_object(socket_id) + + def add_target_to_object(self, source_id, target_id): + """為 mavlink_object 添加轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.add_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Added target {target_id} to socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Adding target {target_id} to socket {source_id}", time.time())) + + def remove_target_from_object(self, source_id, target_id): + """從 mavlink_object 移除轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.remove_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Removed target {target_id} from socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + def main(): - logger.warning(f"Hello this is mainOrchestrator.py") - pp = ControlPanel() - cmd_q = queue.Queue() stop_evt = threading.Event() - panel_thread_obj = None def signal_handler(signum, frame): - """處理 Ctrl+C 信號""" - print("\n收到中斷信號，正在安全退出...") + """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組""" stop_evt.set() - if panel_thread_obj and panel_thread_obj.is_alive(): - panel_thread_obj.join(timeout=2) - sys.exit(0) # 註冊信號處理器 signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) - # 啟動控制面板（改為非 daemon） - panel_thread_obj = threading.Thread(target=pp.panel_thread, args=(cmd_q, stop_evt)) - panel_thread_obj.start() - - print("多層選單控制面板啟動。Ctrl+C 結束程式。") - - try: - while not stop_evt.is_set(): - # 取出面板丟過來的「動作」 - try: - fn = cmd_q.get_nowait() - fn() # 執行對應動作 - except queue.Empty: - pass - - # # 模擬你的長跑邏輯 - # if state.status == "running": - # # 依 speed 前進 - # state.counter += state.speed - - time.sleep(0.1) - except KeyboardInterrupt: - print("\n收到 Ctrl+C，準備結束...") - finally: - stop_evt.set() - if panel_thread_obj.is_alive(): - panel_thread_obj.join(timeout=2) - pass - + orchestrator = Orchestrator(stop_evt) + orchestrator.engageWholeSystem() + orchestrator.mainLoop() # 程式會 block 在這邊 直到收到中斷信號 if __name__ == "__main__": main() diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index a132e97..b4d747f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -8,26 +8,57 @@ # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 # 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function +================= 改版記錄 ============================ + +2025年 6月 20日 +1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 + 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 + 而 multiplexingToSwap 會完全被移除代替方式下一條描述 +2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 + (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) + 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 + 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 +3. mavlink_object 需要加上 state 去管理其狀態 +4. mavlink_object 需要加上 target port 去管理寫出的目標 +5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 +6. 基於第1,2項 updateMultiplexingList 會被完全移除 +7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object +8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 + + +2025年 11月 15日 +1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行 +2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式) +3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 +4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 +5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 +6. mavlink_object 優化 send_message 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +7. 移除迴圈內的 try except 堆疊 增加效能 +8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 + ''' # 基礎功能的 import -import threading -import os -# import queue +import os +import signal import time +import threading import asyncio from enum import Enum, auto +from collections import deque # from typing import Dict, List, Optional, Set, Any, Tuple # mavlink 的 import from pymavlink import mavutil -# ROS2 的 import -from rclpy.node import Node - # 自定義的 import -from .mavlinkDevice import mavlink_device, mavlink_systems -from .mavlinkPublish import mavlink_publisher +from .mavlinkVehicleView import ( + vehicle_registry, + VehicleView, + VehicleComponent, + ComponentType, + ConnectionType +) from .utils import RingBuffer, setup_logger @@ -40,26 +71,24 @@ return_packet_ring = RingBuffer(capacity=256, buffer_id=254) # ====================== 分割線 ===================== -class mavlink_bridge(Node, mavlink_publisher): +# 使用 vehicle_registry 來管理所有的載具視圖 +# vehicle_registry 是從 mavlinkVehicleView 導入的全域實例 + +class mavlink_bridge: ''' 這個 class 就是 固定串流橋接器 - 是用來接收 mavlink 訊息 並進行橋接 - 這個地方是針對 fixed_stream_bridge_queue 的資料做處理的 + 是用來接收 mavlink 訊息 並進行橋接處理 + 這個地方是針對 stream_bridge_ring 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object - 並且看是否有重複 system id - - 整段代碼包含兩大區塊 thread 和 node - - thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理 - 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object - - node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 - 藉由控制 ros2 的機制再把 device object 的資訊發送出去 - - fixed_stream_bridge_queue 置換成 stream_bridge_ring - - ps. 我限制了這個 class 只能有一個 instance + + 此類別負責： + 1. 從 stream_bridge_ring 接收訊息 + 2. 管理 mavlink_systems（device 和 component objects） + 3. 處理接收到的訊息並更新對應的 component 狀態 + 4. 提供發送訊息的介面，將訊息路由到正確的 mavlink_object + + ps. 此 class 為 singleton，只能有一個 instance ''' _instance = None _lock = threading.Lock() # 確保多線程安全 @@ -73,154 +102,244 @@ class mavlink_bridge(Node, mavlink_publisher): def __init__(self): if not hasattr(self, "initialized"): # 防止重複初始化 self.initialized = True + self.thread = None + self.running = False - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems + # 初始化訊息處理器字典 (msg_id -> handler_function) + self._init_message_handlers() + else: + logger.error('mavlink_bridge instance already exists. Do not create another one.') - # 當 object 建立時會直接運行 thread 直到消滅 + def _init_message_handlers(self): + """初始化訊息處理器映射表，提高處理效率""" + self.message_handlers = { + 0: self._handle_heartbeat, # HEARTBEAT + 30: self._handle_attitude, # ATTITUDE + 32: self._handle_local_position, # LOCAL_POSITION_NED + 33: self._handle_global_position, # GLOBAL_POSITION_INT + 74: self._handle_vfr_hud, # VFR_HUD + 147: self._handle_battery_status, # BATTERY_STATUS + } + + def start(self): + """啟動 mavlink_bridge 的運作""" + if not self.running: self.running = True - self.thread = threading.Thread(target=self._run_thread) + self.thread = threading.Thread(target=self._run_thread, name="MavlinkBridge") self.thread.start() else: - logger.error('mavlink_bridge instance already exists. Do not create another one.') + logger.warning("mavlink_bridge is already running") def stop(self): + """停止 mavlink_bridge 的運作""" self.running = False + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=3.0) # === Thread 區塊 === def _run_thread(self): - start_time = time.time() # debug - run_loop_count = 0 # debug - logger.info('Start of mavlink_bridge._run_thread') - # 從 Queue stream_bridge_ring 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 + """主執行緒：從 stream_bridge_ring 接收並處理 mavlink 訊息""" + logger.info('mavlink_bridge started <-') + while self.running: - # # 這個迴圈每秒鐘被執行了幾輪? # 這整段都是 debug 用的 - # current_time = time.time() - # if (current_time - start_time) >= 1.0: - # logger.info(f'mavlink_bridge._run_thread loop count: {run_loop_count}') - # run_loop_count = 0 - # start_time = current_time - # else: - # run_loop_count += 1 - # # ========================= - - + # 檢查是否有訊息 if stream_bridge_ring.is_empty(): + time.sleep(0.001) # 避免忙碌等待 continue + + # 取出訊息包：(socket_id, timestamp, mavlink_msg) msg_pack = stream_bridge_ring.get() - - msg = msg_pack[2] + socket_id, timestamp, msg = msg_pack[0], msg_pack[1], 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: - this_device = mavlink_device() # 創建一個新的 device object - self.mavlink_systems[sysid] = this_device - this_device.socket_id = msg_pack[0] - this_device.sysid = sysid - else: - this_device = self.mavlink_systems[sysid] - - # 若該 component id 存在 則直接使用該 component object - # 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object - # 若該 component id 不存在 又不是 heartbeat 則不處理 - if compid in self.mavlink_systems[sysid].components: - this_component = self.mavlink_systems[sysid].components[compid] - elif msg_id == 0: - # 只有透過 heartbeat 可以創建一個新的 component object - this_component = this_device.mavlink_component() - this_device.components[msg.get_srcComponent()] = this_component - this_component.mav_type = msg.type - this_component.mav_autopilot = msg.autopilot + + # 確保 VehicleView 存在 + vehicle = vehicle_registry.get(sysid) + if vehicle is None: + vehicle = vehicle_registry.register(sysid) + # 存儲 socket_id 到自定義 meta 中 + vehicle.custom_meta['socket_id'] = socket_id + + # 確保 VehicleComponent 存在 + component = vehicle.get_component(compid) + if component is None: + if msg_id == 0: # 只有透過 HEARTBEAT 才能創建新的 component + # 根據 mav_type 判斷 component 類型 + comp_type = self._determine_component_type(msg.type) + component = vehicle.add_component(compid, comp_type) + component.mav_type = msg.type + component.mav_autopilot = msg.autopilot + else: + # component 不存在且非 HEARTBEAT，忽略此訊息 + continue + + # 使用處理器字典分發訊息處理 + if msg_id in self.message_handlers: + try: + self.message_handlers[msg_id](vehicle, component, msg, timestamp) + except Exception as e: + logger.error(f'Error handling message type {msg_id}: {e}') else: - continue - - # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ - - 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 - - # print("mav_type : ", msg.type) # debug - print("get mode :", mavutil.mode_string_v10(msg)) # debug - # print("record mode :", this_component.emitParams['flightMode_mode']) # debug - - 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 + logger.debug(f'Unhandled message type: {msg_id} / {msg.get_type()}') + + logger.info('mavlink_bridge END!') + + def _determine_component_type(self, mav_type: int) -> ComponentType: + """根據 MAV_TYPE 判斷組件類型""" + # MAV_TYPE 定義： + # 0=通用, 1=固定翼, 2=四旋翼, 3=直升機, 4=天線追蹤器, 5=GCS, 6=飛船, + # 26=雲台, 27=ADSB, 28=降落傘等 + if mav_type == 6: # MAV_TYPE_GCS + return ComponentType.GCS + elif mav_type == 26: # MAV_TYPE_GIMBAL + return ComponentType.GIMBAL + elif mav_type == 30: # MAV_TYPE_CAMERA + return ComponentType.CAMERA + elif mav_type in [1, 2, 3, 4, 13, 14, 15, 19, 20, 21, 22]: # 各種飛行器類型 + return ComponentType.AUTOPILOT + else: + return ComponentType.OTHER + + # === 訊息處理器 === + def _handle_heartbeat(self, vehicle, component, msg, timestamp): + """處理 HEARTBEAT 訊息 (msg_id: 0)""" + component.status.mode.base_mode = msg.base_mode + component.status.mode.custom_mode = msg.custom_mode + component.status.mode.mode_name = mavutil.mode_string_v10(msg) + component.status.mode.timestamp = timestamp + component.status.system_status = msg.system_status + component.status.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 + # print("get mode:", mavutil.mode_string_v10(msg)) # debug + + def _handle_attitude(self, vehicle, component, msg, timestamp): + """處理 ATTITUDE 訊息 (msg_id: 30)""" + component.status.attitude.roll = msg.roll + component.status.attitude.pitch = msg.pitch + component.status.attitude.yaw = msg.yaw + component.status.attitude.rollspeed = msg.rollspeed + component.status.attitude.pitchspeed = msg.pitchspeed + component.status.attitude.yawspeed = msg.yawspeed + component.status.attitude.timestamp = timestamp + + def _handle_local_position(self, vehicle, component, msg, timestamp): + """處理 LOCAL_POSITION_NED 訊息 (msg_id: 32)""" + # LOCAL_POSITION_NED 提供相對位置資訊 + component.status.position.relative_altitude = -msg.z # NED 座標系，z 為負表示高度 + component.status.position.timestamp = timestamp + # 也可以存儲到 custom_status 中保留原始資料 + component.status.custom_status['local_position'] = { + 'x': msg.x, 'y': msg.y, 'z': msg.z, + 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz + } + + def _handle_global_position(self, vehicle, component, msg, timestamp): + """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)""" + component.status.position.latitude = msg.lat / 1e7 # 轉換為度 + component.status.position.longitude = msg.lon / 1e7 # 轉換為度 + component.status.position.altitude = msg.alt / 1000.0 # 轉換為公尺 + component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺 + component.status.position.timestamp = timestamp + + def _handle_vfr_hud(self, vehicle, component, msg, timestamp): + """處理 VFR_HUD 訊息 (msg_id: 74)""" + component.status.vfr.airspeed = msg.airspeed + component.status.vfr.groundspeed = msg.groundspeed + component.status.vfr.heading = msg.heading + component.status.vfr.throttle = msg.throttle + component.status.vfr.climb = msg.climb + component.status.vfr.timestamp = timestamp + + def _handle_battery_status(self, vehicle, component, msg, timestamp): + """處理 BATTERY_STATUS 訊息 (msg_id: 147)""" + # 計算電池總電壓（mV 轉 V） + if hasattr(msg, 'voltages') and msg.voltages[0] != 65535: + # voltages 是一個陣列，包含各個電池單元的電壓 + total_voltage = sum(v for v in msg.voltages if v != 65535) / 1000.0 + component.status.battery.voltage = total_voltage + + component.status.battery.current = msg.current_battery / 100.0 if msg.current_battery != -1 else None # cA 轉 A + component.status.battery.remaining = msg.battery_remaining if msg.battery_remaining != -1 else None # 百分比 + component.status.battery.temperature = msg.temperature / 100.0 if hasattr(msg, 'temperature') and msg.temperature != 32767 else None # 轉換為攝氏 + component.status.battery.timestamp = timestamp - elif msg_id == 147: # BATTERY_STATUS 處理 - this_component.emitParams['battery'] = msg + # === 訊息發送功能 === + def send_message(self, message_bytes, target_sysid=None, target_socket_id=None, broadcast=False): + """ + 發送訊息到指定的 mavlink_object + + Args: + message_bytes: 準備好的 mavlink 封包 (bytes) + target_sysid: 目標系統 ID (可選，用於自動查找對應的 socket) + target_socket_id: 目標 socket ID (可選，直接指定) + + Returns: + bool: 是否成功發送 + + 使用方式： + 1. broadcast: 廣播到所有活動的 mavlink_object + 2. 指定 target_socket_id：直接發送到該 socket + 3. 指定 target_sysid：自動查找該系統對應的 socket 並發送 + """ + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: {type(message_bytes)}") + return False - # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑ + # 情況 1: 廣播到所有活動的 mavlink_object + if broadcast: + success_count = 0 + for socket_id, mav_obj in mavlink_object.mavlinkObjects.items(): + if self._send_to_socket(message_bytes, socket_id): + success_count += 1 + + return success_count > 0 - # 若未定義的訊息類型則不處理 並跳出訊息 + # 情況 2: 直接指定 socket_id + if target_socket_id is not None: + return self._send_to_socket(message_bytes, target_socket_id) + + # 情況 3: 透過 sysid 查找對應的 socket + if target_sysid is not None: + vehicle = vehicle_registry.get(target_sysid) + if vehicle and 'socket_id' in vehicle.custom_meta: + socket_id = vehicle.custom_meta['socket_id'] + return self._send_to_socket(message_bytes, socket_id) else: - logger.warning('This Message Type Did not define process method : {} / {}'.format(msg.get_msgId(), msg.get_type())) - continue + logger.warning(f"System ID {target_sysid} not found or no socket_id") + return False - logger.info('End of mavlink_bridge._run_thread') - - # === Node 區塊 === - def _init_node(self): - logger.info('Start of mavlink_bridge._init_node') - super().__init__('mavlink_bridge') # 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 + logger.warning("No target specified for sending message. WTF ARE YOU DOING?") + return False # 若無指定任何目標，則返回失敗 + + def _send_to_socket(self, message_bytes, socket_id): + """ + 將訊息發送到指定的 mavlink_object + + Args: + message_bytes: mavlink 封包 + socket_id: 目標 socket ID + + Returns: + bool: 是否成功 + """ + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False + + mav_obj = mavlink_object.mavlinkObjects[socket_id] + return mav_obj.send_message(message_bytes) # ====================== 分割線 ===================== -''' -本次改版的暫時記錄註解於此 -1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 - 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 - 而 multiplexingToSwap 會完全被移除代替方式下一條描述 -2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 - (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) - 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 - 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 -3. mavlink_object 需要加上 state 去管理其狀態 -4. mavlink_object 需要加上 target port 去管理寫出的目標 -5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 -6. 基於第1,2項 updateMultiplexingList 會被完全移除 -7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object -8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 - -''' - # 定義 mavlink_object 的狀態 class MavlinkObjectState(Enum): - INIT = auto() # 初始化狀態 - RUNNING = auto() # 運行中狀態 - ERROR = auto() # 錯誤狀態 - CLOSED = auto() # 已關閉狀態 + INIT = auto() # 初始化狀態 + RUNNING = auto() # 運行中狀態 + SHUTTINGDOWN = auto() # 關閉中狀態 + ERROR = auto() # 錯誤狀態 + CLOSED = auto() # 已關閉狀態 class mavlink_object: ''' @@ -251,25 +370,16 @@ class mavlink_object: def __init__(self, socket): # 登入所需的 socket self.mavlink_socket = socket - - # 存放目前是否分流到固定串流橋接器和回傳封包處理器的標誌 # 都是要做的 把這個判斷拿掉 - # self.send_to_bridge = True - # self.send_to_return = False # 用於主線程發送消息的緩衝區 - self.outgoing_msg_lock = threading.Lock() - self.outgoing_msgs = [] + self.outgoing_msgs = deque() # 記錄訊息過濾類型 (可選) - self.bridge_msg_types = [0] # 默認只處理 HEARTBEAT - self.return_msg_types = [] + self.bridge_msg_types = set([0, 30]) # 0 HEARTBEAT, 30 ATTITUDE, ... + self.return_msg_types = set() - # 目標端口 - self.target_sockets = [] - - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems + # 轉發到別的 mavlink object 作為目標端口 的列表 + self.target_sockets = set() # 物件變數 self.task = None # Task reference @@ -277,12 +387,20 @@ class mavlink_object: self.dirtyDataMax = 10 # 髒資料容許閾值 self.state = MavlinkObjectState.INIT - logger.info(f'mavlink_object instance {self.socket_id} created') + # logger.info(f'mavlink_object instance {self.socket_id} created') # 先註解掉避免太多 log 但是為了 debug 保留 + + # 附加參數 (並非 mavlink_object 運行本體必要 但是要給上層結構運用的) + # 若這個 socket 是 另一個"主要 socket"的備用連接 則設定為"主要 socket id" + self.primary_socket_id = None # None 表示不是備用連接 + # socket type + self.socket_type = 'UNDEFINED' # 可選 'UDP_INBOUND', 'SERIAL_XBEE'...etc def __del__(self): - # 停止 asyncio task - self.stop() - + # # 先移除其他 socket 指向這個 socket 的目標 # 還是不要在這邊做了 畢竟本來就有判斷 object 不活躍就不轉拋 + # for mavlink_obj in self.mavlinkObjects.values(): + # if self.socket_id in mavlink_obj.target_sockets: + # mavlink_obj.remove_target_socket(self.socket_id) + # 關閉 socket if hasattr(self, 'mavlink_socket') and self.mavlink_socket: try: @@ -304,110 +422,68 @@ class mavlink_object: # except Exception as e: # print(f"Error logging in __del__: {e}") - def start(self): - """啟動 mavlink_object 處理循環""" - if self.state == MavlinkObjectState.RUNNING: - logger.warning(f"mavlink_object {self.socket_id} is already running") - return - - self.state = MavlinkObjectState.RUNNING - # 實際的啟動會由 async_io_manager 處理 - - def stop(self): - """停止 mavlink_object 處理循環""" - if self.state in (MavlinkObjectState.CLOSED, MavlinkObjectState.ERROR): - return - - self.state = MavlinkObjectState.CLOSED - if self.task: - if not self.task.done(): - self.task.cancel() - async def process_data(self): """處理 mavlink 數據的主要 asyncio 協程""" - logger.info(f'Start of mavlink_object.process_data id: {self.socket_id}') + # logger.info(f'Start of mavlink_object id: {self.socket_id}') # 先註解掉避免太多 log 但是為了 debug 保留 while self.state == MavlinkObjectState.RUNNING: timestamp = time.time() - try: #TODO 這邊的錯誤處理要再想想看怎麼做比較好 - # 處理接收到的封包 - mavlinkMsg = self.mavlink_socket.recv_msg() - # except Exception as e: - # logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") - # self.dirtyDataCount += 1 - - # if self.dirtyDataCount >= self.dirtyDataMax: - # logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") - # self.state = MavlinkObjectState.ERROR - # # 不直接退出，嘗試容忍髒數據 - # await asyncio.sleep(0.01) # 短暫暫停 - # continue - # try: - if mavlinkMsg: - # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 - sysid = mavlinkMsg.get_srcSystem() - compid = mavlinkMsg.get_srcComponent() - - if sysid in self.mavlink_systems: # 只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息 - self.mavlink_systems[sysid].updateComponentPacketCount( - compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) #TODO 這邊怪怪的 好像要再檢查 - - # 分發訊息到 RingBuffer - msg_id = mavlinkMsg.get_msgId() - - # if self.send_to_bridge and (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): - if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): - stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg)) - - # if self.send_to_return and (msg_id in self.return_msg_types or -1 in self.return_msg_types): - if (msg_id in self.return_msg_types or -1 in self.return_msg_types): - return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg)) - - # 將接收到的訊息轉發給所有目標端口 - for target_port in self.target_sockets: - if target_port != self.socket_id and target_port in self.mavlinkObjects: - target_obj = self.mavlinkObjects[target_port] - if target_obj.state == MavlinkObjectState.RUNNING: - try: # TODO 藉由 if 的檢測 確定目標端口是開啟狀態後 再進行寫出 之後刪掉 try except - target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) - except Exception as e: - logger.error(f"Error forwarding message to port {target_port}: {e}") - - with self.outgoing_msg_lock: - if self.outgoing_msgs and (send_msg := self.outgoing_msgs.pop(0)): - try: - self.mavlink_socket.write(send_msg) - except Exception as e: - logger.error(f"mavlink_object {self.socket_id} failed to send message: {e}") + # 處理接收到的封包 + mavlinkMsg = self.mavlink_socket.recv_msg() + + if mavlinkMsg: + # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 + sysid = mavlinkMsg.get_srcSystem() + compid = mavlinkMsg.get_srcComponent() - # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權 - await asyncio.sleep(0.001) + # 更新封包統計資訊 + vehicle = vehicle_registry.get(sysid) + if vehicle: # 只有當這個 system id 已經被註冊過才會記錄統計 + component = vehicle.get_component(compid) + if component: + component.update_packet_stats( + mavlinkMsg.get_seq(), + mavlinkMsg.get_msgId(), + timestamp + ) - except asyncio.CancelledError: - logger.info(f'mavlink_object.process_data for id {self.socket_id} was cancelled') - break - except Exception as e: - logger.warning(f"Error in mavlink_object.process_data for id {self.socket_id}: {e}") - self.dirtyDataCount += 1 + # 分發訊息到 RingBuffer + msg_id = mavlinkMsg.get_msgId() - if self.dirtyDataCount >= self.dirtyDataMax: - logger.error(f"Too many dirty data received in mavlink_object {self.socket_id}, entering ERROR state") - self.state = MavlinkObjectState.ERROR - # 不直接退出，嘗試容忍髒數據 - await asyncio.sleep(0.01) # 短暫暫停避免CPU過載 + if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): + stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg)) + + if (msg_id in self.return_msg_types or -1 in self.return_msg_types): + return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg)) - - logger.info(f'End of mavlink_object.process_data id: {self.socket_id}') + # 將全部接收到的訊息轉發給目標列表中的 mavlink_object + for target_socket in self.target_sockets: + if target_socket in self.mavlinkObjects: + target_obj = self.mavlinkObjects[target_socket] + if target_obj.state == MavlinkObjectState.RUNNING: + target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) + + if self.outgoing_msgs: + send_msg = self.outgoing_msgs.popleft() + self.mavlink_socket.write(send_msg) + + + # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權 + await asyncio.sleep(0.001) + + logger.info(f'End of mavlink_object id: {self.socket_id}') self.state = MavlinkObjectState.CLOSED def add_target_socket(self, target_socket_id): """添加一個目標端口用於轉發""" - if target_socket_id not in self.target_sockets and target_socket_id != self.socket_id: - self.target_sockets.append(target_socket_id) - logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}") - return True - return False + if (target_socket_id != self.socket_id) and (target_socket_id != self.primary_socket_id): + if target_socket_id not in self.target_sockets: + self.target_sockets.add(target_socket_id) + logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}") + return True + return False # 已存在 + return False # 不能添加自己 也不能添加主要 socket def remove_target_socket(self, target_socket_id): """移除目標端口""" @@ -415,12 +491,12 @@ class mavlink_object: self.target_sockets.remove(target_socket_id) logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}") return True - return False + return False # 不存在 def set_bridge_message_types(self, msg_types): """設置需要分流到橋接器的訊息類型""" if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): - self.bridge_msg_types = msg_types + self.bridge_msg_types = set(msg_types) return True logger.error(f"Invalid bridge message types: {msg_types}") return False @@ -428,7 +504,7 @@ class mavlink_object: def set_return_message_types(self, msg_types): """設置需要分流到回傳處理器的訊息類型""" if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): - self.return_msg_types = msg_types + self.return_msg_types = set(msg_types) return True logger.error(f"Invalid return message types: {msg_types}") return False @@ -444,31 +520,45 @@ class mavlink_object: Returns: bool: 是否成功添加消息到列表 """ + # 狀態檢查 if self.state != MavlinkObjectState.RUNNING: logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running") return False - try: - # 使用鎖保護共享資源訪問 - with self.outgoing_msg_lock: - self.outgoing_msgs.append(message_bytes) - return True - except Exception as e: - logger.error(f"Error queueing message for mavlink_object {self.socket_id}: {e}") - return False + # 基本數據類型檢查（輕量級） + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: expected bytes/bytearray, got {type(message_bytes)}") + return False + + # 基本長度檢查（MAVLink v1.0 最小8字節，v2.0最小12字節） + if len(message_bytes) < 8: + logger.error(f"Message too short: {len(message_bytes)} bytes (minimum 8)") + return False + + # MAVLink 起始標記檢查（輕量級） + if len(message_bytes) > 0 and message_bytes[0] not in (0xFE, 0xFD): # v1.0: 0xFE, v2.0: 0xFD + logger.error(f"Invalid MAVLink start marker: 0x{message_bytes[0]:02X}") + return False + + # 緩衝區大小保護（防止記憶體耗盡） + if len(self.outgoing_msgs) > 1000: # 可調整的閾值 + logger.warning(f"Outgoing message buffer full for mavlink_object {self.socket_id}") + return False - # def enable_bridge(self, enable=True): - # """啟用或禁用橋接器分流""" - # self.send_to_bridge = enable - - # def enable_return(self, enable=True): - # """啟用或禁用回傳處理器分流""" - # self.send_to_return = enable + self.outgoing_msgs.append(message_bytes) + return True class async_io_manager: """ 管理所有 mavlink_object 實例的 asyncio 任務 提供單一線程來處理所有 mavlink 通道的數據 + + 首先 async_io_manager 是 singleton 的 所以只能有一個實例 + + 這個 async_io_manager 是藉由 start 方法來啟動的 + + start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop + 然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務 """ _instance = None _lock = threading.Lock() @@ -483,157 +573,214 @@ class async_io_manager: if not hasattr(self, 'initialized'): self.initialized = True self.loop = None - self.main_task = None self.running = False - self.managed_objects = {} # socket_id: task - + # self.main_task = None + self.managed_objects = {} # socket_id: mavlink_object + self.thread = None + self._stop_event = threading.Event() + + def __del__(self): + self.loop = None + self.thread = None + def start(self): - """啟動 async_io_manager 和其管理的所有 mavlink_object""" + """ + 啟動 async_io_manager + + """ if self.running: logger.warning("async_io_manager already running") return self.running = True - self.thread = threading.Thread(target=self._run_event_loop) + self._stop_event.clear() + + # 啟動獨立線程 命名為 AsyncIOManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="AsyncIOManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 self.thread.start() - logger.info("async_io_manager started") + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("async_io_manager thread started <-") + return True + else: + logger.error("async_io_manager failed to start") + return False - def stop(self): + def shutdown(self): """停止 async_io_manager 和其管理的所有 mavlink_object""" + + # 自己在 running 狀態下才執行停止程序 if not self.running: return - + + # 停止所有被管理的 mavlink_object 所屬的 task + for socket_id in list(self.managed_objects.keys()): + self.remove_mavlink_object(socket_id) + + # 停止自己的 task self.running = False + self._stop_event.set() + + # 解開事件循環的阻塞 + self.loop.call_soon_threadsafe(self.loop.stop) + + # print("mark A", len(asyncio.all_tasks(self.loop))) # debug - if self.loop: - # 取消所有任務 - for socket_id in list(self.managed_objects.keys()): - self.remove_mavlink_object(socket_id) - - # 停止事件循環 - if self.main_task and not self.main_task.done(): - asyncio.run_coroutine_threadsafe(self._shutdown(), self.loop) - - # 等待線程結束 - if hasattr(self, 'thread') and self.thread.is_alive(): - self.thread.join(timeout=5.0) - - logger.info("async_io_manager stopped") - - async def _shutdown(self): - """正確關閉事件循環的協程""" - tasks = [task for task in asyncio.all_tasks(self.loop) if task is not asyncio.current_task()] + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=10.0) + if self.thread.is_alive(): + logger.warning("async_io_manager thread did not stop gracefully") + os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序 - for task in tasks: - task.cancel() + - await asyncio.gather(*tasks, return_exceptions=True) - self.loop.stop() + logger.info("async_io_manager thread END!") def _run_event_loop(self): """在獨立線程中運行事件循環""" - self.loop = asyncio.new_event_loop() - asyncio.set_event_loop(self.loop) - try: - self.main_task = self.loop.create_task(self._main_task()) + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + logger.info("async_io_manager event loop started <-") + + # 創建主任務 + # main_task = self.loop.create_task(self._main_task()) + + # 運行事件循環 self.loop.run_forever() + except Exception as e: logger.error(f"Error in async_io_manager event loop: {e}") finally: - self.loop.close() + # 清理 + if self.loop: + self.loop.close() self.loop = None self.running = False - logger.info("async_io_manager event loop ended") + logger.info("async_io_manager event loop END!") - async def _main_task(self): - """主任務協程，用於監視和管理其他任務""" + async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管 + """主任務協程 讓 async_io_manager 在執行緒中持續運作""" logger.info("async_io_manager main task started") - try: - while self.running: - # 這邊就是一個無窮迴圈 確保 async_io_manager 物件續存 + while self.running and not self._stop_event.is_set(): + await asyncio.sleep(0.1) - # 每秒鐘檢查並移除已完成或已取消的任務 - await asyncio.sleep(1.0) - for socket_id in list(self.managed_objects.keys()): - task = self.managed_objects[socket_id] - - if task.done(): - try: # TODO 這邊的錯誤處理要再想想看怎麼做比較好 - exc = task.exception() - if exc: - logger.error(f"Task for mavlink_object {socket_id} raised exception: {exc}") - except (asyncio.CancelledError, asyncio.InvalidStateError): - pass - - if socket_id in mavlink_object.mavlinkObjects: - print(f"this is manager, i make socket_id {socket_id} closed, he is now in {mavlink_object.mavlinkObjects[socket_id].state} state.") # debug - mavlink_object.mavlinkObjects[socket_id].state = MavlinkObjectState.CLOSED - - del self.managed_objects[socket_id] - - - - except asyncio.CancelledError: - logger.info("async_io_manager main task was cancelled") - except Exception as e: - logger.error(f"Error in async_io_manager main task: {e}") - logger.info("async_io_manager main task ended") - + def add_mavlink_object(self, mavlink_obj): - """添加一個 mavlink_object 實例到 async_io_manager 並啟動其處理任務""" - if not self.running: + """添加 mavlink_object""" + # 一個防呆 確保有 event loop 與 _main_task 正在運作 + if not self.running or not self.loop: logger.error("Cannot add mavlink_object: async_io_manager is not running") return False - - if not isinstance(mavlink_obj, mavlink_object): - logger.error(f"Invalid mavlink_object: {mavlink_obj}") - return False - + socket_id = mavlink_obj.socket_id if socket_id in self.managed_objects: logger.warning(f"mavlink_object {socket_id} already managed") return False - - # 創建並啟動任務 - if self.loop: - # task = asyncio.run_coroutine_threadsafe(mavlink_obj.process_data(), self.loop).result() - future = asyncio.run_coroutine_threadsafe(mavlink_obj.process_data(), self.loop) - # print(f"Task created for mavlink_object {socket_id}: {future1}") # debug - # task = future1.result() - self.managed_objects[socket_id] = future - mavlink_obj.task = future + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_add_mavlink_object(mavlink_obj), + self.loop + ) + + try: + # 等待結果，設定合理的超時時間 + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout adding mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to add mavlink_object {socket_id}: {e}") + return False + + async def _async_add_mavlink_object(self, mavlink_obj): + """在事件循環線程中同步執行""" + socket_id = mavlink_obj.socket_id + + try: + task = asyncio.create_task(mavlink_obj.process_data()) + self.managed_objects[socket_id] = mavlink_obj + mavlink_obj.task = task mavlink_obj.state = MavlinkObjectState.RUNNING - logger.info(f"Added mavlink_object {socket_id} to async_io_manager") + mavlink_obj.outgoing_msgs.clear() + logger.info(f"Added mavlink_object {socket_id} into manager.") return True - - return False + except Exception as e: + logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}") + return False def remove_mavlink_object(self, socket_id): - """從 async_io_manager 中移除一個 mavlink_object 實例並取消其處理任務""" - if socket_id not in self.managed_objects: - logger.warning(f"mavlink_object {socket_id} not managed by async_io_manager") + """移除 mavlink_object""" + + # 一個防呆 確保有 event loop 正在運作 + if not self.loop: return False - # 取消任務 - task = self.managed_objects[socket_id] - if not task.done(): - task.cancel() + # 同樣使用 run_coroutine_threadsafe + future = asyncio.run_coroutine_threadsafe( + self._async_remove_mavlink_object(socket_id), + self.loop + ) - # 從管理列表中移除 - del self.managed_objects[socket_id] + try: + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to remove mavlink_object {socket_id}: {e}") + return False + + async def _async_remove_mavlink_object(self, socket_id): + """在事件循環線程中同步執行""" + if socket_id not in self.managed_objects: + logger.warning(f"mavlink_object {socket_id} not managed") + return - # 更新 mavlink_object 狀態 - if socket_id in mavlink_object.mavlinkObjects: - mavlink_object.mavlinkObjects[socket_id].state = MavlinkObjectState.CLOSED + mavlink_obj = self.managed_objects[socket_id] + mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN + + if not mavlink_obj.task.done(): + mavlink_obj.task.cancel() - logger.info(f"Removed mavlink_object {socket_id} from async_io_manager") - return True - + # 等待一秒或者 task完全結束 + timeout = 1.0 + start_time = asyncio.get_event_loop().time() + while not mavlink_obj.task.done(): + if asyncio.get_event_loop().time() - start_time > timeout: + break + await asyncio.sleep(0.1) + + # 如果正常結束 則移除 + if mavlink_obj.task.done(): + del self.managed_objects[socket_id] + mavlink_obj.state = MavlinkObjectState.CLOSED + logger.info(f"Removed mavlink_object {socket_id} from manager.") + return True + else: + mavlink_obj.state = MavlinkObjectState.ERROR + logger.warning(f"mavlink_object {socket_id} task did not terminate in time") + return False + def get_managed_objects(self): """獲取所有被管理的對象列表""" return list(self.managed_objects.keys()) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py new file mode 100644 index 0000000..86c0478 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -0,0 +1,435 @@ +""" +VehicleView - Pure State Container +純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 +只提供狀態存取介面，由外部手動餵資料（push state） +""" + +import os +from typing import Dict, Optional, Any +from dataclasses import dataclass, field +from enum import Enum + +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ====================== Enums ===================== + +class ConnectionType(Enum): + """連接類型""" + SERIAL = "serial" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +class ComponentType(Enum): + """組件類型""" + AUTOPILOT = "autopilot" + GCS = "gcs" + CAMERA = "camera" + GIMBAL = "gimbal" + OTHER = "other" + + +class RFModuleType(Enum): + """RF模組類型""" + XBEE = "xbee" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +# ====================== Data Classes ===================== + +@dataclass +class Position: + """位置資訊""" + latitude: Optional[float] = None # 緯度 (度) + longitude: Optional[float] = None # 經度 (度) + altitude: Optional[float] = None # 海拔高度 (公尺) + relative_altitude: Optional[float] = None # 相對高度 (公尺) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Attitude: + """姿態資訊""" + roll: Optional[float] = None # 橫滾角 (弧度) + pitch: Optional[float] = None # 俯仰角 (弧度) + yaw: Optional[float] = None # 偏航角 (弧度) + rollspeed: Optional[float] = None # 橫滾速度 (弧度/秒) + pitchspeed: Optional[float] = None # 俯仰速度 (弧度/秒) + yawspeed: Optional[float] = None # 偏航速度 (弧度/秒) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class FlightMode: + """飛行模式資訊""" + base_mode: Optional[int] = None # MAVLink base mode + custom_mode: Optional[int] = None # 自定義模式 + mode_name: Optional[str] = None # 模式名稱 (例如: "GUIDED", "AUTO") + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Battery: + """電池資訊""" + voltage: Optional[float] = None # 電壓 (V) + current: Optional[float] = None # 電流 (A) + remaining: Optional[int] = None # 剩餘電量 (%) + temperature: Optional[float] = None # 溫度 (攝氏) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class EKF: + """EKF狀態資訊""" + flags: Optional[int] = None # EKF 旗標 + velocity_variance: Optional[float] = None # 速度變異 + pos_horiz_variance: Optional[float] = None # 水平位置變異 + pos_vert_variance: Optional[float] = None # 垂直位置變異 + compass_variance: Optional[float] = None # 羅盤變異 + terrain_alt_variance: Optional[float] = None # 地形高度變異 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class GPS: + """GPS資訊""" + fix_type: Optional[int] = None # GPS fix 類型 (0=無, 1=無fix, 2=2D, 3=3D, 4=DGPS, 5=RTK) + satellites_visible: Optional[int] = None # 可見衛星數 + eph: Optional[int] = None # GPS HDOP 水平精度因子 + epv: Optional[int] = None # GPS VDOP 垂直精度因子 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class VFR: + """VFR HUD資訊""" + airspeed: Optional[float] = None # 空速 (m/s) + groundspeed: Optional[float] = None # 地速 (m/s) + heading: Optional[int] = None # 航向 (度) + throttle: Optional[int] = None # 油門 (%) + climb: Optional[float] = None # 爬升率 (m/s) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class ComponentStatus: + """組件狀態容器""" + position: Position = field(default_factory=Position) + attitude: Attitude = field(default_factory=Attitude) + mode: FlightMode = field(default_factory=FlightMode) + battery: Battery = field(default_factory=Battery) + ekf: EKF = field(default_factory=EKF) + gps: GPS = field(default_factory=GPS) + vfr: VFR = field(default_factory=VFR) + + # 系統狀態 + system_status: Optional[int] = None # MAV_STATE + armed: Optional[bool] = None # 解鎖狀態 + + # 其他自定義狀態 + custom_status: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class PacketStats: + """封包統計資訊""" + received_count: int = 0 # 接收封包數 + lost_count: int = 0 # 遺失封包數 + last_seq: Optional[int] = None # 最後序號 + last_msg_time: Optional[float] = None # 最後訊息時間 + msg_type_count: Dict[int, int] = field(default_factory=dict) # 各類訊息計數 {msg_type: count} + + +@dataclass +class RFStatus: + """RF模組狀態""" + rssi: Optional[int] = None # 信號強度 + noise: Optional[int] = None # 噪音水平 + at_response: Optional[str] = None # AT 命令回應 + link_quality: Optional[int] = None # 連接品質 + timestamp: Optional[float] = None # 時間戳記 + custom_status: Dict[str, Any] = field(default_factory=dict) # 其他自定義狀態 + + +@dataclass +class SocketInfo: + """Socket連接資訊""" + ip: Optional[str] = None # IP位址 + port: Optional[int] = None # 埠號 + local_ip: Optional[str] = None # 本地IP + local_port: Optional[int] = None # 本地埠號 + connected: bool = False # 連接狀態 + + +# ====================== Component Class ===================== + +class VehicleComponent: + """載具組件 - 代表一個 MAVLink component""" + + def __init__(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER): + self.component_id = component_id + self.type = comp_type + + # MAVLink 組件資訊 + self.mav_type: Optional[int] = None # MAV_TYPE + self.mav_autopilot: Optional[int] = None # MAV_AUTOPILOT + + # 狀態容器 + self.status = ComponentStatus() + + # 參數容器 (只存放，不主動下載) + self.parameters: Dict[str, Any] = {} + + # 封包統計 + self.packet_stats = PacketStats() + + def update_packet_stats(self, seq: int, msg_type: int, timestamp: float) -> None: + """ + 更新封包統計 + + Args: + seq: 訊息序號 + msg_type: 訊息類型 + timestamp: 時間戳記 + """ + stats = self.packet_stats + + # 檢查遺失封包 + if stats.last_seq is not None: + expected_seq = (stats.last_seq + 1) % 256 + diff = seq - expected_seq + if diff < 0: + diff += 256 + stats.lost_count += diff + + # 更新統計資訊 + stats.received_count += 1 + stats.last_seq = seq + stats.last_msg_time = timestamp + + # 更新訊息類型計數 + if msg_type in stats.msg_type_count: + stats.msg_type_count[msg_type] += 1 + else: + stats.msg_type_count[msg_type] = 1 + + def reset_packet_stats(self) -> None: + """重置封包統計""" + self.packet_stats = PacketStats() + + def set_parameter(self, param_name: str, param_value: Any) -> None: + """設定參數 (手動餵入)""" + self.parameters[param_name] = param_value + + def get_parameter(self, param_name: str, default: Any = None) -> Any: + """取得參數""" + return self.parameters.get(param_name, default) + + def __str__(self) -> str: + return (f"Component(id={self.component_id}, type={self.type.value}, " + f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, " + f"lost={self.packet_stats.lost_count})") + + +# ====================== RF Module Class ===================== + +class RFModule: + """RF模組資訊容器""" + + def __init__(self, rf_type: RFModuleType = RFModuleType.OTHER): + self.type = rf_type + self.status = RFStatus() + self.socket_info = SocketInfo() + + def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None: + """更新RSSI""" + self.status.rssi = rssi + if timestamp: + self.status.timestamp = timestamp + + def update_at_response(self, response: str, timestamp: Optional[float] = None) -> None: + """更新AT命令回應""" + self.status.at_response = response + if timestamp: + self.status.timestamp = timestamp + + def update_socket_info(self, ip: str = None, port: int = None, + local_ip: str = None, local_port: int = None, + connected: bool = None) -> None: + """更新Socket資訊""" + if ip is not None: + self.socket_info.ip = ip + if port is not None: + self.socket_info.port = port + if local_ip is not None: + self.socket_info.local_ip = local_ip + if local_port is not None: + self.socket_info.local_port = local_port + if connected is not None: + self.socket_info.connected = connected + + def __str__(self) -> str: + return (f"RFModule(type={self.type.value}, rssi={self.status.rssi}, " + f"connected={self.socket_info.connected})") + + +# ====================== Main VehicleView Class ===================== + +class VehicleView: + """ + 載具視圖 - 純狀態容器 + + 特點: + 1. 只有狀態容器，沒有任何主動通訊功能 + 2. 不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 + 3. 可以手動餵資料 (push state) + 4. 可擴充 (支援 RF 模組狀態) + """ + + def __init__(self, sysid: int): + # Meta 資訊 + self.sysid = sysid + self.kind: Optional[str] = None # 載具種類描述 (例如: "Copter", "Plane") + self.vehicle_type: Optional[int] = None # MAV_TYPE + self.connected_via: ConnectionType = ConnectionType.OTHER + + # 組件容器 {component_id: VehicleComponent} + self.components: Dict[int, VehicleComponent] = {} + + # RF模組 + self.rf_module: Optional[RFModule] = None + + # 其他自定義meta資訊 + self.custom_meta: Dict[str, Any] = {} + + def add_component(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER) -> VehicleComponent: + """ + 新增組件 + + Args: + component_id: 組件ID + comp_type: 組件類型 + + Returns: + VehicleComponent: 新增的組件 + """ + if component_id not in self.components: + self.components[component_id] = VehicleComponent(component_id, comp_type) + logger.info(f"Added component {component_id} to system {self.sysid}") + return self.components[component_id] + + def get_component(self, component_id: int) -> Optional[VehicleComponent]: + """取得組件""" + return self.components.get(component_id) + + def remove_component(self, component_id: int) -> bool: + """移除組件""" + if component_id in self.components: + del self.components[component_id] + logger.info(f"Removed component {component_id} from system {self.sysid}") + return True + return False + + def set_rf_module(self, rf_type: RFModuleType) -> RFModule: + """設定RF模組""" + self.rf_module = RFModule(rf_type) + return self.rf_module + + def get_rf_module(self) -> Optional[RFModule]: + """取得RF模組""" + return self.rf_module + + def __str__(self) -> str: + comp_list = ", ".join([str(cid) for cid in self.components.keys()]) + return (f"VehicleView(sysid={self.sysid}, kind={self.kind}, " + f"connected_via={self.connected_via.value}, " + f"components=[{comp_list}], " + f"rf_module={self.rf_module is not None})") + + def to_dict(self) -> Dict[str, Any]: + """轉換為字典格式 (方便序列化/除錯)""" + return { + 'meta': { + 'sysid': self.sysid, + 'kind': self.kind, + 'vehicle_type': self.vehicle_type, + 'connected_via': self.connected_via.value, + 'custom_meta': self.custom_meta + }, + 'components': { + cid: { + 'component_id': comp.component_id, + 'type': comp.type.value, + 'mav_type': comp.mav_type, + 'mav_autopilot': comp.mav_autopilot, + 'packet_stats': { + 'received': comp.packet_stats.received_count, + 'lost': comp.packet_stats.lost_count, + 'last_seq': comp.packet_stats.last_seq, + 'last_msg_time': comp.packet_stats.last_msg_time + }, + 'parameters_count': len(comp.parameters) + } + for cid, comp in self.components.items() + }, + 'rf_module': { + 'type': self.rf_module.type.value, + 'rssi': self.rf_module.status.rssi, + 'socket_connected': self.rf_module.socket_info.connected + } if self.rf_module else None + } + + +# ====================== Registry ===================== + +class VehicleViewRegistry: + """載具視圖註冊表 - 管理所有的 VehicleView""" + + def __init__(self): + self._vehicles: Dict[int, VehicleView] = {} + + def register(self, sysid: int) -> VehicleView: + """註冊新的載具視圖""" + if sysid not in self._vehicles: + self._vehicles[sysid] = VehicleView(sysid) + logger.info(f"Registered new VehicleView for system {sysid}") + return self._vehicles[sysid] + + def get(self, sysid: int) -> Optional[VehicleView]: + """取得載具視圖""" + return self._vehicles.get(sysid) + + def unregister(self, sysid: int) -> bool: + """註銷載具視圖""" + if sysid in self._vehicles: + del self._vehicles[sysid] + logger.info(f"Unregistered VehicleView for system {sysid}") + return True + return False + + def get_all(self) -> Dict[int, VehicleView]: + """取得所有載具視圖""" + return self._vehicles.copy() + + def clear(self) -> None: + """清空所有載具視圖""" + self._vehicles.clear() + logger.info("Cleared all VehicleViews") + + def __len__(self) -> int: + return len(self._vehicles) + + def __contains__(self, sysid: int) -> bool: + return sysid in self._vehicles + + +# ====================== Global Instance ===================== + +# 全域註冊表實例 +vehicle_registry = VehicleViewRegistry() diff --git a/src/fc_network_adapter/tests/demo_integration.py b/src/fc_network_adapter/tests/demo_integration.py index b621e73..7546f65 100644 --- a/src/fc_network_adapter/tests/demo_integration.py +++ b/src/fc_network_adapter/tests/demo_integration.py @@ -14,12 +14,13 @@ from pymavlink import mavutil # 自定義的 import from ..fc_network_adapter import mavlinkObject as mo -from ..fc_network_adapter import mavlinkDevice as md +from ..fc_network_adapter import mavlinkVehicleView as mvv +# from ..fc_network_adapter import mavlinkDevice as md # ====================== 分割線 ===================== test_item = 10 -running_time = 10000 +running_time = 3 print('test_item : ', test_item) @@ -44,10 +45,13 @@ if test_item == 10: time.sleep(0.5) # 等待事件循環啟動 # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" + connection_string="udp:127.0..1:14571" mavlink_socket1 = mavutil.mavlink_connection(connection_string) mavlink_object1 = mo.mavlink_object(mavlink_socket1) + sock = mavlink_socket1.port + print("Socket port:", sock) + manager.add_mavlink_object(mavlink_object1) start_time = time.time() @@ -65,7 +69,7 @@ if test_item == 10: print("return_packet_ring is empty") time.sleep(1) - manager.stop() + manager.shutdown() print('<=== End of Program') @@ -83,20 +87,22 @@ elif test_item == 11: time.sleep(0.5) # 等待事件循環啟動 # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" + connection_string="udp:127.0.0.1:14571" mavlink_socket1 = mavutil.mavlink_connection(connection_string) mavlink_object1 = mo.mavlink_object(mavlink_socket1) manager.add_mavlink_object(mavlink_object1) # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() + bridge = mo.mavlink_bridge() + bridge.start() time.sleep(3) # 印出目前所有 mavlink_systems 的內容 print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) + all_vehicles = mvv.vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle}") start_time = time.time() show_time = time.time() @@ -105,14 +111,15 @@ elif test_item == 11: # print("mark B") show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) + for sysid, vehicle in all_vehicles.items(): + for compid in vehicle.components: + comp = vehicle.get_component(compid) + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count)) + comp.reset_packet_stats() print("===================") - manager.stop() - analyzer.stop() + manager.shutdown() + bridge.stop() print('<=== End of Program') @@ -130,16 +137,16 @@ elif test_item == 12: time.sleep(0.5) # 等待事件循環啟動 # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" + connection_string="udp:127.0.0.1:14571" mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) - connection_string="udp:127.0.0.1:14561" + connection_string="udp:127.0.0.1:14571" mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) # 初始化輸出通道 - connection_string="udpout:127.0.0.1:14550" + connection_string="udpout:127.0.0.1:14551" mavlink_socket_out = mavutil.mavlink_connection(connection_string) mavlink_object_out = mo.mavlink_object(mavlink_socket_out) @@ -160,66 +167,11 @@ elif test_item == 12: time.sleep(1) - manager.stop() + manager.shutdown() print('<=== End of Program') -# elif test_item == 20: -# # 這邊測試 node 生成 topic 的功能 -# # 利用 空的通道 發出假的 heartbeat 封包 -# print('===> Start of Program .Test ', test_item) -# rclpy.init() # 注意要初始化 rclpy 才能使用 node - -# from pymavlink.dialects.v20 import common as mavlink2 - -# sysid = 1 -# compid = 1 - -# # 啟動 mavlink_bridge -# analyzer = mo.mavlink_bridge() - -# mav = mavlink2.MAVLink(None) -# mav.srcSystem = sysid -# mav.srcComponent = compid - -# # 這是一個 heartbeat 封包 -# fake_heartbeat = mavlink2.MAVLink_heartbeat_message( -# type=mavlink2.MAV_TYPE_QUADROTOR, -# autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, -# base_mode=3, -# custom_mode=0, -# system_status=0, -# mavlink_version=3 -# ) -# mavlink_bytes = fake_heartbeat.pack(mav) -# fake_heartbeat_msg = mav.parse_char(mavlink_bytes) - -# print(analyzer.mavlink_systems) - -# mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data -# time.sleep(0.1) -# print(analyzer.mavlink_systems) - -# # ROS2 初始化 -# analyzer._init_node() - -# # 創建 ROS2 topic -# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid -# print("topic created") -# time.sleep(5) - -# # 丟出 topic -# analyzer.emit_info() - -# # 結束程式 -# analyzer.running = False -# analyzer.destroy_node() - -# rclpy.shutdown() -# analyzer.stop() -# analyzer.thread.join() -# print('<=== End of Program') elif test_item == 21: # 需要開啟一個 ardupilot 的模擬器 @@ -291,160 +243,18 @@ elif test_item == 21: mavlink_socket.close() print('<=== End of Program') -# elif test_item == 22: -# # 需要開啟一個 ardupilot 的模擬器 與 GCS -# # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 -# 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_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" -# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) -# mavlink_object_out = mo.mavlink_object(mavlink_socket_out) -# mavlink_object_out.multiplexingToAnalysis = [] +elif test_item == 52: + print('===> Start of Program .Test ', test_item) + manager = mo.async_io_manager() + manager.start() -# # 讓兩個通道互相傳輸 -# mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all -# mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all + # print(manager.thread.is_alive()) -# # 啟動通道 -# mavlink_object_in.run() -# mavlink_object_out.run() + manager.shutdown() -# print('waiting for mavlink data ...') -# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - -# print('目前所有的系統 : ') -# for sysid in analyzer.mavlink_systems: -# print(analyzer.mavlink_systems[sysid]) - -# compid = 1 -# sysid = 1 -# show_time = time.time() -# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") - -# print("start emit info") - -# start_time = time.time() -# show_time = time.time() -# show_time2 = time.time() - -# while time.time() - start_time < running_time: -# if (time.time() - show_time2) >= 1: -# analyzer.emit_info() # 這邊是測試 node 的運行 -# # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] -# fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] -# sss = mavutil.mode_string_v10(fmsg) -# # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) -# print("目前的飛行模式 : {}".format(sss)) -# show_time2 = time.time() - -# # if (time.time() - show_time) >= 2: -# # show_time = time.time() -# # for sysid in analyzer.mavlink_systems: -# # for compid in analyzer.mavlink_systems[sysid].components: -# # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) -# # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) -# # print("===================") - - + time.sleep(1) -# analyzer.destroy_node() -# rclpy.shutdown() + print('manager stopped') - -# # 結束程式 退出所有 thread -# mavlink_object_in.stop() -# mavlink_object_in.thread.join() -# mavlink_socket_in.close() -# mavlink_object_out.stop() -# mavlink_object_out.thread.join() -# mavlink_socket_out.close() -# analyzer.stop() -# analyzer.thread.join() - - -# 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/tests/demo_mavlinkVehicleView.py b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py new file mode 100644 index 0000000..d6954d3 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py @@ -0,0 +1,331 @@ +""" +VehicleView 使用範例 +展示如何使用純狀態容器來管理 MAVLink 載具資訊 +""" + +import time +from ..fc_network_adapter.mavlinkVehicleView import ( + VehicleView, + VehicleComponent, + RFModule, + vehicle_registry, + ConnectionType, + ComponentType, + RFModuleType +) + + +def example_basic_usage(): + """基本使用範例""" + print("=== 基本使用範例 ===\n") + + # 1. 建立載具視圖 + vehicle = VehicleView(sysid=1) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + + print(f"建立載具: {vehicle}\n") + + # 2. 新增 autopilot 組件 + autopilot = vehicle.add_component( + component_id=1, + comp_type=ComponentType.AUTOPILOT + ) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + print(f"新增組件: {autopilot}\n") + + # 3. 手動餵入位置資訊 + autopilot.status.position.latitude = 25.0330 + autopilot.status.position.longitude = 121.5654 + autopilot.status.position.altitude = 100.5 + autopilot.status.position.timestamp = time.time() + + print(f"位置: 緯度={autopilot.status.position.latitude}, " + f"經度={autopilot.status.position.longitude}, " + f"高度={autopilot.status.position.altitude}m\n") + + # 4. 手動餵入姿態資訊 + autopilot.status.attitude.roll = 0.05 # 弧度 + autopilot.status.attitude.pitch = -0.02 + autopilot.status.attitude.yaw = 1.57 + autopilot.status.attitude.timestamp = time.time() + + print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, " + f"Pitch={autopilot.status.attitude.pitch:.3f}, " + f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n") + + # 5. 手動餵入飛行模式 + autopilot.status.mode.base_mode = 89 + autopilot.status.mode.custom_mode = 4 + autopilot.status.mode.mode_name = "GUIDED" + autopilot.status.mode.timestamp = time.time() + + print(f"飛行模式: {autopilot.status.mode.mode_name}\n") + + # 6. 手動餵入電池資訊 + autopilot.status.battery.voltage = 12.6 + autopilot.status.battery.current = 15.2 + autopilot.status.battery.remaining = 75 + autopilot.status.battery.timestamp = time.time() + + print(f"電池: 電壓={autopilot.status.battery.voltage}V, " + f"電流={autopilot.status.battery.current}A, " + f"剩餘={autopilot.status.battery.remaining}%\n") + + +def example_packet_tracking(): + """封包追蹤範例""" + print("\n=== 封包追蹤範例 ===\n") + + vehicle = VehicleView(sysid=2) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 模擬接收封包 + timestamp = time.time() + + # 接收 HEARTBEAT (msg_type=0) + autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp) + + # 接收 ATTITUDE (msg_type=30) + autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1) + + # 接收 GLOBAL_POSITION_INT (msg_type=33) + autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2) + + # 模擬封包遺失 (seq 跳過 3, 4, 5) + autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3) + + stats = autopilot.packet_stats + print(f"封包統計:") + print(f" 接收: {stats.received_count}") + print(f" 遺失: {stats.lost_count}") + print(f" 最後序號: {stats.last_seq}") + print(f" 訊息類型計數: {stats.msg_type_count}\n") + + +def example_parameters(): + """參數管理範例""" + print("\n=== 參數管理範例 ===\n") + + vehicle = VehicleView(sysid=3) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 手動設定參數 (不會主動下載) + autopilot.set_parameter("ARMING_CHECK", 1) + autopilot.set_parameter("ANGLE_MAX", 4500) + autopilot.set_parameter("WPNAV_SPEED", 500) + + print(f"參數數量: {len(autopilot.parameters)}") + print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}") + print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}") + print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n") + + +def example_rf_module(): + """RF模組範例""" + print("\n=== RF模組範例 ===\n") + + vehicle = VehicleView(sysid=4) + vehicle.connected_via = ConnectionType.SERIAL + + # 設定 XBee RF 模組 + rf = vehicle.set_rf_module(RFModuleType.XBEE) + + # 更新 Socket 資訊 + rf.update_socket_info( + ip="192.168.1.100", + port=14550, + local_ip="192.168.1.1", + local_port=14551, + connected=True + ) + + # 更新 RSSI + rf.update_rssi(rssi=-65, timestamp=time.time()) + + # 更新 AT 命令回應 + rf.update_at_response("OK", timestamp=time.time()) + + # 自定義狀態 + rf.status.custom_status['signal_quality'] = 'excellent' + rf.status.custom_status['packet_error_rate'] = 0.001 + + print(f"RF模組: {rf}") + print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}") + print(f"RSSI: {rf.status.rssi} dBm") + print(f"AT回應: {rf.status.at_response}") + print(f"自定義狀態: {rf.status.custom_status}\n") + + +def example_multiple_components(): + """多組件範例""" + print("\n=== 多組件範例 ===\n") + + vehicle = VehicleView(sysid=5) + vehicle.kind = "Copter with Gimbal" + + # Autopilot 組件 + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.mode.mode_name = "AUTO" + + # Gimbal 組件 + gimbal = vehicle.add_component(154, ComponentType.GIMBAL) + gimbal.mav_type = 26 # MAV_TYPE_GIMBAL + gimbal.status.attitude.pitch = -0.785 # 向下45度 + gimbal.status.attitude.yaw = 0.0 + + # Camera 組件 + camera = vehicle.add_component(100, ComponentType.CAMERA) + camera.mav_type = 30 # MAV_TYPE_CAMERA + camera.status.custom_status['recording'] = True + camera.status.custom_status['photo_interval'] = 2.0 + + print(f"載具: {vehicle}") + print(f"組件數量: {len(vehicle.components)}") + for cid, comp in vehicle.components.items(): + print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}") + print() + + +def example_registry(): + """註冊表使用範例""" + print("\n=== 註冊表使用範例 ===\n") + + # 註冊多個載具 + v1 = vehicle_registry.register(sysid=1) + v1.kind = "Copter-1" + v1.add_component(1, ComponentType.AUTOPILOT) + + v2 = vehicle_registry.register(sysid=2) + v2.kind = "Plane-1" + v2.add_component(1, ComponentType.AUTOPILOT) + + v3 = vehicle_registry.register(sysid=3) + v3.kind = "Rover-1" + v3.add_component(1, ComponentType.AUTOPILOT) + + print(f"註冊表中的載具數量: {len(vehicle_registry)}") + + # 取得所有載具 + all_vehicles = vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle.kind}") + + # 檢查載具是否存在 + print(f"\nSystem 2 存在? {2 in vehicle_registry}") + print(f"System 99 存在? {99 in vehicle_registry}") + + # 取得特定載具 + vehicle = vehicle_registry.get(2) + if vehicle: + print(f"\n取得載具: {vehicle}") + + # 註銷載具 + vehicle_registry.unregister(3) + print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n") + + +def example_serialization(): + """序列化範例 (除錯/日誌用)""" + print("\n=== 序列化範例 ===\n") + + vehicle = VehicleView(sysid=10) + vehicle.kind = "Test Copter" + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0' + vehicle.custom_meta['frame_type'] = 'X' + + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.position.altitude = 50.0 + autopilot.status.battery.voltage = 12.4 + autopilot.update_packet_stats(0, 0, time.time()) + autopilot.update_packet_stats(1, 30, time.time()) + + rf = vehicle.set_rf_module(RFModuleType.UDP) + rf.update_rssi(-70) + rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True) + + # 轉換為字典 + data = vehicle.to_dict() + + print("載具資料 (字典格式):") + import json + print(json.dumps(data, indent=2, ensure_ascii=False)) + + +def example_gps_ekf(): + """GPS 與 EKF 範例""" + print("\n\n=== GPS 與 EKF 範例 ===\n") + + vehicle = VehicleView(sysid=11) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # GPS 資訊 + autopilot.status.gps.fix_type = 3 # 3D Fix + autopilot.status.gps.satellites_visible = 12 + autopilot.status.gps.eph = 120 # HDOP = 1.2 + autopilot.status.gps.epv = 180 # VDOP = 1.8 + autopilot.status.gps.timestamp = time.time() + + print(f"GPS:") + print(f" Fix Type: {autopilot.status.gps.fix_type}") + print(f" 衛星數: {autopilot.status.gps.satellites_visible}") + print(f" HDOP: {autopilot.status.gps.eph/100}") + + # EKF 資訊 + autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK + autopilot.status.ekf.velocity_variance = 0.5 + autopilot.status.ekf.pos_horiz_variance = 1.2 + autopilot.status.ekf.pos_vert_variance = 2.0 + autopilot.status.ekf.timestamp = time.time() + + print(f"\nEKF:") + print(f" Flags: 0x{autopilot.status.ekf.flags:X}") + print(f" 速度變異: {autopilot.status.ekf.velocity_variance}") + print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}") + print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n") + + +def example_vfr_hud(): + """VFR HUD 範例""" + print("\n=== VFR HUD 範例 ===\n") + + vehicle = VehicleView(sysid=12) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # VFR HUD 資訊 + autopilot.status.vfr.airspeed = 15.5 # m/s + autopilot.status.vfr.groundspeed = 14.8 # m/s + autopilot.status.vfr.heading = 90 # 東方 + autopilot.status.vfr.throttle = 65 # % + autopilot.status.vfr.climb = 2.5 # m/s + autopilot.status.vfr.timestamp = time.time() + + print(f"VFR HUD:") + print(f" 空速: {autopilot.status.vfr.airspeed} m/s") + print(f" 地速: {autopilot.status.vfr.groundspeed} m/s") + print(f" 航向: {autopilot.status.vfr.heading}°") + print(f" 油門: {autopilot.status.vfr.throttle}%") + print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n") + + +if __name__ == "__main__": + # 執行所有範例 + # example_basic_usage() + # example_packet_tracking() + # example_parameters() + # example_rf_module() + # example_multiple_components() + # example_registry() + # example_serialization() + # example_gps_ekf() + example_vfr_hud() + + print("\n" + "="*50) + print("所有範例執行完成！") + print("="*50) diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py index 29744fc..9c3ca78 100644 --- a/src/fc_network_adapter/tests/test_mavlinkObject.py +++ b/src/fc_network_adapter/tests/test_mavlinkObject.py @@ -13,7 +13,6 @@ import time import threading import socket import asyncio -from unittest.mock import MagicMock, patch # 導入要測試的模組 from ..fc_network_adapter.mavlinkObject import ( @@ -24,62 +23,100 @@ from ..fc_network_adapter.mavlinkObject import ( return_packet_ring ) +# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式) +# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes) +HEARTBEAT_PACKET_1 = bytes([ + 0xFE, # STX (MAVLink 1.0) + 0x09, # payload length (9 bytes) + 0x00, # sequence + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1) + 0x00, 0x00, 0x00, 0x00, # custom_mode = 0 + 0x02, # type = MAV_TYPE_QUADROTOR (2) + 0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3) + 0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64) + 0x03, # system_status = MAV_STATE_STANDBY (3) + 0x03, # mavlink_version = 3 + 0x62, 0x8E # CRC (simplified placeholder) +]) + +HEARTBEAT_PACKET_2 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x01, # sequence (增加) + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x41, 0x03, 0x03, + 0x33, 0xEC +]) + +HEARTBEAT_PACKET_3 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x02, # sequence + 0x02, # system ID = 2 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x42, 0x03, 0x03, + 0x37, 0x44 +]) + class MockMavlinkSocket: - """模擬 Mavlink Socket 的類別，用於測試""" + """模擬 Mavlink Socket 的類別，用於測試 + 使用真實的 MAVLink 封包，而不是模擬的訊息對象 + """ - def __init__(self, test_data=None): + def __init__(self, test_packets=None): + """ + Args: + test_packets: list of bytes，每個元素都是完整的 MAVLink 封包 + """ self.closed = False - self.test_data = test_data or [] - self.data_index = 0 + self.test_packets = test_packets or [] + self.packet_index = 0 self.written_data = [] + # 使用 pymavlink 來解析封包 + from pymavlink import mavutil + self.mav_parser = mavutil.mavlink.MAVLink(self) + def recv_msg(self): - if not self.test_data or self.data_index >= len(self.test_data): + """返回解析後的 MAVLink 訊息對象""" + if not self.test_packets or self.packet_index >= len(self.test_packets): + return None + + packet = self.test_packets[self.packet_index] + self.packet_index += 1 + + # 使用 pymavlink 解析封包 + try: + for byte in packet: + msg = self.mav_parser.parse_char(bytes([byte])) + if msg: + return msg + except Exception as e: + print(f"Error parsing packet: {e}") return None - data = self.test_data[self.data_index] - self.data_index += 1 - return data + return None def write(self, data): + """寫入數據（用於檢查轉發）""" self.written_data.append(data) def close(self): + """關閉 socket""" self.closed = True - - -class MockMavlinkMessage: - """模擬 Mavlink 訊息的類別，用於測試""" - - def __init__(self, msg_id, sysid, compid, seq=0): - self.msg_id = msg_id - self.sysid = sysid - self.compid = compid - self.seq = seq - self.msg_buf = bytes([msg_id, sysid, compid, seq]) - - def get_msgId(self): - return self.msg_id - - def get_srcSystem(self): - return self.sysid - - def get_srcComponent(self): - return self.compid - - def get_seq(self): - return self.seq - - def get_msgbuf(self): - return self.msg_buf - - def get_type(self): - return f"MSG_ID_{self.msg_id}" class TestMavlinkObject(unittest.TestCase): - """測試 mavlink_object 類別""" + """測試 mavlink_object 類別的獨立功能""" def setUp(self): """在每個測試方法執行前準備環境""" @@ -91,35 +128,27 @@ class TestMavlinkObject(unittest.TestCase): stream_bridge_ring.clear() return_packet_ring.clear() - # 創建測試訊息 - self.heartbeat_msg = MockMavlinkMessage(msg_id=0, sysid=1, compid=1) - self.attitude_msg = MockMavlinkMessage(msg_id=30, sysid=1, compid=1) - self.position_msg = MockMavlinkMessage(msg_id=32, sysid=1, compid=1) - - # 創建模擬的 socket # 假的 Mavlink Socket - self.mock_socket = MockMavlinkSocket([ - self.heartbeat_msg, - self.attitude_msg, - self.position_msg - ]) + # 創建模擬的 socket，使用真實封包 + self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1]) # 創建測試對象 self.mavlink_obj = mavlink_object(self.mock_socket) def test_initialization(self): """測試 mavlink_object 初始化是否正確""" - # print("Testing mavlink_object initialization") self.assertEqual(self.mavlink_obj.socket_id, 0) self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT) self.assertEqual(len(self.mavlink_obj.target_sockets), 0) self.assertEqual(self.mavlink_obj.bridge_msg_types, [0]) - + self.assertEqual(self.mavlink_obj.return_msg_types, []) + def test_add_remove_target_socket(self): """測試添加和移除目標端口功能""" # 添加目標端口 self.assertTrue(self.mavlink_obj.add_target_socket(1)) self.assertEqual(len(self.mavlink_obj.target_sockets), 1) self.assertEqual(self.mavlink_obj.target_sockets[0], 1) + self.assertTrue(self.mavlink_obj.add_target_socket(2)) self.assertEqual(len(self.mavlink_obj.target_sockets), 2) self.assertIn(2, self.mavlink_obj.target_sockets) @@ -153,78 +182,31 @@ class TestMavlinkObject(unittest.TestCase): self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid")) self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"])) - def test_send_message(self): - """測試 send_message 功能""" - # 創建一個新的 mavlink_object 實例 - mock_socket = MockMavlinkSocket() - mavlink_obj = mavlink_object(mock_socket) - - # 準備測試數據 - test_message1 = b"test_message_1" - test_message2 = b"test_message_2" - - # 測試初始狀態 - self.assertEqual(len(mock_socket.written_data), 0) - + def test_send_message_validation(self): + """測試 send_message 的數據驗證功能（不需要啟動 manager）""" # 測試非運行狀態下發送消息 - self.assertFalse(mavlink_obj.send_message(test_message1)) - self.assertEqual(len(mock_socket.written_data), 0) - - # 啟動 manager - manager = async_io_manager() - manager.start() - time.sleep(0.5) # 等待事件循環啟動 - - # 添加對象到 manager - manager.add_mavlink_object(mavlink_obj) - time.sleep(0.1) # 等待對象啟動 - - # 確認對象狀態 - self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) - - # 測試發送消息 - self.assertTrue(mavlink_obj.send_message(test_message1)) - time.sleep(0.2) # 等待消息處理 + self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) - # 確認消息已發送 - self.assertEqual(len(mock_socket.written_data), 1) - self.assertEqual(mock_socket.written_data[0], test_message1) - - # 測試連續發送多條消息 - self.assertTrue(mavlink_obj.send_message(test_message2)) - time.sleep(0.2) # 等待消息處理 - - # 確認兩條消息都已發送 - self.assertEqual(len(mock_socket.written_data), 2) - self.assertEqual(mock_socket.written_data[1], test_message2) - - # 模擬發送出錯的情況 - class ErrorWriteSocket(MockMavlinkSocket): - def write(self, data): - raise Exception("Write error") - - error_socket = ErrorWriteSocket() - error_obj = mavlink_object(error_socket) - manager.add_mavlink_object(error_obj) - time.sleep(0.1) # 等待對象啟動 + # 測試無效的數據類型 + self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態 + self.assertFalse(self.mavlink_obj.send_message("invalid")) + self.assertFalse(self.mavlink_obj.send_message(123)) - # 發送消息到錯誤的 socket - self.assertTrue(error_obj.send_message(test_message1)) - time.sleep(0.2) # 等待消息處理 + # 測試太短的封包 + self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00]))) - # 即使寫入失敗，send_message 應該也返回 True - # 因為消息已成功加入到佇列中，只是後續的實際發送失敗 + # 測試無效的起始標記 + invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00]) + self.assertFalse(self.mavlink_obj.send_message(invalid_packet)) - # 停止 manager - manager.stop() - time.sleep(0.5) # 等待 manager 停止 + # 測試有效的封包可以加入佇列 + self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1) - # 測試對象已關閉後發送消息 - self.assertFalse(mavlink_obj.send_message(test_message1)) - self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 + self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態 class TestAsyncIOManager(unittest.TestCase): - """測試 async_io_manager 類別""" + """測試 async_io_manager 類別的獨立功能""" def setUp(self): """在每個測試方法執行前準備環境""" @@ -239,14 +221,9 @@ class TestAsyncIOManager(unittest.TestCase): # 創建 async_io_manager 實例 self.manager = async_io_manager() - # 模擬 mavlink 對象 - self.mock_socket1 = MockMavlinkSocket([ - MockMavlinkMessage(msg_id=0, sysid=1, compid=1), - MockMavlinkMessage(msg_id=30, sysid=1, compid=1) - ]) - self.mock_socket2 = MockMavlinkSocket([ - MockMavlinkMessage(msg_id=0, sysid=2, compid=1) - ]) + # 創建模擬 mavlink 對象，使用真實封包 + self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2]) + self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3]) self.mavlink_obj1 = mavlink_object(self.mock_socket1) self.mavlink_obj2 = mavlink_object(self.mock_socket2) @@ -254,10 +231,10 @@ class TestAsyncIOManager(unittest.TestCase): def tearDown(self): """在每個測試方法執行後清理環境""" if self.manager.running: - self.manager.stop() + self.manager.shutdown() def test_singleton_pattern(self): - """測試 async_io_manager 的單例模式 就是不會產生兩個 magager""" + """測試 async_io_manager 的單例模式""" manager1 = async_io_manager() manager2 = async_io_manager() self.assertIs(manager1, manager2) @@ -275,7 +252,7 @@ class TestAsyncIOManager(unittest.TestCase): self.assertIs(self.manager.thread, old_thread) # 停止管理器 - self.manager.stop() + self.manager.shutdown() self.assertFalse(self.manager.running) # 最多等待 5 秒讓線程結束 @@ -313,157 +290,179 @@ class TestAsyncIOManager(unittest.TestCase): self.assertFalse(self.manager.remove_mavlink_object(999)) # 停止管理器 - self.manager.stop() + self.manager.shutdown() + +class TestIntegration(unittest.TestCase): + """整合測試，測試多個 mavlink_object 之間的互動與資料流""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.shutdown() + + def test_send_message_with_manager(self): + """測試透過 async_io_manager 發送訊息的完整流程""" + # 創建一個新的 mavlink_object 實例 + mock_socket = MockMavlinkSocket() + mavlink_obj = mavlink_object(mock_socket) - def test_data_processing(self): - """測試數據處理""" + # 測試初始狀態 + self.assertEqual(len(mock_socket.written_data), 0) + + # 測試非運行狀態下發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 0) + + # 啟動 manager + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象到 manager + self.manager.add_mavlink_object(mavlink_obj) + time.sleep(0.1) # 等待對象啟動 + + # 確認對象狀態 + self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) + + # 測試發送消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + time.sleep(0.2) # 等待消息處理 + + # 確認消息已發送 + self.assertEqual(len(mock_socket.written_data), 1) + self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1) + + # 測試連續發送多條消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2)) + time.sleep(0.2) # 等待消息處理 + + # 確認兩條消息都已發送 + self.assertEqual(len(mock_socket.written_data), 2) + self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2) + + # 停止 manager + self.manager.shutdown() + time.sleep(0.5) # 等待 manager 停止 + + # 測試對象已關閉後發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 + + def test_data_processing_and_forwarding(self): + """測試數據處理與轉發流程""" # 創建用於轉發的 mavlink_objects + mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,]) mock_socket3 = MockMavlinkSocket() - mavlink_obj3 = mavlink_object(mock_socket3) - - # 設置轉發 - self.mavlink_obj1.add_target_socket(2) # socket1 轉發到 socket3 + mavlink_obj1 = mavlink_object(mock_socket1) + mavlink_obj3 = mavlink_object(mock_socket3) + # 設置訊息類型 - self.mavlink_obj1.set_bridge_message_types([0, 30]) # HEARTBEAT, ATTITUDE - # self.mavlink_obj1.enable_return(True) - self.mavlink_obj1.set_return_message_types([30]) # ATTITUDE + mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT + + # 設置轉發: obj1 -> obj3 + mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1) # 啟動管理器並添加對象 self.manager.start() time.sleep(0.5) # 等待事件循環啟動 - self.manager.add_mavlink_object(self.mavlink_obj1) + """ + 這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現 + mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包 + 若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失 + """ self.manager.add_mavlink_object(mavlink_obj3) + self.manager.add_mavlink_object(mavlink_obj1) # 等待處理完成 - time.sleep(1.0) + time.sleep(0.5) - # print("testing Mark A") - # 檢查 Ring buffer 是否有正確的數據 - self.assertEqual(stream_bridge_ring.size(), 2) # HEARTBEAT + ATTITUDE - self.assertEqual(return_packet_ring.size(), 1) # ATTITUDE + self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT - a = stream_bridge_ring.get() - print(f"stream_bridge_ring: {a}") - # 檢查是否正確轉發 - self.assertEqual(len(mock_socket3.written_data), 2) # HEARTBEAT + ATTITUDE - - # print("testing Mark B") - - # 停止管理器 - self.manager.stop() - - def test_error_handling(self): - """測試錯誤處理情況""" - print("=== mark A ===") - # 創建一個會引發異常的 socket - class ErrorSocket(MockMavlinkSocket): - def recv_msg(self): - raise Exception("Test exception") + self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT - error_socket = ErrorSocket() - mavlink_obj_err = mavlink_object(error_socket) + # 停止管理器 + self.manager.shutdown() - # 啟動管理器並添加對象 + def test_bidirectional_forwarding(self): + """測試雙向轉發""" + # 清空全局變數和 ring buffer + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建三個 mavlink 對象，模擬三個通道 + socket1 = MockMavlinkSocket() + socket2 = MockMavlinkSocket() + socket3 = MockMavlinkSocket() + + obj1 = mavlink_object(socket1) + obj2 = mavlink_object(socket2) + obj3 = mavlink_object(socket3) + + # 設置雙向轉發 + # obj1 <-> obj2 <-> obj3 + obj1.add_target_socket(1) # obj1 -> obj2 + obj2.add_target_socket(0) # obj2 -> obj1 + obj2.add_target_socket(2) # obj2 -> obj3 + obj3.add_target_socket(1) # obj3 -> obj2 + + # 啟動 async_io_manager self.manager.start() time.sleep(0.5) # 等待事件循環啟動 + + # 添加所有 mavlink_object + self.manager.add_mavlink_object(obj1) + self.manager.add_mavlink_object(obj2) + self.manager.add_mavlink_object(obj3) - self.manager.add_mavlink_object(mavlink_obj_err) - - print("=== mark B ===") - - # 等待錯誤處理 + # 對三個對象添加數據 + socket1.test_packets.append(HEARTBEAT_PACKET_1) + socket2.test_packets.append(HEARTBEAT_PACKET_2) + socket3.test_packets.append(HEARTBEAT_PACKET_3) + + # 等待處理所有訊息 time.sleep(1.0) - - # 對象應該進入錯誤狀態，但不會崩潰 - # self.assertEqual(mavlink_obj_err.state, MavlinkObjectState.ERROR) - - print("=== mark C ===") - - # 管理器應該仍在運行 - self.assertTrue(self.manager.running) - - - # 故意等一段時間 確認 socket 有被 manager 處理掉 - time.sleep(3) - + + # 檢查轉發結果 + # socket1 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket1.written_data), 1) + + # socket2 應該收到 socket1 和 socket3 的訊息 + self.assertGreaterEqual(len(socket2.written_data), 2) + + # socket3 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket3.written_data), 1) + + # 檢查 ring buffer 的數據 + # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] + self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT + # 停止管理器 - self.manager.stop() - - -class TestIntegration(unittest.TestCase): - """整合測試，測試多個 mavlink_object 之間的互動""" - - def test_bidirectional_forwarding(self): - """測試雙向轉發""" - # 清空全局變數和 ring buffer - mavlink_object.mavlinkObjects = {} - mavlink_object.socket_num = 0 - stream_bridge_ring.clear() - return_packet_ring.clear() - - # 創建三個 mavlink 對象，模擬三個通道 - socket1 = MockMavlinkSocket([ - MockMavlinkMessage(msg_id=0, sysid=1, compid=1), - MockMavlinkMessage(msg_id=30, sysid=1, compid=1) - ]) - socket2 = MockMavlinkSocket([ - MockMavlinkMessage(msg_id=0, sysid=2, compid=1), - MockMavlinkMessage(msg_id=32, sysid=2, compid=1) - ]) - socket3 = MockMavlinkSocket([ - MockMavlinkMessage(msg_id=0, sysid=3, compid=1), - MockMavlinkMessage(msg_id=33, sysid=3, compid=1) - ]) - - obj1 = mavlink_object(socket1) - obj2 = mavlink_object(socket2) - obj3 = mavlink_object(socket3) - - # 設置雙向轉發 - # obj1 <-> obj2 <-> obj3 - obj1.add_target_socket(1) # obj1 -> obj2 - obj2.add_target_socket(0) # obj2 -> obj1 - obj2.add_target_socket(2) # obj2 -> obj3 - obj3.add_target_socket(1) # obj3 -> obj2 - - # 啟動 async_io_manager - manager = async_io_manager() - manager.start() - time.sleep(0.5) # 等待事件循環啟動 - - # 添加所有 mavlink_object - manager.add_mavlink_object(obj1) - manager.add_mavlink_object(obj2) - manager.add_mavlink_object(obj3) - - # 等待處理所有訊息 - time.sleep(1.5) - - # 檢查轉發結果 - # socket1 應該收到 socket2 和 socket3 的訊息 - self.assertEqual(len(socket1.written_data), 4) # 2 from obj2, 2 from obj3 via obj2 - - # socket2 應該收到 socket1 和 socket3 的訊息 - self.assertEqual(len(socket2.written_data), 4) # 2 from obj1, 2 from obj3 - - # socket3 應該收到 socket1 和 socket2 的訊息 - self.assertEqual(len(socket3.written_data), 4) # 2 from obj1 via obj2, 2 from obj2 - - # 檢查 ring buffer 的數據 - # 假設所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] - # 應該有 3 個 HEARTBEAT 訊息 - self.assertEqual(stream_bridge_ring.size(), 3) # 3 HEARTBEAT - - # 停止管理器 - manager.stop() - - + self.manager.shutdown() if __name__ == "__main__": - unittest.main(defaultTest="TestMavlinkObject.test_send_message") - # unittest.main(defaultTest="TestAsyncIOManager") + # 可以指定要運行的測試 + # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation") + # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects") + unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding") + unittest.main() + From 7158e9548ae9e166804da417f6fc8fc913ad36fe Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 1 Dec 2025 18:14:42 +0800 Subject: [PATCH 078/146] Add external package geographic_info and angles and mavros_msgs --- .gitmodules | 6 + README.md | 31 +- src/external/angles | 1 + src/external/geographic_info | 1 + src/external/mavros_msgs/CHANGELOG.rst | 1009 +++++++++++++++++ src/external/mavros_msgs/CMakeLists.txt | 208 ++++ .../include/mavros_msgs/mavlink_convert.hpp | 145 +++ .../mavros_msgs/mavros_msgs_mapping_rule.yaml | 8 + src/external/mavros_msgs/msg/ADSBVehicle.msg | 66 ++ .../mavros_msgs/msg/ActuatorControl.msg | 16 + src/external/mavros_msgs/msg/Altitude.msg | 12 + .../mavros_msgs/msg/AttitudeTarget.msg | 17 + src/external/mavros_msgs/msg/CamIMUStamp.msg | 4 + .../mavros_msgs/msg/CameraImageCaptured.msg | 11 + .../mavros_msgs/msg/CellularStatus.msg | 9 + src/external/mavros_msgs/msg/CommandCode.msg | 200 ++++ .../msg/CompanionProcessStatus.msg | 19 + src/external/mavros_msgs/msg/DebugValue.msg | 26 + src/external/mavros_msgs/msg/ESCInfo.msg | 14 + src/external/mavros_msgs/msg/ESCInfoItem.msg | 12 + src/external/mavros_msgs/msg/ESCStatus.msg | 9 + .../mavros_msgs/msg/ESCStatusItem.msg | 11 + src/external/mavros_msgs/msg/ESCTelemetry.msg | 10 + .../mavros_msgs/msg/ESCTelemetryItem.msg | 15 + .../mavros_msgs/msg/EstimatorStatus.msg | 23 + .../mavros_msgs/msg/ExtendedState.msg | 19 + src/external/mavros_msgs/msg/FileEntry.msg | 12 + src/external/mavros_msgs/msg/GPSINPUT.msg | 37 + src/external/mavros_msgs/msg/GPSRAW.msg | 37 + src/external/mavros_msgs/msg/GPSRTK.msg | 18 + .../msg/GimbalDeviceAttitudeStatus.msg | 32 + .../msg/GimbalDeviceInformation.msg | 34 + .../msg/GimbalDeviceSetAttitude.msg | 18 + .../msg/GimbalManagerInformation.msg | 29 + .../msg/GimbalManagerSetAttitude.msg | 24 + .../msg/GimbalManagerSetPitchyaw.msg | 27 + .../mavros_msgs/msg/GimbalManagerStatus.msg | 19 + .../mavros_msgs/msg/GlobalPositionTarget.msg | 34 + .../mavros_msgs/msg/HilActuatorControls.msg | 10 + src/external/mavros_msgs/msg/HilControls.msg | 18 + src/external/mavros_msgs/msg/HilGPS.msg | 17 + src/external/mavros_msgs/msg/HilSensor.msg | 16 + .../mavros_msgs/msg/HilStateQuaternion.msg | 15 + src/external/mavros_msgs/msg/HomePosition.msg | 10 + .../mavros_msgs/msg/LandingTarget.msg | 32 + src/external/mavros_msgs/msg/LogData.msg | 11 + src/external/mavros_msgs/msg/LogEntry.msg | 15 + .../mavros_msgs/msg/MagnetometerReporter.msg | 4 + .../mavros_msgs/msg/ManualControl.msg | 7 + src/external/mavros_msgs/msg/Mavlink.msg | 38 + src/external/mavros_msgs/msg/MountControl.msg | 18 + .../mavros_msgs/msg/NavControllerOutput.msg | 12 + .../mavros_msgs/msg/OnboardComputerStatus.msg | 25 + src/external/mavros_msgs/msg/OpticalFlow.msg | 9 + .../mavros_msgs/msg/OpticalFlowRad.msg | 14 + src/external/mavros_msgs/msg/OverrideRCIn.msg | 9 + src/external/mavros_msgs/msg/Param.msg | 11 + src/external/mavros_msgs/msg/ParamEvent.msg | 14 + src/external/mavros_msgs/msg/ParamValue.msg | 12 + src/external/mavros_msgs/msg/PlayTuneV2.msg | 10 + .../mavros_msgs/msg/PositionTarget.msg | 32 + src/external/mavros_msgs/msg/RCIn.msg | 5 + src/external/mavros_msgs/msg/RCOut.msg | 4 + src/external/mavros_msgs/msg/RTCM.msg | 6 + src/external/mavros_msgs/msg/RTKBaseline.msg | 23 + src/external/mavros_msgs/msg/RadioStatus.msg | 16 + src/external/mavros_msgs/msg/State.msg | 82 ++ src/external/mavros_msgs/msg/StatusEvent.msg | 19 + src/external/mavros_msgs/msg/StatusText.msg | 17 + src/external/mavros_msgs/msg/SysStatus.msg | 15 + .../mavros_msgs/msg/TerrainReport.msg | 12 + src/external/mavros_msgs/msg/Thrust.msg | 5 + .../mavros_msgs/msg/TimesyncStatus.msg | 7 + src/external/mavros_msgs/msg/Trajectory.msg | 19 + src/external/mavros_msgs/msg/Tunnel.msg | 27 + src/external/mavros_msgs/msg/VehicleInfo.msg | 31 + src/external/mavros_msgs/msg/VfrHud.msg | 11 + src/external/mavros_msgs/msg/Vibration.msg | 7 + src/external/mavros_msgs/msg/Waypoint.msg | 45 + src/external/mavros_msgs/msg/WaypointList.msg | 9 + .../mavros_msgs/msg/WaypointReached.msg | 7 + .../mavros_msgs/msg/WheelOdomStamped.msg | 6 + src/external/mavros_msgs/package.xml | 46 + src/external/mavros_msgs/srv/CommandAck.srv | 11 + src/external/mavros_msgs/srv/CommandBool.srv | 6 + src/external/mavros_msgs/srv/CommandHome.srv | 10 + src/external/mavros_msgs/srv/CommandInt.srv | 19 + src/external/mavros_msgs/srv/CommandLong.srv | 17 + src/external/mavros_msgs/srv/CommandTOL.srv | 10 + .../mavros_msgs/srv/CommandTOLLocal.srv | 10 + .../mavros_msgs/srv/CommandTriggerControl.srv | 8 + .../srv/CommandTriggerInterval.srv | 7 + .../mavros_msgs/srv/CommandVtolTransition.srv | 16 + src/external/mavros_msgs/srv/EndpointAdd.srv | 14 + src/external/mavros_msgs/srv/EndpointDel.srv | 17 + src/external/mavros_msgs/srv/FileChecksum.srv | 12 + src/external/mavros_msgs/srv/FileClose.srv | 10 + src/external/mavros_msgs/srv/FileList.srv | 10 + src/external/mavros_msgs/srv/FileMakeDir.srv | 9 + src/external/mavros_msgs/srv/FileOpen.srv | 17 + src/external/mavros_msgs/srv/FileRead.srv | 13 + src/external/mavros_msgs/srv/FileRemove.srv | 9 + .../mavros_msgs/srv/FileRemoveDir.srv | 9 + src/external/mavros_msgs/srv/FileRename.srv | 10 + src/external/mavros_msgs/srv/FileTruncate.srv | 10 + src/external/mavros_msgs/srv/FileWrite.srv | 12 + .../mavros_msgs/srv/GimbalGetInformation.srv | 10 + .../srv/GimbalManagerCameraTrack.srv | 28 + .../srv/GimbalManagerConfigure.srv | 32 + .../mavros_msgs/srv/GimbalManagerPitchyaw.srv | 27 + .../mavros_msgs/srv/GimbalManagerSetRoi.srv | 38 + .../mavros_msgs/srv/LogRequestData.srv | 11 + .../mavros_msgs/srv/LogRequestEnd.srv | 4 + .../mavros_msgs/srv/LogRequestList.srv | 9 + .../mavros_msgs/srv/MessageInterval.srv | 7 + .../mavros_msgs/srv/MountConfigure.srv | 28 + src/external/mavros_msgs/srv/ParamGet.srv | 8 + src/external/mavros_msgs/srv/ParamPull.srv | 8 + src/external/mavros_msgs/srv/ParamPush.srv | 9 + src/external/mavros_msgs/srv/ParamSet.srv | 9 + src/external/mavros_msgs/srv/ParamSetV2.srv | 14 + src/external/mavros_msgs/srv/SetMavFrame.srv | 36 + src/external/mavros_msgs/srv/SetMode.srv | 22 + src/external/mavros_msgs/srv/StreamRate.srv | 17 + .../mavros_msgs/srv/VehicleInfoGet.srv | 14 + .../mavros_msgs/srv/WaypointClear.srv | 4 + src/external/mavros_msgs/srv/WaypointPull.srv | 7 + src/external/mavros_msgs/srv/WaypointPush.srv | 11 + .../mavros_msgs/srv/WaypointSetCurrent.srv | 7 + 129 files changed, 3622 insertions(+), 5 deletions(-) create mode 100644 .gitmodules create mode 160000 src/external/angles create mode 160000 src/external/geographic_info create mode 100644 src/external/mavros_msgs/CHANGELOG.rst create mode 100644 src/external/mavros_msgs/CMakeLists.txt create mode 100644 src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp create mode 100644 src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml create mode 100644 src/external/mavros_msgs/msg/ADSBVehicle.msg create mode 100644 src/external/mavros_msgs/msg/ActuatorControl.msg create mode 100644 src/external/mavros_msgs/msg/Altitude.msg create mode 100644 src/external/mavros_msgs/msg/AttitudeTarget.msg create mode 100644 src/external/mavros_msgs/msg/CamIMUStamp.msg create mode 100644 src/external/mavros_msgs/msg/CameraImageCaptured.msg create mode 100644 src/external/mavros_msgs/msg/CellularStatus.msg create mode 100644 src/external/mavros_msgs/msg/CommandCode.msg create mode 100644 src/external/mavros_msgs/msg/CompanionProcessStatus.msg create mode 100644 src/external/mavros_msgs/msg/DebugValue.msg create mode 100644 src/external/mavros_msgs/msg/ESCInfo.msg create mode 100644 src/external/mavros_msgs/msg/ESCInfoItem.msg create mode 100644 src/external/mavros_msgs/msg/ESCStatus.msg create mode 100644 src/external/mavros_msgs/msg/ESCStatusItem.msg create mode 100644 src/external/mavros_msgs/msg/ESCTelemetry.msg create mode 100644 src/external/mavros_msgs/msg/ESCTelemetryItem.msg create mode 100644 src/external/mavros_msgs/msg/EstimatorStatus.msg create mode 100644 src/external/mavros_msgs/msg/ExtendedState.msg create mode 100644 src/external/mavros_msgs/msg/FileEntry.msg create mode 100644 src/external/mavros_msgs/msg/GPSINPUT.msg create mode 100644 src/external/mavros_msgs/msg/GPSRAW.msg create mode 100644 src/external/mavros_msgs/msg/GPSRTK.msg create mode 100644 src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg create mode 100644 src/external/mavros_msgs/msg/GimbalDeviceInformation.msg create mode 100644 src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg create mode 100644 src/external/mavros_msgs/msg/GimbalManagerInformation.msg create mode 100644 src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg create mode 100644 src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg create mode 100644 src/external/mavros_msgs/msg/GimbalManagerStatus.msg create mode 100644 src/external/mavros_msgs/msg/GlobalPositionTarget.msg create mode 100644 src/external/mavros_msgs/msg/HilActuatorControls.msg create mode 100644 src/external/mavros_msgs/msg/HilControls.msg create mode 100644 src/external/mavros_msgs/msg/HilGPS.msg create mode 100644 src/external/mavros_msgs/msg/HilSensor.msg create mode 100644 src/external/mavros_msgs/msg/HilStateQuaternion.msg create mode 100644 src/external/mavros_msgs/msg/HomePosition.msg create mode 100644 src/external/mavros_msgs/msg/LandingTarget.msg create mode 100644 src/external/mavros_msgs/msg/LogData.msg create mode 100644 src/external/mavros_msgs/msg/LogEntry.msg create mode 100644 src/external/mavros_msgs/msg/MagnetometerReporter.msg create mode 100644 src/external/mavros_msgs/msg/ManualControl.msg create mode 100644 src/external/mavros_msgs/msg/Mavlink.msg create mode 100644 src/external/mavros_msgs/msg/MountControl.msg create mode 100644 src/external/mavros_msgs/msg/NavControllerOutput.msg create mode 100644 src/external/mavros_msgs/msg/OnboardComputerStatus.msg create mode 100644 src/external/mavros_msgs/msg/OpticalFlow.msg create mode 100644 src/external/mavros_msgs/msg/OpticalFlowRad.msg create mode 100644 src/external/mavros_msgs/msg/OverrideRCIn.msg create mode 100644 src/external/mavros_msgs/msg/Param.msg create mode 100644 src/external/mavros_msgs/msg/ParamEvent.msg create mode 100644 src/external/mavros_msgs/msg/ParamValue.msg create mode 100644 src/external/mavros_msgs/msg/PlayTuneV2.msg create mode 100644 src/external/mavros_msgs/msg/PositionTarget.msg create mode 100644 src/external/mavros_msgs/msg/RCIn.msg create mode 100644 src/external/mavros_msgs/msg/RCOut.msg create mode 100644 src/external/mavros_msgs/msg/RTCM.msg create mode 100644 src/external/mavros_msgs/msg/RTKBaseline.msg create mode 100644 src/external/mavros_msgs/msg/RadioStatus.msg create mode 100644 src/external/mavros_msgs/msg/State.msg create mode 100644 src/external/mavros_msgs/msg/StatusEvent.msg create mode 100644 src/external/mavros_msgs/msg/StatusText.msg create mode 100644 src/external/mavros_msgs/msg/SysStatus.msg create mode 100644 src/external/mavros_msgs/msg/TerrainReport.msg create mode 100644 src/external/mavros_msgs/msg/Thrust.msg create mode 100644 src/external/mavros_msgs/msg/TimesyncStatus.msg create mode 100644 src/external/mavros_msgs/msg/Trajectory.msg create mode 100644 src/external/mavros_msgs/msg/Tunnel.msg create mode 100644 src/external/mavros_msgs/msg/VehicleInfo.msg create mode 100644 src/external/mavros_msgs/msg/VfrHud.msg create mode 100644 src/external/mavros_msgs/msg/Vibration.msg create mode 100644 src/external/mavros_msgs/msg/Waypoint.msg create mode 100644 src/external/mavros_msgs/msg/WaypointList.msg create mode 100644 src/external/mavros_msgs/msg/WaypointReached.msg create mode 100644 src/external/mavros_msgs/msg/WheelOdomStamped.msg create mode 100644 src/external/mavros_msgs/package.xml create mode 100644 src/external/mavros_msgs/srv/CommandAck.srv create mode 100644 src/external/mavros_msgs/srv/CommandBool.srv create mode 100644 src/external/mavros_msgs/srv/CommandHome.srv create mode 100644 src/external/mavros_msgs/srv/CommandInt.srv create mode 100644 src/external/mavros_msgs/srv/CommandLong.srv create mode 100644 src/external/mavros_msgs/srv/CommandTOL.srv create mode 100644 src/external/mavros_msgs/srv/CommandTOLLocal.srv create mode 100644 src/external/mavros_msgs/srv/CommandTriggerControl.srv create mode 100644 src/external/mavros_msgs/srv/CommandTriggerInterval.srv create mode 100644 src/external/mavros_msgs/srv/CommandVtolTransition.srv create mode 100644 src/external/mavros_msgs/srv/EndpointAdd.srv create mode 100644 src/external/mavros_msgs/srv/EndpointDel.srv create mode 100644 src/external/mavros_msgs/srv/FileChecksum.srv create mode 100644 src/external/mavros_msgs/srv/FileClose.srv create mode 100644 src/external/mavros_msgs/srv/FileList.srv create mode 100644 src/external/mavros_msgs/srv/FileMakeDir.srv create mode 100644 src/external/mavros_msgs/srv/FileOpen.srv create mode 100644 src/external/mavros_msgs/srv/FileRead.srv create mode 100644 src/external/mavros_msgs/srv/FileRemove.srv create mode 100644 src/external/mavros_msgs/srv/FileRemoveDir.srv create mode 100644 src/external/mavros_msgs/srv/FileRename.srv create mode 100644 src/external/mavros_msgs/srv/FileTruncate.srv create mode 100644 src/external/mavros_msgs/srv/FileWrite.srv create mode 100644 src/external/mavros_msgs/srv/GimbalGetInformation.srv create mode 100644 src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv create mode 100644 src/external/mavros_msgs/srv/GimbalManagerConfigure.srv create mode 100644 src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv create mode 100644 src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv create mode 100644 src/external/mavros_msgs/srv/LogRequestData.srv create mode 100644 src/external/mavros_msgs/srv/LogRequestEnd.srv create mode 100644 src/external/mavros_msgs/srv/LogRequestList.srv create mode 100644 src/external/mavros_msgs/srv/MessageInterval.srv create mode 100644 src/external/mavros_msgs/srv/MountConfigure.srv create mode 100644 src/external/mavros_msgs/srv/ParamGet.srv create mode 100644 src/external/mavros_msgs/srv/ParamPull.srv create mode 100644 src/external/mavros_msgs/srv/ParamPush.srv create mode 100644 src/external/mavros_msgs/srv/ParamSet.srv create mode 100644 src/external/mavros_msgs/srv/ParamSetV2.srv create mode 100644 src/external/mavros_msgs/srv/SetMavFrame.srv create mode 100644 src/external/mavros_msgs/srv/SetMode.srv create mode 100644 src/external/mavros_msgs/srv/StreamRate.srv create mode 100644 src/external/mavros_msgs/srv/VehicleInfoGet.srv create mode 100644 src/external/mavros_msgs/srv/WaypointClear.srv create mode 100644 src/external/mavros_msgs/srv/WaypointPull.srv create mode 100644 src/external/mavros_msgs/srv/WaypointPush.srv create mode 100644 src/external/mavros_msgs/srv/WaypointSetCurrent.srv diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..6630485 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "src/external/geographic_info"] + path = src/external/geographic_info + url = https://github.com/ros-geographic-info/geographic_info.git +[submodule "src/external/angles"] + path = src/external/angles + url = https://github.com/ros/angles.git diff --git a/README.md b/README.md index 490d84b..4f9dff7 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,23 @@ ROS2 1. Gazebo Garden 2. Ardupilot +=== +依賴的 ROS 庫 +1. https://github.com/ros-geographic-info/geographic_info.git +2. https://github.com/ros/angles.git +3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的 + +Clone 專案後 請先執行這些指令 +```bash +# 1.同步 submodule +cd ~/AirTrapMine +git submodule init +git submodule update +# 2. build 需要的 package +colcon build --packages-select angles geographic_msgs mavros_msgs +``` + + === Package 簡述 1. unitdev 為各自協作者做開發時的測試區 @@ -39,10 +56,14 @@ Package 簡述 N. logs 是執行時期的記錄檔 === -請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式 +主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式 例如 在 ~/AirTrapMine/src/ 資料夾下 -> python -m fc_network_adapter.fc_network_adapter.mainOrchestrator -> python -m fc_network_adapter.tests.test_ringBuffer -> python -m fc_network_adapter.tests.demo_integration - +```bash +# 記得先開啟 依賴 Package 到 overlay +. ./install/local_setup.bash +# 範例 +python -m fc_network_adapter.fc_network_adapter.mainOrchestrator +python -m fc_network_adapter.tests.test_ringBuffer +python -m fc_network_adapter.tests.demo_integration +``` diff --git a/src/external/angles b/src/external/angles new file mode 160000 index 0000000..a96224f --- /dev/null +++ b/src/external/angles @@ -0,0 +1 @@ +Subproject commit a96224f9ab3ac51fe8fd981c1e1554528dc4345a diff --git a/src/external/geographic_info b/src/external/geographic_info new file mode 160000 index 0000000..bc73c05 --- /dev/null +++ b/src/external/geographic_info @@ -0,0 +1 @@ +Subproject commit bc73c05ee79c31a88b4a23b545a2fe55eae8089e diff --git a/src/external/mavros_msgs/CHANGELOG.rst b/src/external/mavros_msgs/CHANGELOG.rst new file mode 100644 index 0000000..3cbecbe --- /dev/null +++ b/src/external/mavros_msgs/CHANGELOG.rst @@ -0,0 +1,1009 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mavros_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.9.0 (2024-10-10) +------------------ + +2.8.0 (2024-06-07) +------------------ +* regenerate all using cogall.sh +* Merge branch 'master' into ros2 + * master: + 1.19.0 + update changelog + gps_global_origin: remove LLA to ECEF conversion +* 1.19.0 +* update changelog +* removed prefix in enums in messages and changed to use existing functions for string and quaternion convert +* Final touches + Added functionality that was overlooked for camera tracking if supported, added copyright info, added custom exception thrown when mode enumerator is not understood +* Added gimbal_control plugin + Added all functionality to support a plugin to enable compatibility with MAVLink Gimbal Protocol v2 +* Contributors: Frederik Mazur Andersen, Mark-Beaty, Vladimir Ermakov + +1.19.0 (2024-06-06) +------------------- + +2.7.0 (2024-03-03) +------------------ +* re-generate with cogall.sh +* Merge branch 'master' into ros2 + * master: + 1.18.0 + update changelog + sys_status.cpp: improve timeout code + sys_status.cpp: Add a SYS_STATUS message publisher + [camera plugin] Fix image_index and capture_result not properly filled + Fix missing semi-colon + GPS_STATUS Plugin: Fill in available messages for ROS1 legacy +* 1.18.0 +* update changelog +* sys_status.cpp: Add a SYS_STATUS message publisher +* cog checksum +* remove event_time_boot_ms, fill stamp instead +* handle events +* Fix errata in GPSRAW.msg +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Seunghwan Jo, Vladimir Ermakov, victor + +1.18.0 (2024-03-03) +------------------- +* sys_status.cpp: Add a SYS_STATUS message publisher +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas + +2.6.0 (2023-09-09) +------------------ +* msgs: move generator code +* cog: regenerate all +* Merge branch 'master' into ros2 + * master: + 1.17.0 + update changelog + cog: regenerate all + Bugfix/update map origin with home position (`#1892 `_) + mavros: Remove extra ';' + mavros_extras: Fix some init order warnings + Suppress warnings from included headers + 1.16.0 + update changelog + made it such that the gp_origin topic published latched. + use hpp instead of deprecated .h pluginlib headers +* 1.17.0 +* update changelog +* cog: regenerate all +* local takeoff and land topics (`#1890 `_) + * local takeoff and land topics + * vector3 position type, rename to TOLLocal + * remove auto include line +* Merge pull request `#1871 `_ from Vladislavert/feature/optical_flow_msg + Addition of New OpticalFlow.msg +* Added geometry_msgs/Vector3 to OpticalFlow.msg +* Added vectors to the message OpticalFlow.msg +* Added message optical flow +* 1.16.0 +* update changelog +* Contributors: Ido Guzi, Vladimir Ermakov, Vladislavert + +2.5.0 (2023-05-05) +------------------ + +2.4.0 (2022-12-30) +------------------ +* msgs: re-generate +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Contributors: Sverre Velten Rothmund, Vladimir Ermakov + +2.3.0 (2022-09-24) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* Contributors: Vladimir Ermakov + +2.2.0 (2022-06-27) +------------------ +* Merge branch 'master' into ros2 + * master: + mount_control.cpp: detect MOUNT_ORIENTATION stale messages + ESCTelemetryItem.msg: correct RPM units + apm_config.yaml: add mount configuration + sys_status.cpp fix free memory for values > 64KiB + uncrustify cellular_status.cpp + Add CellularStatus plugin and message + *_config.yaml: document usage of multiple batteries diagnostics + sys_status.cpp: fix compilation + sys_status.cpp: support diagnostics on up-to 10 batteries + sys_status.cpp: do not use harcoded constants + sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics + sys_status.cpp: fix enabling of mem_diag and hwst_diag + sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix). + sys_status.cpp: ignore sys_status mavlink messages from gimbals + mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add() + sys_status.cpp: remove deprecated BATTERY2 mavlink message support + Mount control plugin: add configurable diagnostics + Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() + Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. + Disable startup_px4_usb_quirk in px4_config.yaml +* msgs: support humble + +2.1.1 (2022-03-02) +------------------ + +2.1.0 (2022-02-02) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* re-generate all coglets +* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames + Add extra MAV_FRAMES to waypoint message +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html +* Add plugin for reporting terrain height estimate from FCU +* 1.12.2 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* Contributors: Vladimir Ermakov, matt + +2.0.5 (2021-11-28) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* extras: fix some more lint warns +* msgs: update conversion header +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* msgs: use pragmas to ignore unaligned pointer warnings +* msgs: fix convert const +* msgs: try to hide 'unaligned pointer' warning +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* Merge branch 'master' into ros2 + * master: + msgs: add yaw field to GPS_INPUT +* msgs: add yaw field to GPS_INPUT +* Contributors: Jaeyoung-Lim, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* Merge branch 'master' into ros2 + * master: + msgs: update gpsraw to have yaw field +* msgs: update gpsraw to have yaw field +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel.msg: Generate enum with cog +* mavros_msgs: Add Tunnel message +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* MountControl.msg: fix copy-paste +* 1.9.0 +* update changelog +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Added GPS_INPUT plugin +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* msgs: re-generate file lists +* Merge branch 'master' into ros2 + * master: + extras: esc_telemetry: fix build + extras: fix esc_telemetry centi-volt/amp conversion + extras: uncrustify all plugins + plugins: reformat xml + extras: reformat plugins xml + extras: fix apm esc_telemetry + msgs: fix types for apm's esc telemetry + actually allocate memory for the telemetry information + fixed some compile errors + added esc_telemetry plugin + Reset calibration flag when re-calibrating. Prevent wrong data output. + Exclude changes to launch files. + Delete debug files. + Apply uncrustify changes. + Set progress array to global to prevent erasing data. + Move Compass calibration report to extras. Rewrite code based on instructions. + Remove extra message from CMakeLists. + Add message and service definition. + Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* msgs: fix types for apm's esc telemetry +* actually allocate memory for the telemetry information +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Remove extra message from CMakeLists. +* Add message and service definition. +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Ricardo Marques, Russell, Vladimir Ermakov + +2.0.3 (2021-06-20) +------------------ + +2.0.2 (2021-06-20) +------------------ + +2.0.1 (2021-06-06) +------------------ +* Add rcl_interfaces dependency +* Merge branch 'master' into ros2 + * master: + readme: update + 1.8.0 + update changelog + Create semgrep-analysis.yml + Create codeql-analysis.yml +* 1.8.0 +* update changelog +* Contributors: Rob Clarke, Vladimir Ermakov + +2.0.0 (2021-05-28) +------------------ +* msgs: update command codes +* msgs: update param services +* plugins: setpoint_velocity: port to ros2 +* Merge branch 'master' into ros2 + * master: + 1.7.1 + update changelog + re-generate all pymavlink enums + 1.7.0 + update changelog +* mavros: generate plugin list +* Merge branch 'master' into ros2 + * master: + msgs: re-generate the code + lib: re-generate the code + plugins: mission: re-generate the code + MissionBase: correction to file information + MissionBase: add copyright from origional waypoint.cpp + uncrustify + whitespace + add rallypoint and geofence plugins to mavros plugins xml + add rallypoint and geofence plugins to CMakeList + Geofence: add geofence plugin + Rallypoint: add rallypoint plugin + Waypoint: inherit MissionBase class for mission protocol + MissionBase: breakout mission protocol from waypoint.cpp + README: Update PX4 Autopilot references + Fix https://github.com/mavlink/mavros/issues/849 +* router: catch DeviceError +* router: weak_ptr segfaults, replace with shared_ptr +* router: implement params handler +* mavros: router decl done +* lib: port enum_to_string +* lib: update sensor_orientation +* msgs: add linter +* libmavconn: start porintg, will use plain asio, without boost +* msgs: remove redundant dependency which result in colcon warning +* msgs: cogify file lists +* Merge pull request `#1186 `_ from PickNikRobotics/ros2 + mavros_msgs Ros2 +* Merge branch 'ros2' into ros2 +* msgs: start porting to ROS2 +* fixing cmakelists +* updating msg and srv list +* reenable VfrHud once renamed to match ROS2 conventions + add ros1_bridge mapping rule for renamed VfrHud message +* make mavro_msgs compile in ROS 2 +* Contributors: Mikael Arguedas, Mike Lautman, Vladimir Ermakov + +1.17.0 (2023-09-09) +------------------- +* cog: regenerate all +* Contributors: Vladimir Ermakov + +1.16.0 (2023-05-05) +------------------- + +1.15.0 (2022-12-30) +------------------- +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Contributors: Sverre Velten Rothmund, Vladimir Ermakov + +1.14.0 (2022-09-24) +------------------- +* Merge pull request `#1742 `_ from amilcarlucas/correct_rpm_units + ESCTelemetryItem.msg: correct RPM units +* ESCTelemetryItem.msg: correct RPM units +* Merge pull request `#1727 `_ from BV-OpenSource/pr-cellular-status + Pr cellular status +* Add CellularStatus plugin and message +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Vladimir Ermakov + +1.13.0 (2022-01-13) +------------------- +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* re-generate all coglets +* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames + Add extra MAV_FRAMES to waypoint message +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html +* Add plugin for reporting terrain height estimate from FCU +* Contributors: Vladimir Ermakov, matt + +1.12.2 (2021-12-12) +------------------- + +1.12.1 (2021-11-29) +------------------- + +1.12.0 (2021-11-27) +------------------- + +1.11.1 (2021-11-24) +------------------- + +1.11.0 (2021-11-24) +------------------- +* msgs: use pragmas to ignore unaligned pointer warnings +* msgs: fix convert const +* msgs: try to hide 'unaligned pointer' warning +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* msgs: add yaw field to GPS_INPUT +* Contributors: Jaeyoung-Lim, Vladimir Ermakov + +1.10.0 (2021-11-04) +------------------- +* msgs: update gpsraw to have yaw field +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel.msg: Generate enum with cog +* mavros_msgs: Add Tunnel message +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* MountControl.msg: fix copy-paste +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Morten Fyhn Amundsen, Vladimir Ermakov + +1.9.0 (2021-09-09) +------------------ +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Added GPS_INPUT plugin +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* msgs: fix types for apm's esc telemetry +* actually allocate memory for the telemetry information +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Remove extra message from CMakeLists. +* Add message and service definition. +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Ricardo Marques, Russell, Vladimir Ermakov + +1.8.0 (2021-05-05) +------------------ + +1.7.1 (2021-04-05) +------------------ +* re-generate all pymavlink enums +* Contributors: Vladimir Ermakov + +1.7.0 (2021-04-05) +------------------ +* msgs: re-generate the code +* Contributors: Vladimir Ermakov + +1.6.0 (2021-02-15) +------------------ + +1.5.2 (2021-02-02) +------------------ + +1.5.1 (2021-01-04) +------------------ + +1.5.0 (2020-11-11) +------------------ +* mavros_msgs/VehicleInfo: Add flight_custom_version field + Mirroring the field in the corresponding MAVLink message. +* mavros_msgs/State: Fix PX4 flight mode constants + Turns out ROS message string literals don't need quotes, + so adding quotes creates strings including the quotes. +* mavros_msgs/State: Add flight mode constants +* mavros_msgs: Don't move temporary objects +* Contributors: Morten Fyhn Amundsen + +1.4.0 (2020-09-11) +------------------ +* play_tune: Assign tune format directly +* play_tune: Write new plugin +* Contributors: Morten Fyhn Amundsen + +1.3.0 (2020-08-08) +------------------ +* Add esc_status plugin. +* Add gps_status plugin to publish GPS_RAW and GPS_RTK messages from FCU. + The timestamps for the gps_status topics take into account the mavlink time and uses the convienence function +* adding support for publishing rtkbaseline msgs over ROS +* Contributors: CSCE439, Dr.-Ing. Amilcar do Carmo Lucas, Ricardo Marques + +1.2.0 (2020-05-22) +------------------ +* add yaw to CMD_DO_SET_HOME +* Contributors: David Jablonski + +1.1.0 (2020-04-04) +------------------ + +1.0.0 (2020-01-01) +------------------ + +0.33.4 (2019-12-12) +------------------- +* Splitted the message fields. +* Updated esimator status msg according to the new cog based definition of estimator status. +* Added comments to msg. +* Added new line char at end of message. +* Added a publisher for estimator status message received from mavlink in sys_status. +* Contributors: saifullah3396 + +0.33.3 (2019-11-13) +------------------- + +0.33.2 (2019-11-13) +------------------- + +0.33.1 (2019-11-11) +------------------- +* resolved merge conflict +* Contributors: David Jablonski + +0.33.0 (2019-10-10) +------------------- +* Add vtol transition service +* Apply comments +* Add mount configure service message +* cog: Update all generated code +* added manual flag to mavros/state +* use header.stamp to fill mavlink msg field time_usec +* use cog for copy +* adapt message and plugin after mavlink message merge +* rename message and adjust fields +* add component id to mavros message to distinguish ROS msgs from different systems +* component_status message and plugin draft +* Contributors: David Jablonski, Jaeyoung-Lim, Vladimir Ermakov, baumanta + +0.32.2 (2019-09-09) +------------------- + +0.32.1 (2019-08-08) +------------------- + +0.32.0 (2019-07-06) +------------------- +* add mav_cmd associated with each point in trajectory plugin +* Use MountControl Msg +* Define new MountControl.msg +* Contributors: Jaeyoung-Lim, Martina Rivizzigno + +0.31.0 (2019-06-07) +------------------- +* mavros_msgs: LandingTarget: update msg description link +* extras: landing target: improve usability and flexibility +* Contributors: TSC21 + +0.30.0 (2019-05-20) +------------------- + +0.29.2 (2019-03-06) +------------------- + +0.29.1 (2019-03-03) +------------------- +* All: catkin lint files +* mavros_msgs: Fix line endings for OpticalFlowRad message +* Contributors: Pierre Kancir, sfalexrog + +0.29.0 (2019-02-02) +------------------- +* Fix broken documentation URLs +* Merge branch 'master' into param-timeout +* mavros_extras: Wheel odometry plugin updated according to the final mavlink WHEEL_DISTANCE message. +* mavros_msgs: Float32ArrayStamped replaced by WheelOdomStamped. +* mavros_msgs: Float32ArrayStamped message added. + For streaming timestamped data from FCU sensors (RPM, WHEEL_DISTANCE, etc.) +* msgs: Fix message id type, mavlink v2 uses 24 bit msg ids +* mavros_msgs: add MessageInterval.srv to CMakeLists +* sys_status: add set_message_interval service +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Pavlo Kolomiiets, Randy Mackay, Vladimir Ermakov + +0.28.0 (2019-01-03) +------------------- +* plugin:param: publish new param value +* Merge pull request `#1148 `_ from Kiwa21/pr-param-value + param plugin : add msg and publisher to catch latest param value +* msgs: update Header +* sys_state: Small cleanup of `#1150 `_ +* VehicleInfo : add srv into sys_status plugin to request basic info from vehicle +* mavros_msgs/msg/LogData.msg: Define "offset" field to be of type uint32 +* param plugin : add msg and publisher to catch latest param value +* style clean up +* Use component_id to determine message sender +* change message name from COMPANION_STATUS to COMPANION_PROCESS_STATUS +* change message to include pid +* Change from specific avoidance status message to a more generic companion status message +* Add message for avoidance status +* Contributors: Gregoire Linard, Vladimir Ermakov, baumanta, mlvov + +0.27.0 (2018-11-12) +------------------- +* Add service to send mavlink TRIGG_INTERVAL commands + Adapt trigger_control service to current mavlink cmd spec. Add a new service to change trigger interval and integration time +* Contributors: Moritz Zimmermann + +0.26.3 (2018-08-21) +------------------- +* fixup! 5a4344a2dcedc157f93b620cebd2e0b273ec24be +* mavros_msgs: Add msg and srv files related to log transfer +* Contributors: mlvov + +0.26.2 (2018-08-08) +------------------- +* Updating the gps_rtk plugin to fit mavros guidelines: + - Updating max_frag_len to allow changes in size in MAVLink seamlessly + - Using std::copy instead of memset + - Zero fill with std::fill + - Preapply the sequence flags + - Use of std iterators + - Add the maximal data size in the mavros_msgs +* Renaming the GPS RTK module, Adding fragmentation, Changing the RTCM message +* RTK Plugin; to forward RTCM messages + Signed-off-by: Alexis Paques +* Contributors: Alexis Paques + +0.26.1 (2018-07-19) +------------------- + +0.26.0 (2018-06-06) +------------------- +* mavros_msgs : add timesync status message +* Contributors: Mohammed Kabir + +0.25.1 (2018-05-14) +------------------- + +0.25.0 (2018-05-11) +------------------- +* trajectory: add time_horizon field +* change message name from ObstacleAvoidance to Trajectory since it is + general enough to support any type of trajectory +* CMakeLists: add ObstacleAvoidance message +* add ObstacleAvoidance message +* msgs: Update message doc link +* CommandCode: update list of available commands on MAV_CMD enum (`#995 `_) +* Contributors: Martina, Nuno Marques, Vladimir Ermakov + +0.24.0 (2018-04-05) +------------------- +* Add ability to send STATUSTEXT messages +* Contributors: Anass Al + +0.23.3 (2018-03-09) +------------------- + +0.23.2 (2018-03-07) +------------------- + +0.23.1 (2018-02-27) +------------------- + +0.23.0 (2018-02-03) +------------------- + +0.22.0 (2017-12-11) +------------------- +* SetMavFrame.srv: add FRAME\_ prefix +* Add cog for SetMavFrame.srv +* Setpoints: add service to specify frame +* Contributors: Pierre Kancir, khancyr + +0.21.5 (2017-11-16) +------------------- + +0.21.4 (2017-11-01) +------------------- + +0.21.3 (2017-10-28) +------------------- +* plugin waypoints: Use stamped message +* add debug plugin +* Contributors: TSC21, Vladimir Ermakov + +0.21.2 (2017-09-25) +------------------- + +0.21.1 (2017-09-22) +------------------- + +0.21.0 (2017-09-14) +------------------- +* plugin waypoint: Rename current seq in wp list message +* waypoint: Publish current waypoint seq +* waypoint partial: code style cleanup +* waypoint partial: extend existing service +* Partial waypoint: added wp_transfered to push partial service response +* Partial waypoint: added partial updating to mavwp +* Contributors: James Mare, James Stewart, Vladimir Ermakov + +0.20.1 (2017-08-28) +------------------- + +0.20.0 (2017-08-23) +------------------- +* HIL Plugin + * add HilSensor.msg, HilStateQuaternion.msg, and add them in CMakeLists.txt + * Add hil_sensor.cpp plugin to send HIL_SENSOR mavlink message to FCU. + * fix HilSensor.msg. Make it more compact. + * Fix HilStateQuaternion.msg. Make it more compact. + * Add hil_state_quaternion plugin + * fix files: some variable names were wrong+some syntax problems + * fix syntax error in plugin .cpp files, make msg files match corresponding mavlink definitions + * fix plugin source files + * fix syntax + * fix function name. It was wrong. + * add HIL_GPS plugin + * add HilGPS.msg to CMakeList + * fix missing semicolon + * fix call of class name + * Add ACTUATOR_CONTROL_TARGET MAVLink message + * fix code + * increase number of fake satellites + * control sensor and control rates + * change control rate + * change control rate + * fix fake gps rate + * fix + * fix plugin_list + * fix + * remove unnecessary hil_sensor_mixin + * update HilSensor.msg and usage + * update HilStateQuaterion.msg and usage + * redo some changes; update HilGPS.msg and usage + * update hil_controls msg - use array of floats for aux channels + * merge actuator_control with actuator_control_target + * remove hil_sensor_mixin.h + * update actuator_control logic + * merge all plugins into a single one + * delete the remaining plugin files + * update description + * redo some changes; reduce LOC + * fix type cast on gps coord + * add HIL_OPTICAL_FLOW send based on OpticalFlowRad sub + * update authors list + * update subscribers names + * refactor gps coord convention + * add HIL_RC_INPUTS_RAW sender; cog protec msg structure and content + * apply correct rc_in translation; redo cog + * apply proper rotations and frame transforms + * remote throttle + * fix typo and msg api + * small changes + * refactor rcin_raw_cb + * new refactor to rcin_raw_cb arrays + * update velocity to meters + * readjust all the units so to match mavlink msg def + * update cog + * correct cog conversion + * refefine msg definitions to remove overhead + * hil: apply frame transform to body frame +* msgs fix `#625 `_: Rename SetMode.Response.success to mode_sent +* [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (`#700 `_) + * plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together + * plugins: typo correction: replace throttle with thrust + * plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg + * plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust) + * setpoint_attitude: update the logic of thrust normalization verification + * setpoint_attitude: implement sync between tf listener and thrust subscriber + * TF sync listener: generalize topic type that can be syncronized with TF2 + * TF2ListenerMixin: keep class template, use template for tf sync method only + * TF2ListenerMixin: fix and improve sync tf2_start method + * general update to yaml config files and parameters + * setpoint_attitude: add note on Thrust sub name + * setpoint_attitude: TF sync: pass subscriber pointer instead of binding it +* Use GeographicLib tools to guarantee ROS msg def and enhance features (`#693 `_) + * first commit + * Check for GeographicLib first without having to install it from the beginning each compile time + * add necessary cmake files + * remove gps_conversions.h and use GeographicLib to obtain the UTM coordinates + * move conversion functions to utils.h + * geographic conversions: update CMakeLists and package.xml + * geographic conversions: force download of the datasets + * geographic conversions: remove unneeded cmake module + * dependencies: use SHARED libs of geographiclib + * dependencies: correct FindGeographicLib.cmake so it can work for common Debian platforms + * CMakeList: do not be so restrict about GeographicLib dependency + * global position: odometry-use ECEF instead of UTM; update other fields + * global position: make travis happy + * global position: fix ident + * global_position: apply correct frames and frame transforms given each coordinate frame + * global_position: convert rcvd global origin to ECEF + * global_position: be more explicit about the ecef-enu transform + * global position: use home position as origin of map frame + * global position: minor refactoring + * global position: shield code with exception catch + * fix identation + * move dataset install to script; update README with new functionalities + * update README with warning + * global_position: fix identation + * update HomePosition to be consistent with the conversions in global_position to ensure the correct transformation of height + * home|global_position: fix compile errors, logic and dependencies + * home position: add height conversion + * travis: update to get datasets + * install geo dataset: update to verify alternative dataset folders + * travis: remove dataset install to allow clean build + * hp and gp: initialize geoid dataset once and make it thread safe + * README: update description relative to GeographicLib; fix typos + * global position: improve doxygen references + * README: update with some tips on rosdep install +* update ExtendedState with new MAV_LANDED_STATE enum +* Contributors: Nicklas Stockton, Nuno Marques, Vladimir Ermakov + +0.19.0 (2017-05-05) +------------------- +* msgs: Add cog script to finish ADSBVehicle.msg +* extras: Add ADSB plugin +* plugin `#695 `_: Fix plugin +* plugin: Add home_position +* Contributors: Nuno Marques, Vladimir Ermakov + +0.18.7 (2017-02-24) +------------------- +* trigger interface : rename to cycle_time to be consistent with PX4 +* Contributors: Kabir Mohammed + +0.18.6 (2017-02-07) +------------------- +* Plugins: system_status change status field to system_status + Add comment to State.msg for system_status enum +* Plugins: add system_status to state message +* Contributors: Pierre Kancir + +0.18.5 (2016-12-12) +------------------- + +0.18.4 (2016-11-11) +------------------- +* msgs: Fix `#609 `_ +* add hil_actuator_controls mavlink message +* Contributors: Beat Kung, Vladimir Ermakov + +0.18.3 (2016-07-07) +------------------- + +0.18.2 (2016-06-30) +------------------- + +0.18.1 (2016-06-24) +------------------- + +0.18.0 (2016-06-23) +------------------- +* Adding anchor to the HIL_CONTROLS message reference link +* Utilizing synchronise_stamp and adding reference to MAVLINK msg documentation +* Added a plugin that publishes HIL_CONTROLS as ROS messages +* node: Rename plugib base class - API incompatible to old class +* msgs `#543 `_: Update for MAVLink 2.0 +* Contributors: Pavel, Vladimir Ermakov + +0.17.3 (2016-05-20) +------------------- + +0.17.2 (2016-04-29) +------------------- + +0.17.1 (2016-03-28) +------------------- + +0.17.0 (2016-02-09) +------------------- +* rebased with master +* Contributors: francois + +0.16.6 (2016-02-04) +------------------- + +0.16.5 (2016-01-11) +------------------- + +0.16.4 (2015-12-14) +------------------- +* Update mavlink message documentation links +* remove "altitude\_" prefix from members +* implemented altitude plugin +* Contributors: Andreas Antener, Vladimir Ermakov + +0.16.3 (2015-11-19) +------------------- + +0.16.2 (2015-11-17) +------------------- + +0.16.1 (2015-11-13) +------------------- + +0.16.0 (2015-11-09) +------------------- +* msgs `#418 `_: add message for attitude setpoints +* plugin: waypoint fix `#414 `_: remove GOTO service. + It is replaced with more standard global setpoint messages. +* msgs `#415 `_: Add message for raw global setpoint +* msgs `#402 `_: PositionTarget message type +* setting constant values and reference docs +* pass new extended state to ros +* msgs `#371 `_: add missing message +* msgs `#371 `_: add HomePosition message +* Contributors: Andreas Antener, Vladimir Ermakov + +0.15.0 (2015-09-17) +------------------- +* msgs `#286 `_: fix bug with packet header. +* msgs `#286 `_: Add valid flag and checksum to Mavlink.msg +* plugin: manual_control: Use shared pointer message + Fix alphabetic order of msgs. +* removed old commend in .msg file +* Add MANUAL_CONTROL handling with new plugin +* Contributors: Vladimir Ermakov, v01d + +0.14.2 (2015-08-20) +------------------- + +0.14.1 (2015-08-19) +------------------- + +0.14.0 (2015-08-17) +------------------- +* msgs: Add mixer group constants ActuatorControl +* msgs: Add notes to message headers. +* msgs: sort msgs in alphabetical order +* msgs: use std::move for mavlink->ros convert +* msgs: add note about convert function +* msgs: change description, make catkin lint happy +* msgs: move convert functions to msgs package. +* msgs: fix message generator and runtime depent tags +* msgs: remove never used Mavlink.fromlcm field. +* msgs: add package name for all non basic types +* msgs: fix msgs build +* msgs `#354 `_: move all messages to mavros_msgs package. +* Contributors: Vladimir Ermakov diff --git a/src/external/mavros_msgs/CMakeLists.txt b/src/external/mavros_msgs/CMakeLists.txt new file mode 100644 index 0000000..a3220b0 --- /dev/null +++ b/src/external/mavros_msgs/CMakeLists.txt @@ -0,0 +1,208 @@ +cmake_minimum_required(VERSION 3.5) +project(mavros_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +# include_directories(include) + +# [[[cog: +# import mavros_cog +# ]]] +# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +set(msg_files + # [[[cog: + # mavros_cog.outl_glob_files('msg', '*.msg') + # ]]] + msg/ADSBVehicle.msg + msg/ActuatorControl.msg + msg/Altitude.msg + msg/AttitudeTarget.msg + msg/CamIMUStamp.msg + msg/CameraImageCaptured.msg + msg/CellularStatus.msg + msg/CommandCode.msg + msg/CompanionProcessStatus.msg + msg/DebugValue.msg + msg/ESCInfo.msg + msg/ESCInfoItem.msg + msg/ESCStatus.msg + msg/ESCStatusItem.msg + msg/ESCTelemetry.msg + msg/ESCTelemetryItem.msg + msg/EstimatorStatus.msg + msg/ExtendedState.msg + msg/FileEntry.msg + msg/GPSINPUT.msg + msg/GPSRAW.msg + msg/GPSRTK.msg + msg/GimbalDeviceAttitudeStatus.msg + msg/GimbalDeviceInformation.msg + msg/GimbalDeviceSetAttitude.msg + msg/GimbalManagerInformation.msg + msg/GimbalManagerSetAttitude.msg + msg/GimbalManagerSetPitchyaw.msg + msg/GimbalManagerStatus.msg + msg/GlobalPositionTarget.msg + msg/HilActuatorControls.msg + msg/HilControls.msg + msg/HilGPS.msg + msg/HilSensor.msg + msg/HilStateQuaternion.msg + msg/HomePosition.msg + msg/LandingTarget.msg + msg/LogData.msg + msg/LogEntry.msg + msg/MagnetometerReporter.msg + msg/ManualControl.msg + msg/Mavlink.msg + msg/MountControl.msg + msg/NavControllerOutput.msg + msg/OnboardComputerStatus.msg + msg/OpticalFlow.msg + msg/OpticalFlowRad.msg + msg/OverrideRCIn.msg + msg/Param.msg + msg/ParamEvent.msg + msg/ParamValue.msg + msg/PlayTuneV2.msg + msg/PositionTarget.msg + msg/RCIn.msg + msg/RCOut.msg + msg/RTCM.msg + msg/RTKBaseline.msg + msg/RadioStatus.msg + msg/State.msg + msg/StatusEvent.msg + msg/StatusText.msg + msg/SysStatus.msg + msg/TerrainReport.msg + msg/Thrust.msg + msg/TimesyncStatus.msg + msg/Trajectory.msg + msg/Tunnel.msg + msg/VehicleInfo.msg + msg/VfrHud.msg + msg/Vibration.msg + msg/Waypoint.msg + msg/WaypointList.msg + msg/WaypointReached.msg + msg/WheelOdomStamped.msg + # [[[end]]] (checksum: a8e24eb0a6da5cea6cc049fdc6b2612e) +) + +set(srv_files + # [[[cog: + # mavros_cog.outl_glob_files('srv', '*.srv') + # ]]] + srv/CommandAck.srv + srv/CommandBool.srv + srv/CommandHome.srv + srv/CommandInt.srv + srv/CommandLong.srv + srv/CommandTOL.srv + srv/CommandTOLLocal.srv + srv/CommandTriggerControl.srv + srv/CommandTriggerInterval.srv + srv/CommandVtolTransition.srv + srv/EndpointAdd.srv + srv/EndpointDel.srv + srv/FileChecksum.srv + srv/FileClose.srv + srv/FileList.srv + srv/FileMakeDir.srv + srv/FileOpen.srv + srv/FileRead.srv + srv/FileRemove.srv + srv/FileRemoveDir.srv + srv/FileRename.srv + srv/FileTruncate.srv + srv/FileWrite.srv + srv/GimbalGetInformation.srv + srv/GimbalManagerCameraTrack.srv + srv/GimbalManagerConfigure.srv + srv/GimbalManagerPitchyaw.srv + srv/GimbalManagerSetRoi.srv + srv/LogRequestData.srv + srv/LogRequestEnd.srv + srv/LogRequestList.srv + srv/MessageInterval.srv + srv/MountConfigure.srv + srv/ParamGet.srv + srv/ParamPull.srv + srv/ParamPush.srv + srv/ParamSet.srv + srv/ParamSetV2.srv + srv/SetMavFrame.srv + srv/SetMode.srv + srv/StreamRate.srv + srv/VehicleInfoGet.srv + srv/WaypointClear.srv + srv/WaypointPull.srv + srv/WaypointPush.srv + srv/WaypointSetCurrent.srv + # [[[end]]] (checksum: cd7701b28a3176d96ef65cb1f2157917) +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES + builtin_interfaces + rcl_interfaces + geographic_msgs + geometry_msgs + sensor_msgs + std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +install( + FILES mavros_msgs_mapping_rule.yaml + DESTINATION share/${PROJECT_NAME} +) + +if(rcl_interfaces_VERSION VERSION_LESS "1.2.0") + install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.hpp" + ) +else() + # NOTE(vooon): Humble + install( + DIRECTORY include/ + DESTINATION include/mavros_msgs + FILES_MATCHING PATTERN "*.hpp" + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() +# vim: ts=2 sw=2 et: diff --git a/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp new file mode 100644 index 0000000..6241e8a --- /dev/null +++ b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp @@ -0,0 +1,145 @@ +// +// Copyright 2015,2016,2021 Vladimir Ermakov. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief Mavlink convert utils + * @file + * @author Vladimir Ermakov + */ + +#pragma once +#ifndef MAVROS_MSGS__MAVLINK_CONVERT_HPP_ +#define MAVROS_MSGS__MAVLINK_CONVERT_HPP_ + +#include +#include + +#include + +namespace mavros_msgs +{ +namespace mavlink +{ + +using ::mavlink::mavlink_message_t; +using mavros_msgs::msg::Mavlink; + +// [[[cog: +// FIELD_NAMES = [ +// "magic", +// "len", +// "incompat_flags", +// "compat_flags", +// "seq", +// "sysid", +// "compid", +// "msgid", +// "checksum", +// ] +// ]]] +// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +// NOTE(vooon): Ignore impossible warning as +// memcpy() should work with unaligned pointers without any trouble. +// +// warning: taking address of packed member of ‘mavlink::__mavlink_message’ +// may result in an unaligned pointer value +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" + +/** + * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t + * + * @note signature vector should be empty for unsigned OR + * MAVLINK_SIGNATURE_BLOCK size for signed messages + * + * @param[in] rmsg mavros_msgs/Mavlink message + * @param[out] mmsg mavlink_message_t struct + * @return true if success + */ +inline bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg) +{ + if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) { + return false; + } + + if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) { + return false; + } + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"mmsg.{f} = rmsg.{f};") + // ]]] + mmsg.magic = rmsg.magic; + mmsg.len = rmsg.len; + mmsg.incompat_flags = rmsg.incompat_flags; + mmsg.compat_flags = rmsg.compat_flags; + mmsg.seq = rmsg.seq; + mmsg.sysid = rmsg.sysid; + mmsg.compid = rmsg.compid; + mmsg.msgid = rmsg.msgid; + mmsg.checksum = rmsg.checksum; + // [[[end]]] (checksum: 0b66f0fc1cd46db0f18a2429c56a6b8c) + std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64); + std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature); + + return true; +} + +/** + * @brief Convert mavlink_message_t to mavros/Mavlink + * + * @param[in] mmsg mavlink_message_t struct + * @param[out] rmsg mavros_msgs/Mavlink message + * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE) + * @return true, this convertion can't fail + */ +inline bool convert( + const mavlink_message_t & mmsg, Mavlink & rmsg, + uint8_t framing_status = Mavlink::FRAMING_OK) +{ + const size_t payload64_len = (mmsg.len + 7) / 8; + + rmsg.framing_status = framing_status; + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"rmsg.{f} = mmsg.{f};") + // ]]] + rmsg.magic = mmsg.magic; + rmsg.len = mmsg.len; + rmsg.incompat_flags = mmsg.incompat_flags; + rmsg.compat_flags = mmsg.compat_flags; + rmsg.seq = mmsg.seq; + rmsg.sysid = mmsg.sysid; + rmsg.compid = mmsg.compid; + rmsg.msgid = mmsg.msgid; + rmsg.checksum = mmsg.checksum; + // [[[end]]] (checksum: 64ef6c1af60c622ed427e005d8ca4f2a) + rmsg.payload64.assign( + mmsg.payload64, + mmsg.payload64 + payload64_len); + + // copy signature block only if message is signed + if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) { + rmsg.signature.assign( + mmsg.signature, + mmsg.signature + sizeof(mmsg.signature)); + } else { + rmsg.signature.clear(); + } + + return true; +} + +#pragma GCC diagnostic pop + +} // namespace mavlink +} // namespace mavros_msgs + +#endif // MAVROS_MSGS__MAVLINK_CONVERT_HPP_ diff --git a/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml new file mode 100644 index 0000000..9f69364 --- /dev/null +++ b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml @@ -0,0 +1,8 @@ +# This file defines mappings between ROS 1 and ROS 2 interfaces. +# It is used with the ros1_bridge to allow for communcation between ROS 1 and ROS 2. + +- + ros1_package_name: 'mavros_msgs' + ros1_message_name: 'VFR_HUD' + ros2_package_name: 'mavros_msgs' + ros2_message_name: 'VfrHud' diff --git a/src/external/mavros_msgs/msg/ADSBVehicle.msg b/src/external/mavros_msgs/msg/ADSBVehicle.msg new file mode 100644 index 0000000..44b9caa --- /dev/null +++ b/src/external/mavros_msgs/msg/ADSBVehicle.msg @@ -0,0 +1,66 @@ +# The location and information of an ADSB vehicle +# +# https://mavlink.io/en/messages/common.html#ADSB_VEHICLE + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('ADSB_ALTITUDE_TYPE', 'ALT_') +# mavros_cog.idl_decl_enum('ADSB_EMITTER_TYPE', 'EMITTER_') +# mavros_cog.idl_decl_enum('ADSB_FLAGS', 'FLAG_', 16) +# ]]] +# ADSB_ALTITUDE_TYPE +uint8 ALT_PRESSURE_QNH = 0 # Altitude reported from a Baro source using QNH reference +uint8 ALT_GEOMETRIC = 1 # Altitude reported from a GNSS source +# ADSB_EMITTER_TYPE +uint8 EMITTER_NO_INFO = 0 +uint8 EMITTER_LIGHT = 1 +uint8 EMITTER_SMALL = 2 +uint8 EMITTER_LARGE = 3 +uint8 EMITTER_HIGH_VORTEX_LARGE = 4 +uint8 EMITTER_HEAVY = 5 +uint8 EMITTER_HIGHLY_MANUV = 6 +uint8 EMITTER_ROTOCRAFT = 7 +uint8 EMITTER_UNASSIGNED = 8 +uint8 EMITTER_GLIDER = 9 +uint8 EMITTER_LIGHTER_AIR = 10 +uint8 EMITTER_PARACHUTE = 11 +uint8 EMITTER_ULTRA_LIGHT = 12 +uint8 EMITTER_UNASSIGNED2 = 13 +uint8 EMITTER_UAV = 14 +uint8 EMITTER_SPACE = 15 +uint8 EMITTER_UNASSGINED3 = 16 +uint8 EMITTER_EMERGENCY_SURFACE = 17 +uint8 EMITTER_SERVICE_SURFACE = 18 +uint8 EMITTER_POINT_OBSTACLE = 19 +# ADSB_FLAGS +uint16 FLAG_VALID_COORDS = 1 +uint16 FLAG_VALID_ALTITUDE = 2 +uint16 FLAG_VALID_HEADING = 4 +uint16 FLAG_VALID_VELOCITY = 8 +uint16 FLAG_VALID_CALLSIGN = 16 +uint16 FLAG_VALID_SQUAWK = 32 +uint16 FLAG_SIMULATED = 64 +uint16 FLAG_VERTICAL_VELOCITY_VALID = 128 +uint16 FLAG_BARO_VALID = 256 +uint16 FLAG_SOURCE_UAT = 32768 +# [[[end]]] (checksum: a34f2a081739921b6e3e443ed0516d8d) + +std_msgs/Header header + +uint32 icao_address +string callsign + +float64 latitude +float64 longitude +float32 altitude # AMSL + +float32 heading # deg [0..360) +float32 hor_velocity # m/s +float32 ver_velocity # m/s + +uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum +uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum + +builtin_interfaces/Duration tslc # Duration from last communication, seconds [0..255] +uint16 flags # ADSB_FLAGS bit field +uint16 squawk # Squawk code diff --git a/src/external/mavros_msgs/msg/ActuatorControl.msg b/src/external/mavros_msgs/msg/ActuatorControl.msg new file mode 100644 index 0000000..5332a08 --- /dev/null +++ b/src/external/mavros_msgs/msg/ActuatorControl.msg @@ -0,0 +1,16 @@ +# raw servo values for direct actuator controls +# +# about groups, mixing and channels: +# https://pixhawk.org/dev/mixing + +# constant for mixer group +uint8 PX4_MIX_FLIGHT_CONTROL = 0 +uint8 PX4_MIX_FLIGHT_CONTROL_VTOL_ALT = 1 +uint8 PX4_MIX_PAYLOAD = 2 +uint8 PX4_MIX_MANUAL_PASSTHROUGH = 3 +#uint8 PX4_MIX_FC_MC_VIRT = 4 +#uint8 PX4_MIX_FC_FW_VIRT = 5 + +std_msgs/Header header +uint8 group_mix +float32[8] controls diff --git a/src/external/mavros_msgs/msg/Altitude.msg b/src/external/mavros_msgs/msg/Altitude.msg new file mode 100644 index 0000000..81ae0a9 --- /dev/null +++ b/src/external/mavros_msgs/msg/Altitude.msg @@ -0,0 +1,12 @@ +# Altitude information +# +# https://mavlink.io/en/messages/common.html#ALTITUDE + +std_msgs/Header header + +float32 monotonic +float32 amsl +float32 local +float32 relative +float32 terrain +float32 bottom_clearance diff --git a/src/external/mavros_msgs/msg/AttitudeTarget.msg b/src/external/mavros_msgs/msg/AttitudeTarget.msg new file mode 100644 index 0000000..a62712b --- /dev/null +++ b/src/external/mavros_msgs/msg/AttitudeTarget.msg @@ -0,0 +1,17 @@ +# Message for SET_ATTITUDE_TARGET +# +# Some complex system requires all feautures that mavlink +# message provide. See issue #402, #418. + +std_msgs/Header header + +uint8 type_mask +uint8 IGNORE_ROLL_RATE = 1 # body_rate.x +uint8 IGNORE_PITCH_RATE = 2 # body_rate.y +uint8 IGNORE_YAW_RATE = 4 # body_rate.z +uint8 IGNORE_THRUST = 64 +uint8 IGNORE_ATTITUDE = 128 # orientation field + +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 body_rate +float32 thrust diff --git a/src/external/mavros_msgs/msg/CamIMUStamp.msg b/src/external/mavros_msgs/msg/CamIMUStamp.msg new file mode 100644 index 0000000..52b9e8b --- /dev/null +++ b/src/external/mavros_msgs/msg/CamIMUStamp.msg @@ -0,0 +1,4 @@ +# IMU-Camera synchronisation data + +builtin_interfaces/Time frame_stamp # Timestamp when the camera was triggered +int32 frame_seq_id # Sequence number of the image frame diff --git a/src/external/mavros_msgs/msg/CameraImageCaptured.msg b/src/external/mavros_msgs/msg/CameraImageCaptured.msg new file mode 100644 index 0000000..44c1a98 --- /dev/null +++ b/src/external/mavros_msgs/msg/CameraImageCaptured.msg @@ -0,0 +1,11 @@ +# MAVLink message: CAMERA_IMAGE_CAPTURED +# https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED + +std_msgs/Header header + +geometry_msgs/Quaternion orientation # Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) +geographic_msgs/GeoPoint geo +float32 relative_alt # mm Altitude above ground +int32 image_index # Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) +int8 capture_result # Boolean indicating success (1) or failure (0) while capturing this image. +string file_url #URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. diff --git a/src/external/mavros_msgs/msg/CellularStatus.msg b/src/external/mavros_msgs/msg/CellularStatus.msg new file mode 100644 index 0000000..2c9f99f --- /dev/null +++ b/src/external/mavros_msgs/msg/CellularStatus.msg @@ -0,0 +1,9 @@ +#Follows https://mavlink.io/en/messages/common.html#CELLULAR_STATUS specification + +uint8 status +uint8 failure_reason +uint8 type +uint8 quality +uint16 mcc +uint16 mnc +uint16 lac \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/CommandCode.msg b/src/external/mavros_msgs/msg/CommandCode.msg new file mode 100644 index 0000000..00fd7da --- /dev/null +++ b/src/external/mavros_msgs/msg/CommandCode.msg @@ -0,0 +1,200 @@ +# MAV_CMD command codes. +# Actual meaning and params you may find in MAVLink documentation +# https://mavlink.io/en/messages/common.html#MAV_CMD + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum_mav_cmd() +# ]]] +# MAV_CMD_AIRFRAME +uint16 AIRFRAME_CONFIGURATION = 2520 + +# MAV_CMD_ARM +uint16 ARM_AUTHORIZATION_REQUEST = 3001 # Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + +# MAV_CMD_CAMERA +uint16 CAMERA_TRACK_POINT = 2004 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. +uint16 CAMERA_TRACK_RECTANGLE = 2005 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. +uint16 CAMERA_STOP_TRACKING = 2010 # Stops ongoing tracking. + +# MAV_CMD_CAN +uint16 CAN_FORWARD = 32000 # Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages + +# MAV_CMD_COMPONENT +uint16 COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component + +# MAV_CMD_CONDITION +uint16 CONDITION_DELAY = 112 # Delay mission state machine. +uint16 CONDITION_CHANGE_ALT = 113 # Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached. +uint16 CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. +uint16 CONDITION_YAW = 115 # Reach a certain target angle. +uint16 CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + +# MAV_CMD_CONTROL +uint16 CONTROL_HIGH_LATENCY = 2600 # Request to start/stop transmitting over the high latency telemetry + +# MAV_CMD_DO +uint16 DO_FOLLOW = 32 # Begin following a target +uint16 DO_FOLLOW_REPOSITION = 33 # Reposition the MAV after a follow target command has been sent +uint16 DO_SET_MODE = 176 # Set system mode. +uint16 DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times +uint16 DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points +uint16 DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. +uint16 DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. +uint16 DO_SET_RELAY = 181 # Set a relay to a condition. +uint16 DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period. +uint16 DO_SET_SERVO = 183 # Set a servo to a desired PWM value. +uint16 DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. +uint16 DO_FLIGHTTERMINATION = 185 # Terminate flight immediately +uint16 DO_CHANGE_ALTITUDE = 186 # Change altitude set point. +uint16 DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude/Altitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. +uint16 DO_RALLY_LAND = 190 # Mission command to perform a landing from a rally point. +uint16 DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. +uint16 DO_REPOSITION = 192 # Reposition the vehicle to a specific WGS84 global position. +uint16 DO_PAUSE_CONTINUE = 193 # If in a GPS controlled position mode, hold the current position or continue. +uint16 DO_SET_REVERSE = 194 # Set moving direction to forward or reverse. +uint16 DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_SYSID = 198 # Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. +uint16 DO_CONTROL_VIDEO = 200 # Control onboard camera system. +uint16 DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_DIGICAM_CONFIGURE = 202 # Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). +uint16 DO_DIGICAM_CONTROL = 203 # Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). +uint16 DO_MOUNT_CONFIGURE = 204 # Mission command to configure a camera or antenna mount +uint16 DO_MOUNT_CONTROL = 205 # Mission command to control a camera or antenna mount +uint16 DO_SET_CAM_TRIGG_DIST = 206 # Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. +uint16 DO_FENCE_ENABLE = 207 # Mission command to enable the geofence +uint16 DO_PARACHUTE = 208 # Mission item/command to release a parachute or enable/disable auto release. +uint16 DO_MOTOR_TEST = 209 # Mission command to perform motor test. +uint16 DO_INVERTED_FLIGHT = 210 # Change to/from inverted flight. +uint16 DO_GRIPPER = 211 # Mission command to operate a gripper. +uint16 DO_AUTOTUNE_ENABLE = 212 # Enable/disable autotune. +uint16 DO_SET_CAM_TRIGG_INTERVAL = 214 # Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. +uint16 DO_MOUNT_CONTROL_QUAT = 220 # Mission command to control a camera or antenna mount, using a quaternion as reference. +uint16 DO_GUIDED_MASTER = 221 # set id of master controller +uint16 DO_GUIDED_LIMITS = 222 # Set limits for external control +uint16 DO_ENGINE_CONTROL = 223 # Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines +uint16 DO_SET_MISSION_CURRENT = 224 # Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). +uint16 DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration +uint16 DO_JUMP_TAG = 601 # Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. +uint16 DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate. +uint16 DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control. +uint16 DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system. +uint16 DO_VTOL_TRANSITION = 3000 # Request VTOL transition +uint16 DO_ADSB_OUT_IDENT = 10001 # Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. +uint16 DO_WINCH = 42600 # Command to operate winch. + +# MAV_CMD_FIXED +uint16 FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. + +# MAV_CMD_GET +uint16 GET_HOME_POSITION = 410 # Request the home position from the vehicle. +uint16 GET_MESSAGE_INTERVAL = 510 # Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. + +# MAV_CMD_IMAGE +uint16 IMAGE_START_CAPTURE = 2000 # Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. +uint16 IMAGE_STOP_CAPTURE = 2001 # Stop image capture sequence. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + +# MAV_CMD_JUMP +uint16 JUMP_TAG = 600 # Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. + +# MAV_CMD_LOGGING +uint16 LOGGING_START = 2510 # Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) +uint16 LOGGING_STOP = 2511 # Request to stop streaming log data over MAVLink + +# MAV_CMD_MISSION +uint16 MISSION_START = 300 # start running a mission + +# MAV_CMD_NAV +uint16 NAV_WAYPOINT = 16 # Navigate to waypoint. +uint16 NAV_LOITER_UNLIM = 17 # Loiter around this waypoint an unlimited amount of time +uint16 NAV_LOITER_TURNS = 18 # Loiter around this waypoint for X turns +uint16 NAV_LOITER_TIME = 19 # Loiter around this waypoint for X seconds +uint16 NAV_RETURN_TO_LAUNCH = 20 # Return to launch location +uint16 NAV_LAND = 21 # Land at location. +uint16 NAV_TAKEOFF = 22 # Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. +uint16 NAV_LAND_LOCAL = 23 # Land at local position (local frame only) +uint16 NAV_TAKEOFF_LOCAL = 24 # Takeoff from local position (local frame only) +uint16 NAV_FOLLOW = 25 # Vehicle following, i.e. this waypoint represents the position of a moving vehicle +uint16 NAV_CONTINUE_AND_CHANGE_ALT = 30 # Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. +uint16 NAV_LOITER_TO_ALT = 31 # Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. +uint16 NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. +uint16 NAV_SPLINE_WAYPOINT = 82 # Navigate to waypoint using a spline path. +uint16 NAV_VTOL_TAKEOFF = 84 # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). +uint16 NAV_VTOL_LAND = 85 # Land using VTOL mode +uint16 NAV_GUIDED_ENABLE = 92 # hand control over to an external controller +uint16 NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time +uint16 NAV_PAYLOAD_PLACE = 94 # Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. +uint16 NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration +uint16 NAV_SET_YAW_SPEED = 213 # Sets a desired vehicle turn angle and speed change. +uint16 NAV_FENCE_RETURN_POINT = 5000 # Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. +uint16 NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001 # Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. +uint16 NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002 # Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. +uint16 NAV_FENCE_CIRCLE_INCLUSION = 5003 # Circular fence area. The vehicle must stay inside this area. +uint16 NAV_FENCE_CIRCLE_EXCLUSION = 5004 # Circular fence area. The vehicle must stay outside this area. +uint16 NAV_RALLY_POINT = 5100 # Rally point. You can have multiple rally points defined. + +# MAV_CMD_OBLIQUE +uint16 OBLIQUE_SURVEY = 260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. + +# MAV_CMD_OVERRIDE +uint16 OVERRIDE_GOTO = 252 # Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. + +# MAV_CMD_PANORAMA +uint16 PANORAMA_CREATE = 2800 # Create a panorama at the current position + +# MAV_CMD_PAYLOAD +uint16 PAYLOAD_PREPARE_DEPLOY = 30001 # Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. +uint16 PAYLOAD_CONTROL_DEPLOY = 30002 # Control the payload deployment. + +# MAV_CMD_PREFLIGHT +uint16 PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. +uint16 PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. +uint16 PREFLIGHT_UAVCAN = 243 # Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). +uint16 PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. +uint16 PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. + +# MAV_CMD_REQUEST +uint16 REQUEST_MESSAGE = 512 # Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). +uint16 REQUEST_PROTOCOL_VERSION = 519 # Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message +uint16 REQUEST_AUTOPILOT_CAPABILITIES = 520 # Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message +uint16 REQUEST_CAMERA_INFORMATION = 521 # Request camera information (CAMERA_INFORMATION). +uint16 REQUEST_CAMERA_SETTINGS = 522 # Request camera settings (CAMERA_SETTINGS). +uint16 REQUEST_STORAGE_INFORMATION = 525 # Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. +uint16 REQUEST_CAMERA_CAPTURE_STATUS = 527 # Request camera capture status (CAMERA_CAPTURE_STATUS) +uint16 REQUEST_FLIGHT_INFORMATION = 528 # Request flight information (FLIGHT_INFORMATION) +uint16 REQUEST_VIDEO_STREAM_INFORMATION = 2504 # Request video stream information (VIDEO_STREAM_INFORMATION) +uint16 REQUEST_VIDEO_STREAM_STATUS = 2505 # Request video stream status (VIDEO_STREAM_STATUS) + +# MAV_CMD_RESET +uint16 RESET_CAMERA_SETTINGS = 529 # Reset all camera settings to Factory Default + +# MAV_CMD_RUN +uint16 RUN_PREARM_CHECKS = 401 # Instructs system to run pre-arm checks. This command should return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed, otherwse MAV_RESULT_ACCEPTED. Note that the return value from executing this command does not indicate whether the vehicle is armable or not, just whether the system has successfully run/is currently running the checks. The result of the checks is reflected in the SYS_STATUS message. + +# MAV_CMD_SET +uint16 SET_MESSAGE_INTERVAL = 511 # Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. +uint16 SET_CAMERA_MODE = 530 # Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. +uint16 SET_CAMERA_ZOOM = 531 # Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). +uint16 SET_CAMERA_FOCUS = 532 # Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). +uint16 SET_GUIDED_SUBMODE_STANDARD = 4000 # This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. +uint16 SET_GUIDED_SUBMODE_CIRCLE = 4001 # This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. + +# MAV_CMD_START +uint16 START_RX_PAIR = 500 # Starts receiver pairing. + +# MAV_CMD_STORAGE +uint16 STORAGE_FORMAT = 526 # Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. + +# MAV_CMD_UAVCAN +uint16 UAVCAN_GET_NODE_INFO = 5200 # Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. + +# MAV_CMD_VIDEO +uint16 VIDEO_START_CAPTURE = 2500 # Starts video capture (recording). +uint16 VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture (recording). +uint16 VIDEO_START_STREAMING = 2502 # Start video streaming +uint16 VIDEO_STOP_STREAMING = 2503 # Stop the given video stream + +# [[[end]]] (checksum: 73ee94ac661c9fcb61528a6668f71d94) diff --git a/src/external/mavros_msgs/msg/CompanionProcessStatus.msg b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg new file mode 100644 index 0000000..ea8b08c --- /dev/null +++ b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg @@ -0,0 +1,19 @@ +# Mavros message: COMPANIONPROCESSSTATUS + +std_msgs/Header header + +uint8 state # See enum COMPANION_PROCESS_STATE +uint8 component # See enum MAV_COMPONENT + +uint8 MAV_STATE_UNINIT = 0 +uint8 MAV_STATE_BOOT = 1 +uint8 MAV_STATE_CALIBRATING = 2 +uint8 MAV_STATE_STANDBY = 3 +uint8 MAV_STATE_ACTIVE = 4 +uint8 MAV_STATE_CRITICAL = 5 +uint8 MAV_STATE_EMERGENCY = 6 +uint8 MAV_STATE_POWEROFF = 7 +uint8 MAV_STATE_FLIGHT_TERMINATION = 8 + +uint8 MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196 +uint8 MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197 diff --git a/src/external/mavros_msgs/msg/DebugValue.msg b/src/external/mavros_msgs/msg/DebugValue.msg new file mode 100644 index 0000000..286b6c2 --- /dev/null +++ b/src/external/mavros_msgs/msg/DebugValue.msg @@ -0,0 +1,26 @@ +# Msg for Debug MAVLink API +# +# Supported types: +# DEBUG https://mavlink.io/en/messages/common.html#DEBUG +# DEBUG_VECTOR https://mavlink.io/en/messages/common.html#DEBUG_VECT +# DEBUG_FLOAT_ARRAY https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY +# NAMED_VALUE_FLOAT https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT +# NAMED_VALUE_INT https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT + +std_msgs/Header header + +int32 index # index value of DEBUG value (-1 if not indexed) +int32 array_id # Unique ID used to discriminate between DEBUG_FLOAT_ARRAYS (-1 if not used) + +string name # value name/key + +float32 value_float # float value for NAMED_VALUE_FLOAT and DEBUG +int32 value_int # int value for NAMED_VALUE_INT +float32[] data # DEBUG vector or array + +uint8 type +uint8 TYPE_DEBUG = 0 +uint8 TYPE_DEBUG_VECT = 1 +uint8 TYPE_DEBUG_FLOAT_ARRAY = 2 +uint8 TYPE_NAMED_VALUE_FLOAT = 3 +uint8 TYPE_NAMED_VALUE_INT = 4 diff --git a/src/external/mavros_msgs/msg/ESCInfo.msg b/src/external/mavros_msgs/msg/ESCInfo.msg new file mode 100644 index 0000000..71bcc41 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCInfo.msg @@ -0,0 +1,14 @@ +# ESCInfo.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_INFO + +std_msgs/Header header + +uint16 counter +uint8 count +uint8 connection_type +uint8 info +mavros_msgs/ESCInfoItem[] esc_info + diff --git a/src/external/mavros_msgs/msg/ESCInfoItem.msg b/src/external/mavros_msgs/msg/ESCInfoItem.msg new file mode 100644 index 0000000..fc63856 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCInfoItem.msg @@ -0,0 +1,12 @@ +# ESCInfoItem.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_INFO + +std_msgs/Header header + +uint16 failure_flags +uint32 error_count +int32 temperature + diff --git a/src/external/mavros_msgs/msg/ESCStatus.msg b/src/external/mavros_msgs/msg/ESCStatus.msg new file mode 100644 index 0000000..870579b --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCStatus.msg @@ -0,0 +1,9 @@ +# ESCStatus.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_STATUS + +std_msgs/Header header + +mavros_msgs/ESCStatusItem[] esc_status diff --git a/src/external/mavros_msgs/msg/ESCStatusItem.msg b/src/external/mavros_msgs/msg/ESCStatusItem.msg new file mode 100644 index 0000000..252aa0b --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCStatusItem.msg @@ -0,0 +1,11 @@ +# ESCStatusItem.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_STATUS + +std_msgs/Header header + +int32 rpm +float32 voltage +float32 current diff --git a/src/external/mavros_msgs/msg/ESCTelemetry.msg b/src/external/mavros_msgs/msg/ESCTelemetry.msg new file mode 100644 index 0000000..f0c5916 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCTelemetry.msg @@ -0,0 +1,10 @@ +# APM ESC Telemetry as returned by BLHeli +# +# See: +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12 + +std_msgs/Header header + +mavros_msgs/ESCTelemetryItem[] esc_telemetry diff --git a/src/external/mavros_msgs/msg/ESCTelemetryItem.msg b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg new file mode 100644 index 0000000..e5c0c17 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg @@ -0,0 +1,15 @@ +# APM ESC Telemetry as returned by BLHeli +# +# See: +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12 + +std_msgs/Header header + +float32 temperature # deg C +float32 voltage # V +float32 current # A +float32 totalcurrent # Ah +int32 rpm # 1/min +uint16 count # count of telemetry packets diff --git a/src/external/mavros_msgs/msg/EstimatorStatus.msg b/src/external/mavros_msgs/msg/EstimatorStatus.msg new file mode 100644 index 0000000..40101b8 --- /dev/null +++ b/src/external/mavros_msgs/msg/EstimatorStatus.msg @@ -0,0 +1,23 @@ +# Current autopilot estimator state +# +# https://mavlink.io/en/messages/common.html#ESTIMATOR_STATUS_FLAGS + +std_msgs/Header header +bool attitude_status_flag + +bool velocity_horiz_status_flag +bool velocity_vert_status_flag + +bool pos_horiz_rel_status_flag +bool pos_horiz_abs_status_flag + +bool pos_vert_abs_status_flag +bool pos_vert_agl_status_flag + +bool const_pos_mode_status_flag + +bool pred_pos_horiz_rel_status_flag +bool pred_pos_horiz_abs_status_flag + +bool gps_glitch_status_flag +bool accel_error_status_flag \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/ExtendedState.msg b/src/external/mavros_msgs/msg/ExtendedState.msg new file mode 100644 index 0000000..47c5c29 --- /dev/null +++ b/src/external/mavros_msgs/msg/ExtendedState.msg @@ -0,0 +1,19 @@ +# Extended autopilot state +# +# https://mavlink.io/en/messages/common.html#EXTENDED_SYS_STATE + +uint8 VTOL_STATE_UNDEFINED = 0 +uint8 VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VTOL_STATE_MC = 3 +uint8 VTOL_STATE_FW = 4 + +uint8 LANDED_STATE_UNDEFINED = 0 +uint8 LANDED_STATE_ON_GROUND = 1 +uint8 LANDED_STATE_IN_AIR = 2 +uint8 LANDED_STATE_TAKEOFF = 3 +uint8 LANDED_STATE_LANDING = 4 + +std_msgs/Header header +uint8 vtol_state +uint8 landed_state diff --git a/src/external/mavros_msgs/msg/FileEntry.msg b/src/external/mavros_msgs/msg/FileEntry.msg new file mode 100644 index 0000000..e733326 --- /dev/null +++ b/src/external/mavros_msgs/msg/FileEntry.msg @@ -0,0 +1,12 @@ +# File/Dir information + +uint8 TYPE_FILE = 0 +uint8 TYPE_DIRECTORY = 1 + +string name +uint8 type +uint64 size + +# Not supported by MAVLink FTP +#builtin_interfaces/Time atime +#int32 access_flags diff --git a/src/external/mavros_msgs/msg/GPSINPUT.msg b/src/external/mavros_msgs/msg/GPSINPUT.msg new file mode 100644 index 0000000..15a038a --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSINPUT.msg @@ -0,0 +1,37 @@ +# FCU GPS INPUT message for the gps_input plugin +# mavlink GPS_INPUT message. + +std_msgs/Header header +## GPS_FIX_TYPE enum +uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected +uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected +uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position +uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position +uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position +uint8 GPS_FIX_TYPE_RTK_FLOATR = 5 # TK float, 3D position +uint8 GPS_FIX_TYPE_RTK_FIXEDR = 6 # TK Fixed, 3D position +uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations +uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position +uint8 fix_type # [GPS_FIX_TYPE] GPS fix type + +uint8 gps_id # ID of the GPS for multiple GPS inputs +uint16 ignore_flags # Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided. + +uint32 time_week_ms # [ms] GPS time (from start of GPS week) +uint16 time_week # GPS week number +int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid) +int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid) +float32 alt # [m] Altitude (MSL). Positive for up. + +float32 hdop # [m] GPS HDOP horizontal dilution of position. +float32 vdop # [m] GPS VDOP vertical dilution of position +float32 vn # [m/s] GPS velocity in NORTH direction in earth-fixed NED frame +float32 ve # [m/s] GPS velocity in EAST direction in earth-fixed NED frame +float32 vd # [m/s] GPS velocity in DOWN direction in earth-fixed NED frame + +float32 speed_accuracy # [m/s] GPS speed accuracy +float32 horiz_accuracy # [m] GPS horizontal accuracy +float32 vert_accuracy # [m] GPS vertical accuracy + +uint8 satellites_visible # Number of satellites visible. If unknown, set to 255 +uint16 yaw # [cdeg] Yaw in earth frame from north. diff --git a/src/external/mavros_msgs/msg/GPSRAW.msg b/src/external/mavros_msgs/msg/GPSRAW.msg new file mode 100644 index 0000000..5b754b3 --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSRAW.msg @@ -0,0 +1,37 @@ +# FCU GPS RAW message for the gps_status plugin +# A merge of mavlink GPS_RAW_INT and +# mavlink GPS2_RAW messages. + +std_msgs/Header header +## GPS_FIX_TYPE enum +uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected +uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected +uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position +uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position +uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position +uint8 GPS_FIX_TYPE_RTK_FLOAT = 5 # RTK float, 3D position +uint8 GPS_FIX_TYPE_RTK_FIXED = 6 # RTK Fixed, 3D position +uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations +uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position +uint8 fix_type # [GPS_FIX_TYPE] GPS fix type + +int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid) +int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid) +int32 alt # [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. +uint16 eph # GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX +uint16 epv # GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX +uint16 vel # [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX +uint16 cog # [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX +uint8 satellites_visible # Number of satellites visible. If unknown, set to 255 + +# -*- only available with MAVLink v2.0 and GPS_RAW_INT messages -*- +int32 alt_ellipsoid # [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. +uint32 h_acc # [mm] Position uncertainty. Positive for up. +uint32 v_acc # [mm] Altitude uncertainty. Positive for up. +uint32 vel_acc # [mm] Speed uncertainty. Positive for up. +int32 hdg_acc # [degE5] Heading / track uncertainty +uint16 yaw # [cdeg] Yaw in earth frame from north. + +# -*- only available with MAVLink v2.0 and GPS2_RAW messages -*- +uint8 dgps_numch # Number of DGPS satellites +uint32 dgps_age # [ms] Age of DGPS info diff --git a/src/external/mavros_msgs/msg/GPSRTK.msg b/src/external/mavros_msgs/msg/GPSRTK.msg new file mode 100644 index 0000000..2cac818 --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSRTK.msg @@ -0,0 +1,18 @@ +# FCU GPS RTK message for the gps_status plugin +# A copy of mavlink GPS_RTK message + +std_msgs/Header header + +uint8 rtk_receiver_id # Identification of connected RTK receiver. +int16 wn # GPS Week Number of last baseline. +uint32 tow # [ms] GPS Time of Week of last baseline. +uint8 rtk_health # GPS-specific health report for RTK data. +uint8 rtk_rate # [Hz] Rate of baseline messages being received by GPS. +uint8 nsats # Current number of sats used for RTK calculation. +int32 baseline_a # [mm] Current baseline in ECEF x or NED north component, depends on header.frame_id. +int32 baseline_b # [mm] Current baseline in ECEF y or NED east component, depends on header.frame_id. +int32 baseline_c # [mm] Current baseline in ECEF z or NED down component, depends on header.frame_id. +uint32 accuracy # Current estimate of baseline accuracy. +int32 iar_num_hypotheses # Current number of integer ambiguity hypotheses. + + diff --git a/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg new file mode 100644 index 0000000..612375d --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg @@ -0,0 +1,32 @@ +# MAVLink message: GIMBAL_DEVICE_ATTITUDE_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS + +std_msgs/Header header + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint16 flags # Current gimbal flags set (bitwise) - See GIMBAL_DEVICE_FLAGS +#GIMBAL_DEVICE_FLAGS +uint16 FLAGS_RETRACT = 1 # Set to retracted safe position (no stabilization), takes presedence over all other flags. +uint16 FLAGS_NEUTRAL = 2 # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation. +uint16 FLAGS_ROLL_LOCK = 4 # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal. +uint16 FLAGS_PITCH_LOCK = 8 # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default. +uint16 FLAGS_YAW_LOCK = 16 # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle). + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity (NaN if unknown) +float32 angular_velocity_y # Y component of angular velocity (NaN if unknown) +float32 angular_velocity_z # Z component of angular velocity (NaN if unknown) + +uint32 failure_flags # Failure flags (0 for no failure) (bitwise) - See GIMBAL_DEVICE_ERROR_FLAGS +#GIMBAL_DEVICE_ERROR_FLAGS +uint32 ERROR_FLAGS_AT_ROLL_LIMIT = 1 # Gimbal device is limited by hardware roll limit. +uint32 ERROR_FLAGS_AT_PITCH_LIMIT = 2 # Gimbal device is limited by hardware pitch limit. +uint32 ERROR_FLAGS_AT_YAW_LIMIT = 4 # Gimbal device is limited by hardware yaw limit. +uint32 ERROR_FLAGS_ENCODER_ERROR = 8 # There is an error with the gimbal encoders. +uint32 ERROR_FLAGS_POWER_ERROR = 16 # There is an error with the gimbal power source. +uint32 ERROR_FLAGS_MOTOR_ERROR = 32 # There is an error with the gimbal motor's. +uint32 ERROR_FLAGS_SOFTWARE_ERROR = 64 # There is an error with the gimbal's software. +uint32 ERROR_FLAGS_COMMS_ERROR = 128 # There is an error with the gimbal's communication. +uint32 ERROR_FLAGS_CALIBRATION_RUNNING = 256 # Gimbal is currently calibrating. diff --git a/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg new file mode 100644 index 0000000..0d68309 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg @@ -0,0 +1,34 @@ +# MAVLink message: GIMBAL_DEVICE_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +std_msgs/Header header + +string vendor_name # Name of the gimbal vendor. +string model_name # Name of the gimbal model. +string custom_name # Custom name of the gimbal given to it by the user. +uint32 firmware_version # Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +uint32 hardware_version # Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +uint64 uid # UID of gimbal hardware (0 if unknown). + +uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_DEVICE_CAP_FLAGS +#GIMBAL_DEVICE_CAP_FLAGS +uint32 CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position +uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized +uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis. +uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle +uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized) +uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis. +uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle +uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized) +uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis. +uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default) +uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available) +uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk). + +uint16 custom_cap_flags # Bitmap for use for gimbal-specific capability flags. +float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 pitch_min # Minimum pitch angle (positive: up, negative: down) +float32 pitch_max # Maximum pitch angle (positive: up, negative: down) +float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left) +float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg new file mode 100644 index 0000000..fdbc73d --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg @@ -0,0 +1,18 @@ +# MAVLink message: GIMBAL_DEVICE_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint16 flags # Low level gimbal flags (bitwise) - See GIMBAL_DEVICE_FLAGS +#GIMBAL_DEVICE_FLAGS +uint16 FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint16 FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint16 FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint16 FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint16 FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerInformation.msg b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg new file mode 100644 index 0000000..38dbc88 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg @@ -0,0 +1,29 @@ +# MAVLink message: GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION + +std_msgs/Header header + +uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_MANAGER_CAP_FLAGS +#GIMBAL_MANAGER_CAP_FLAGS +uint32 CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT. +uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL. +uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS. +uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW. +uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK. +uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS. +uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW. +uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK. +uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS. +uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW. +uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK. +uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. +uint32 CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position. +uint32 CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position. + +uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for. +float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 pitch_min # Minimum pitch angle (positive: up, negative: down) +float32 pitch_max # Maximum pitch angle (positive: up, negative: down) +float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left) +float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg new file mode 100644 index 0000000..8bbec37 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg @@ -0,0 +1,24 @@ +# MAVLink message: GIMBAL_MANAGER_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use (bitwise) - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg new file mode 100644 index 0000000..e87874e --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg @@ -0,0 +1,27 @@ +# MAVLink message: GIMBAL_MANAGER_SET_PITCHYAW +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW +# Note that this message structure is identical also to GIMBAL_MANAGER_SET_MANUAL_CONTROL and is +# reused as such by the plugin +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +float32 pitch # Pitch angle (positive: up, negative: down, NaN to be ignored). +float32 yaw # Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). +float32 pitch_rate # Pitch angular rate (positive: up, negative: down, NaN to be ignored). +float32 yaw_rate # Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerStatus.msg b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg new file mode 100644 index 0000000..8df8b3e --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg @@ -0,0 +1,19 @@ +# MAVLink message: GIMBAL_MANAGER_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS + +std_msgs/Header header + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for. + +uint8 sysid_primary # System ID of MAVLink component with primary control, 0 for none. +uint8 compid_primary # Component ID of MAVLink component with primary control, 0 for none. +uint8 sysid_secondary # System ID of MAVLink component with secondary control, 0 for none. +uint8 compid_secondary # Component ID of MAVLink component with secondary control, 0 for none. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GlobalPositionTarget.msg b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg new file mode 100644 index 0000000..445a1a7 --- /dev/null +++ b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg @@ -0,0 +1,34 @@ +# Message for SET_POSITION_TARGET_GLOBAL_INT +# +# https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT +# Some complex system requires all feautures that mavlink +# message provide. See issue #402, #415. + +std_msgs/Header header + +uint8 coordinate_frame +uint8 FRAME_GLOBAL_INT = 5 +uint8 FRAME_GLOBAL_REL_ALT = 6 +uint8 FRAME_GLOBAL_TERRAIN_ALT = 11 + +uint16 type_mask +uint16 IGNORE_LATITUDE = 1 # Position ignore flags +uint16 IGNORE_LONGITUDE = 2 +uint16 IGNORE_ALTITUDE = 4 +uint16 IGNORE_VX = 8 # Velocity vector ignore flags +uint16 IGNORE_VY = 16 +uint16 IGNORE_VZ = 32 +uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags +uint16 IGNORE_AFY = 128 +uint16 IGNORE_AFZ = 256 +uint16 FORCE = 512 # Force in af vector flag +uint16 IGNORE_YAW = 1024 +uint16 IGNORE_YAW_RATE = 2048 + +float64 latitude +float64 longitude +float32 altitude # in meters, AMSL or above terrain +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration_or_force +float32 yaw +float32 yaw_rate diff --git a/src/external/mavros_msgs/msg/HilActuatorControls.msg b/src/external/mavros_msgs/msg/HilActuatorControls.msg new file mode 100644 index 0000000..755384f --- /dev/null +++ b/src/external/mavros_msgs/msg/HilActuatorControls.msg @@ -0,0 +1,10 @@ +# HilActuatorControls.msg +# +# ROS representation of MAVLink HIL_ACTUATOR_CONTROLS +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS + +std_msgs/Header header +float32[16] controls +uint8 mode +uint64 flags diff --git a/src/external/mavros_msgs/msg/HilControls.msg b/src/external/mavros_msgs/msg/HilControls.msg new file mode 100644 index 0000000..a953aff --- /dev/null +++ b/src/external/mavros_msgs/msg/HilControls.msg @@ -0,0 +1,18 @@ +# HilControls.msg +# +# ROS representation of MAVLink HIL_CONTROLS +# (deprecated, use HIL_ACTUATOR_CONTROLS instead) +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_CONTROLS + +std_msgs/Header header +float32 roll_ailerons +float32 pitch_elevator +float32 yaw_rudder +float32 throttle +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +uint8 mode +uint8 nav_mode diff --git a/src/external/mavros_msgs/msg/HilGPS.msg b/src/external/mavros_msgs/msg/HilGPS.msg new file mode 100644 index 0000000..d74c204 --- /dev/null +++ b/src/external/mavros_msgs/msg/HilGPS.msg @@ -0,0 +1,17 @@ +# HilControls.msg +# +# ROS representation of MAVLink HIL_GPS +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_GPS + +std_msgs/Header header +uint8 fix_type +geographic_msgs/GeoPoint geo +uint16 eph +uint16 epv +uint16 vel +int16 vn +int16 ve +int16 vd +uint16 cog +uint8 satellites_visible diff --git a/src/external/mavros_msgs/msg/HilSensor.msg b/src/external/mavros_msgs/msg/HilSensor.msg new file mode 100644 index 0000000..c7ceccb --- /dev/null +++ b/src/external/mavros_msgs/msg/HilSensor.msg @@ -0,0 +1,16 @@ +# HilSensor.msg +# +# ROS representation of MAVLink HIL_SENSOR +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_SENSOR + +std_msgs/Header header + +geometry_msgs/Vector3 acc +geometry_msgs/Vector3 gyro +geometry_msgs/Vector3 mag +float32 abs_pressure +float32 diff_pressure +float32 pressure_alt +float32 temperature +uint32 fields_updated diff --git a/src/external/mavros_msgs/msg/HilStateQuaternion.msg b/src/external/mavros_msgs/msg/HilStateQuaternion.msg new file mode 100644 index 0000000..980db1e --- /dev/null +++ b/src/external/mavros_msgs/msg/HilStateQuaternion.msg @@ -0,0 +1,15 @@ +# HilStateQuaternion.msg +# +# ROS representation of MAVLink HIL_STATE_QUATERNION +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION + +std_msgs/Header header + +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 linear_velocity +geographic_msgs/GeoPoint geo +float32 ind_airspeed +float32 true_airspeed diff --git a/src/external/mavros_msgs/msg/HomePosition.msg b/src/external/mavros_msgs/msg/HomePosition.msg new file mode 100644 index 0000000..f73bb51 --- /dev/null +++ b/src/external/mavros_msgs/msg/HomePosition.msg @@ -0,0 +1,10 @@ +# MAVLink message: HOME_POSITION +# https://mavlink.io/en/messages/common.html#HOME_POSITION + +std_msgs/Header header + +geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum + +geometry_msgs/Point position # local position +geometry_msgs/Quaternion orientation # XXX: verify field name (q[4]) +geometry_msgs/Vector3 approach # position of the end of approach vector diff --git a/src/external/mavros_msgs/msg/LandingTarget.msg b/src/external/mavros_msgs/msg/LandingTarget.msg new file mode 100644 index 0000000..d5e6bd0 --- /dev/null +++ b/src/external/mavros_msgs/msg/LandingTarget.msg @@ -0,0 +1,32 @@ +# MAVLink message: LANDING_TARGET +# https://mavlink.io/en/messages/common.html + +std_msgs/Header header + +## MAV_FRAME enum +uint8 GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) +uint8 LOCAL_NED = 2 # Local coordinate frame, Z-up (x: north, y: east, z: down). +uint8 MISSION = 3 # NOT a coordinate frame, indicates a mission command. +uint8 GLOBAL_RELATIVE_ALT = 4 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 LOCAL_ENU = 5 # Local coordinate frame, Z-down (x: east, y: north, z: up) +uint8 GLOBAL_INT = 6 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) +uint8 GLOBAL_RELATIVE_ALT_INT = 7 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 LOCAL_OFFSET_NED = 8 # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. +uint8 BODY_NED = 9 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. +uint8 BODY_OFFSET_NED = 10 # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. +uint8 GLOBAL_TERRAIN_ALT = 11 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 GLOBAL_TERRAIN_ALT_INT = 12 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + +## LANDING_TARGET_TYPE enum +uint8 LIGHT_BEACON = 0 # Landing target signaled by light beacon (ex: IR-LOCK) +uint8 RADIO_BEACON = 1 # Landing target signaled by radio beacon (ex: ILS, NDB) +uint8 VISION_FIDUCIAL = 2 # Landing target represented by a fiducial marker (ex: ARTag) +uint8 VISION_OTHER = 3 # Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) + +uint8 target_num +uint8 frame +float32[2] angle +float32 distance +float32[2] size +geometry_msgs/Pose pose +uint8 type diff --git a/src/external/mavros_msgs/msg/LogData.msg b/src/external/mavros_msgs/msg/LogData.msg new file mode 100644 index 0000000..829a0a3 --- /dev/null +++ b/src/external/mavros_msgs/msg/LogData.msg @@ -0,0 +1,11 @@ +# Reply to LogRequestData, - a chunk of a log +# +# :id: - log id +# :offset: - offset into the log +# :data: - chunk of data (if zero-sized, then there are no more chunks) + +std_msgs/Header header + +uint16 id +uint32 offset +uint8[] data diff --git a/src/external/mavros_msgs/msg/LogEntry.msg b/src/external/mavros_msgs/msg/LogEntry.msg new file mode 100644 index 0000000..b212686 --- /dev/null +++ b/src/external/mavros_msgs/msg/LogEntry.msg @@ -0,0 +1,15 @@ +# Information about a single log +# +# :id: - log id +# :num_logs: - total number of logs +# :last_log_num: - id of last log +# :time_utc: - UTC timestamp of log (ros::Time(0) if not available) +# :size: - size of log in bytes (may be approximate) + +std_msgs/Header header + +uint16 id +uint16 num_logs +uint16 last_log_num +builtin_interfaces/Time time_utc +uint32 size diff --git a/src/external/mavros_msgs/msg/MagnetometerReporter.msg b/src/external/mavros_msgs/msg/MagnetometerReporter.msg new file mode 100644 index 0000000..593f5f6 --- /dev/null +++ b/src/external/mavros_msgs/msg/MagnetometerReporter.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +uint8 report +float32 confidence \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/ManualControl.msg b/src/external/mavros_msgs/msg/ManualControl.msg new file mode 100644 index 0000000..f6db210 --- /dev/null +++ b/src/external/mavros_msgs/msg/ManualControl.msg @@ -0,0 +1,7 @@ +# Manual Control state +std_msgs/Header header +float32 x +float32 y +float32 z +float32 r +uint16 buttons diff --git a/src/external/mavros_msgs/msg/Mavlink.msg b/src/external/mavros_msgs/msg/Mavlink.msg new file mode 100644 index 0000000..afb80ed --- /dev/null +++ b/src/external/mavros_msgs/msg/Mavlink.msg @@ -0,0 +1,38 @@ +# Mavlink message transport type. +# +# Used to transport mavlink_message_t via ROS topic +# +# :framing_status: +# Frame decoding status: OK, CRC error, bad Signature (mavlink v2.0) +# You may simply drop all non valid messages. +# Used for GCS Bridge to transport unknown messages. +# +# :magic: +# STX byte, used to determine protocol version v1.0 or v2.0. +# +# Please use mavros_msgs::mavlink::convert() from +# to convert between ROS and MAVLink message type + +# mavlink_framing_t enum +uint8 FRAMING_OK = 1 +uint8 FRAMING_BAD_CRC = 2 +uint8 FRAMING_BAD_SIGNATURE = 3 + +# stx values +uint8 MAVLINK_V10 = 254 +uint8 MAVLINK_V20 = 253 + +std_msgs/Header header +uint8 framing_status + +uint8 magic # STX byte +uint8 len +uint8 incompat_flags +uint8 compat_flags +uint8 seq +uint8 sysid +uint8 compid +uint32 msgid # 24-bit message id +uint16 checksum +uint64[<=33] payload64 # max size: (255+2+7)/8 +uint8[<=13] signature # optional signature, max size: 13 diff --git a/src/external/mavros_msgs/msg/MountControl.msg b/src/external/mavros_msgs/msg/MountControl.msg new file mode 100644 index 0000000..0c2ee72 --- /dev/null +++ b/src/external/mavros_msgs/msg/MountControl.msg @@ -0,0 +1,18 @@ +# MAVLink message: DO_MOUNT_CONTROL +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL + +std_msgs/Header header + +uint8 mode # See enum MAV_MOUNT_MODE. +uint8 MAV_MOUNT_MODE_RETRACT = 0 +uint8 MAV_MOUNT_MODE_NEUTRAL = 1 +uint8 MAV_MOUNT_MODE_MAVLINK_TARGETING = 2 +uint8 MAV_MOUNT_MODE_RC_TARGETING = 3 +uint8 MAV_MOUNT_MODE_GPS_POINT = 4 + +float32 pitch # pitch degrees or degrees/second depending on mount mode. +float32 roll # roll degrees or degrees/second depending on mount mode. +float32 yaw # yaw degrees or degrees/second depending on mount mode. +float32 altitude # altitude depending on mount mode. +float32 latitude # latitude in degrees * 1E7, set if appropriate mount mode. +float32 longitude # longitude in degrees * 1E7, set if appropriate mount mode. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/NavControllerOutput.msg b/src/external/mavros_msgs/msg/NavControllerOutput.msg new file mode 100644 index 0000000..d5f8f6d --- /dev/null +++ b/src/external/mavros_msgs/msg/NavControllerOutput.msg @@ -0,0 +1,12 @@ +# https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT + +std_msgs/Header header + +float32 nav_roll # Current desired roll +float32 nav_pitch # Current desired pitch +int16 nav_bearing # Current desired heading +int16 target_bearing # Bearing to current waypoint/target +uint16 wp_dist # Distance to active waypoint +float32 alt_error # Current altitude error +float32 aspd_error # Current airspeed error +float32 xtrack_error # Current crosstrack error on x-y plane \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/OnboardComputerStatus.msg b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg new file mode 100644 index 0000000..d6d06b0 --- /dev/null +++ b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg @@ -0,0 +1,25 @@ +# Mavros message: ONBOARDCOMPUTERSTATUS + +std_msgs/Header header + +uint8 component # See enum MAV_COMPONENT + +uint32 uptime # [ms] Time since system boot +uint8 type # Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. +uint8[8] cpu_cores # CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. +uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused +uint8[4] gpu_cores # GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused +uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. +int8 temperature_board # [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. +int8[8] temperature_core # [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. +int16[4] fan_speed # [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. +uint32 ram_usage # [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. +uint32 ram_total # [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. +uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. +uint32[4] storage_usage # [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. +uint32[4] storage_total # [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_type # Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary. +uint32[6] link_tx_rate # [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_rx_rate # [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_tx_max # [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_rx_max # [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/OpticalFlow.msg b/src/external/mavros_msgs/msg/OpticalFlow.msg new file mode 100644 index 0000000..db857b8 --- /dev/null +++ b/src/external/mavros_msgs/msg/OpticalFlow.msg @@ -0,0 +1,9 @@ +# OPTICAL_FLOW message data + +std_msgs/Header header + +geometry_msgs/Vector3 flow +geometry_msgs/Vector3 flow_comp_m +uint8 quality +float32 ground_distance +geometry_msgs/Vector3 flow_rate diff --git a/src/external/mavros_msgs/msg/OpticalFlowRad.msg b/src/external/mavros_msgs/msg/OpticalFlowRad.msg new file mode 100644 index 0000000..f773fac --- /dev/null +++ b/src/external/mavros_msgs/msg/OpticalFlowRad.msg @@ -0,0 +1,14 @@ +# OPTICAL_FLOW_RAD message data + +std_msgs/Header header + +uint32 integration_time_us +float32 integrated_x +float32 integrated_y +float32 integrated_xgyro +float32 integrated_ygyro +float32 integrated_zgyro +int16 temperature +uint8 quality +uint32 time_delta_distance_us +float32 distance diff --git a/src/external/mavros_msgs/msg/OverrideRCIn.msg b/src/external/mavros_msgs/msg/OverrideRCIn.msg new file mode 100644 index 0000000..81bc040 --- /dev/null +++ b/src/external/mavros_msgs/msg/OverrideRCIn.msg @@ -0,0 +1,9 @@ +# Override RC Input +# Currently MAVLink defines override for 18 channels + +# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE + +uint16 CHAN_RELEASE=0 +uint16 CHAN_NOCHANGE=65535 + +uint16[18] channels diff --git a/src/external/mavros_msgs/msg/Param.msg b/src/external/mavros_msgs/msg/Param.msg new file mode 100644 index 0000000..f2ccfaa --- /dev/null +++ b/src/external/mavros_msgs/msg/Param.msg @@ -0,0 +1,11 @@ +# Parameter msg. + +# XXX DEPRECATED: replaced by ParamEvent + +std_msgs/Header header + +string param_id +mavros_msgs/ParamValue value + +uint16 param_index +uint16 param_count diff --git a/src/external/mavros_msgs/msg/ParamEvent.msg b/src/external/mavros_msgs/msg/ParamEvent.msg new file mode 100644 index 0000000..493df0a --- /dev/null +++ b/src/external/mavros_msgs/msg/ParamEvent.msg @@ -0,0 +1,14 @@ +# Parameter Event +# +# That messages replaces mavros_msgs/Param from mavros v1. +# Reason for that: ROS2 have native message for parameters +# +# ROS2 also have it's own ParameterEvent stream, which could be used +# to get FCU updates too. But that message is simpler to use. + +std_msgs/Header header + +string param_id +rcl_interfaces/ParameterValue value +uint16 param_index +uint16 param_count diff --git a/src/external/mavros_msgs/msg/ParamValue.msg b/src/external/mavros_msgs/msg/ParamValue.msg new file mode 100644 index 0000000..4a98639 --- /dev/null +++ b/src/external/mavros_msgs/msg/ParamValue.msg @@ -0,0 +1,12 @@ +# Parameter value storage type. +# +# Integer and float fields: +# +# if integer != 0: it is integer value +# else if real != 0.0: it is float value +# else: it is zero. + +# XXX DEPRECATED: replaced by rmw_interfaces/ParameterValue + +int64 integer +float64 real diff --git a/src/external/mavros_msgs/msg/PlayTuneV2.msg b/src/external/mavros_msgs/msg/PlayTuneV2.msg new file mode 100644 index 0000000..d92a976 --- /dev/null +++ b/src/external/mavros_msgs/msg/PlayTuneV2.msg @@ -0,0 +1,10 @@ +# Play tune V2 +# +# https://mavlink.io/en/messages/common.html#PLAY_TUNE_V2 + +## TUNE_FORMAT enum +uint8 QBASIC1_1 = 1 +uint8 MML_MODERN = 2 + +uint8 format +string tune diff --git a/src/external/mavros_msgs/msg/PositionTarget.msg b/src/external/mavros_msgs/msg/PositionTarget.msg new file mode 100644 index 0000000..680be87 --- /dev/null +++ b/src/external/mavros_msgs/msg/PositionTarget.msg @@ -0,0 +1,32 @@ +# Message for SET_POSITION_TARGET_LOCAL_NED +# +# Some complex system requires all feautures that mavlink +# message provide. See issue #402. + +std_msgs/Header header + +uint8 coordinate_frame +uint8 FRAME_LOCAL_NED = 1 +uint8 FRAME_LOCAL_OFFSET_NED = 7 +uint8 FRAME_BODY_NED = 8 +uint8 FRAME_BODY_OFFSET_NED = 9 + +uint16 type_mask +uint16 IGNORE_PX = 1 # Position ignore flags +uint16 IGNORE_PY = 2 +uint16 IGNORE_PZ = 4 +uint16 IGNORE_VX = 8 # Velocity vector ignore flags +uint16 IGNORE_VY = 16 +uint16 IGNORE_VZ = 32 +uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags +uint16 IGNORE_AFY = 128 +uint16 IGNORE_AFZ = 256 +uint16 FORCE = 512 # Force in af vector flag +uint16 IGNORE_YAW = 1024 +uint16 IGNORE_YAW_RATE = 2048 + +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration_or_force +float32 yaw +float32 yaw_rate diff --git a/src/external/mavros_msgs/msg/RCIn.msg b/src/external/mavros_msgs/msg/RCIn.msg new file mode 100644 index 0000000..2a38d9c --- /dev/null +++ b/src/external/mavros_msgs/msg/RCIn.msg @@ -0,0 +1,5 @@ +# RAW RC input state + +std_msgs/Header header +uint8 rssi +uint16[] channels diff --git a/src/external/mavros_msgs/msg/RCOut.msg b/src/external/mavros_msgs/msg/RCOut.msg new file mode 100644 index 0000000..5ba3d15 --- /dev/null +++ b/src/external/mavros_msgs/msg/RCOut.msg @@ -0,0 +1,4 @@ +# RAW Servo out state + +std_msgs/Header header +uint16[] channels diff --git a/src/external/mavros_msgs/msg/RTCM.msg b/src/external/mavros_msgs/msg/RTCM.msg new file mode 100644 index 0000000..8c74498 --- /dev/null +++ b/src/external/mavros_msgs/msg/RTCM.msg @@ -0,0 +1,6 @@ +# RTCM message for the gps_rtk plugin +# The gps_rtk plugin will fragment the data if necessary and +# forward it to the FCU via Mavlink through the available link. +# data should be <= 4*180, higher will be discarded. +std_msgs/Header header +uint8[] data diff --git a/src/external/mavros_msgs/msg/RTKBaseline.msg b/src/external/mavros_msgs/msg/RTKBaseline.msg new file mode 100644 index 0000000..cbbd386 --- /dev/null +++ b/src/external/mavros_msgs/msg/RTKBaseline.msg @@ -0,0 +1,23 @@ +# RTKBaseline received from the FCU. +# Full description: https://mavlink.io/en/messages/common.html#GPS_RTK +# Mavlink Common, #127and #128 + +std_msgs/Header header + +uint32 time_last_baseline_ms +uint8 rtk_receiver_id +uint16 wn +uint32 tow +uint8 rtk_health +uint8 rtk_rate +uint8 nsats + +uint8 baseline_coords_type +uint8 RTK_BASELINE_COORDINATE_SYSTEM_ECEF = 0 # Earth-centered, earth-fixed +uint8 RTK_BASELINE_COORDINATE_SYSTEM_NED = 1 # RTK basestation centered, north, east, down + +int32 baseline_a_mm +int32 baseline_b_mm +int32 baseline_c_mm +uint32 accuracy +int32 iar_num_hypotheses diff --git a/src/external/mavros_msgs/msg/RadioStatus.msg b/src/external/mavros_msgs/msg/RadioStatus.msg new file mode 100644 index 0000000..dc7d3b4 --- /dev/null +++ b/src/external/mavros_msgs/msg/RadioStatus.msg @@ -0,0 +1,16 @@ +# RADIO_STATUS message + +std_msgs/Header header + +# message data +uint8 rssi +uint8 remrssi +uint8 txbuf +uint8 noise +uint8 remnoise +uint16 rxerrors +uint16 fixed + +# calculated +float32 rssi_dbm +float32 remrssi_dbm diff --git a/src/external/mavros_msgs/msg/State.msg b/src/external/mavros_msgs/msg/State.msg new file mode 100644 index 0000000..db00886 --- /dev/null +++ b/src/external/mavros_msgs/msg/State.msg @@ -0,0 +1,82 @@ +# Current autopilot state +# +# Known modes listed here: +# http://wiki.ros.org/mavros/CustomModes +# +# For system_status values +# see https://mavlink.io/en/messages/common.html#MAV_STATE +# + +std_msgs/Header header +bool connected +bool armed +bool guided +bool manual_input +string mode +uint8 system_status + +string MODE_APM_PLANE_MANUAL = MANUAL +string MODE_APM_PLANE_CIRCLE = CIRCLE +string MODE_APM_PLANE_STABILIZE = STABILIZE +string MODE_APM_PLANE_TRAINING = TRAINING +string MODE_APM_PLANE_ACRO = ACRO +string MODE_APM_PLANE_FBWA = FBWA +string MODE_APM_PLANE_FBWB = FBWB +string MODE_APM_PLANE_CRUISE = CRUISE +string MODE_APM_PLANE_AUTOTUNE = AUTOTUNE +string MODE_APM_PLANE_AUTO = AUTO +string MODE_APM_PLANE_RTL = RTL +string MODE_APM_PLANE_LOITER = LOITER +string MODE_APM_PLANE_LAND = LAND +string MODE_APM_PLANE_GUIDED = GUIDED +string MODE_APM_PLANE_INITIALISING = INITIALISING +string MODE_APM_PLANE_QSTABILIZE = QSTABILIZE +string MODE_APM_PLANE_QHOVER = QHOVER +string MODE_APM_PLANE_QLOITER = QLOITER +string MODE_APM_PLANE_QLAND = QLAND +string MODE_APM_PLANE_QRTL = QRTL + +string MODE_APM_COPTER_STABILIZE = STABILIZE +string MODE_APM_COPTER_ACRO = ACRO +string MODE_APM_COPTER_ALT_HOLD = ALT_HOLD +string MODE_APM_COPTER_AUTO = AUTO +string MODE_APM_COPTER_GUIDED = GUIDED +string MODE_APM_COPTER_LOITER = LOITER +string MODE_APM_COPTER_RTL = RTL +string MODE_APM_COPTER_CIRCLE = CIRCLE +string MODE_APM_COPTER_POSITION = POSITION +string MODE_APM_COPTER_LAND = LAND +string MODE_APM_COPTER_OF_LOITER = OF_LOITER +string MODE_APM_COPTER_DRIFT = DRIFT +string MODE_APM_COPTER_SPORT = SPORT +string MODE_APM_COPTER_FLIP = FLIP +string MODE_APM_COPTER_AUTOTUNE = AUTOTUNE +string MODE_APM_COPTER_POSHOLD = POSHOLD +string MODE_APM_COPTER_BRAKE = BRAKE +string MODE_APM_COPTER_THROW = THROW +string MODE_APM_COPTER_AVOID_ADSB = AVOID_ADSB +string MODE_APM_COPTER_GUIDED_NOGPS = GUIDED_NOGPS + +string MODE_APM_ROVER_MANUAL = MANUAL +string MODE_APM_ROVER_LEARNING = LEARNING +string MODE_APM_ROVER_STEERING = STEERING +string MODE_APM_ROVER_HOLD = HOLD +string MODE_APM_ROVER_AUTO = AUTO +string MODE_APM_ROVER_RTL = RTL +string MODE_APM_ROVER_GUIDED = GUIDED +string MODE_APM_ROVER_INITIALISING = INITIALISING + +string MODE_PX4_MANUAL = MANUAL +string MODE_PX4_ACRO = ACRO +string MODE_PX4_ALTITUDE = ALTCTL +string MODE_PX4_POSITION = POSCTL +string MODE_PX4_OFFBOARD = OFFBOARD +string MODE_PX4_STABILIZED = STABILIZED +string MODE_PX4_RATTITUDE = RATTITUDE +string MODE_PX4_MISSION = AUTO.MISSION +string MODE_PX4_LOITER = AUTO.LOITER +string MODE_PX4_RTL = AUTO.RTL +string MODE_PX4_LAND = AUTO.LAND +string MODE_PX4_RTGS = AUTO.RTGS +string MODE_PX4_READY = AUTO.READY +string MODE_PX4_TAKEOFF = AUTO.TAKEOFF diff --git a/src/external/mavros_msgs/msg/StatusEvent.msg b/src/external/mavros_msgs/msg/StatusEvent.msg new file mode 100644 index 0000000..3f90aea --- /dev/null +++ b/src/external/mavros_msgs/msg/StatusEvent.msg @@ -0,0 +1,19 @@ +# EVENT message representation +# https://mavlink.io/en/messages/common.html#EVENT + +# Severity levels +uint8 EMERGENCY = 0 +uint8 ALERT = 1 +uint8 CRITICAL = 2 +uint8 ERROR = 3 +uint8 WARNING = 4 +uint8 NOTICE = 5 +uint8 INFO = 6 +uint8 DEBUG = 7 + +# Fields +std_msgs/Header header +uint8 severity +uint32 px4_id +uint8[40] arguments +uint16 sequence diff --git a/src/external/mavros_msgs/msg/StatusText.msg b/src/external/mavros_msgs/msg/StatusText.msg new file mode 100644 index 0000000..901b869 --- /dev/null +++ b/src/external/mavros_msgs/msg/StatusText.msg @@ -0,0 +1,17 @@ +# STATUSTEXT message representation +# https://mavlink.io/en/messages/common.html#STATUSTEXT + +# Severity levels +uint8 EMERGENCY = 0 +uint8 ALERT = 1 +uint8 CRITICAL = 2 +uint8 ERROR = 3 +uint8 WARNING = 4 +uint8 NOTICE = 5 +uint8 INFO = 6 +uint8 DEBUG = 7 + +# Fields +std_msgs/Header header +uint8 severity +string text diff --git a/src/external/mavros_msgs/msg/SysStatus.msg b/src/external/mavros_msgs/msg/SysStatus.msg new file mode 100644 index 0000000..551940a --- /dev/null +++ b/src/external/mavros_msgs/msg/SysStatus.msg @@ -0,0 +1,15 @@ +std_msgs/Header header + +uint32 sensors_present +uint32 sensors_enabled +uint32 sensors_health +uint16 load +uint16 voltage_battery +int16 current_battery +int8 battery_remaining +uint16 drop_rate_comm +uint16 errors_comm +uint16 errors_count1 +uint16 errors_count2 +uint16 errors_count3 +uint16 errors_count4 \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/TerrainReport.msg b/src/external/mavros_msgs/msg/TerrainReport.msg new file mode 100644 index 0000000..5c9efce --- /dev/null +++ b/src/external/mavros_msgs/msg/TerrainReport.msg @@ -0,0 +1,12 @@ +# Message for TERRAIN_REPORT +# https://mavlink.io/en/messages/common.html#TERRAIN_REPORT + +std_msgs/Header header + +float64 latitude +float64 longitude +uint16 spacing +float32 terrain_height # in meters, terrain height +float32 current_height # in meters, vehicle height above terrain +uint16 pending +uint16 loaded diff --git a/src/external/mavros_msgs/msg/Thrust.msg b/src/external/mavros_msgs/msg/Thrust.msg new file mode 100644 index 0000000..fa2fcd6 --- /dev/null +++ b/src/external/mavros_msgs/msg/Thrust.msg @@ -0,0 +1,5 @@ +# Thrust to send to the FCU + +std_msgs/Header header + +float32 thrust diff --git a/src/external/mavros_msgs/msg/TimesyncStatus.msg b/src/external/mavros_msgs/msg/TimesyncStatus.msg new file mode 100644 index 0000000..dbfbf30 --- /dev/null +++ b/src/external/mavros_msgs/msg/TimesyncStatus.msg @@ -0,0 +1,7 @@ +# Status of the MAVLink time synchroniser + +std_msgs/Header header +uint64 remote_timestamp_ns # remote system timestamp (nanoseconds) +int64 observed_offset_ns # raw time offset directly observed from this timesync packet (nanoseconds) +int64 estimated_offset_ns # smoothed time offset between companion system and Mavros (nanoseconds) +float32 round_trip_time_ms # round trip time of this timesync packet (milliseconds) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/Trajectory.msg b/src/external/mavros_msgs/msg/Trajectory.msg new file mode 100644 index 0000000..c43f63a --- /dev/null +++ b/src/external/mavros_msgs/msg/Trajectory.msg @@ -0,0 +1,19 @@ +# MAVLink message: TRAJECTORY +# https://mavlink.io/en/messages/common.html#TRAJECTORY + +std_msgs/Header header + +uint8 type # See enum MAV_TRAJECTORY_REPRESENTATION. +uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 +uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1 + +mavros_msgs/PositionTarget point_1 +mavros_msgs/PositionTarget point_2 +mavros_msgs/PositionTarget point_3 +mavros_msgs/PositionTarget point_4 +mavros_msgs/PositionTarget point_5 + +uint8[5] point_valid # States if respective point is valid. +uint16[5] command # MAV_CMD associated with each point. UINT16_MAX if unused. + +float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN diff --git a/src/external/mavros_msgs/msg/Tunnel.msg b/src/external/mavros_msgs/msg/Tunnel.msg new file mode 100644 index 0000000..a04dcd1 --- /dev/null +++ b/src/external/mavros_msgs/msg/Tunnel.msg @@ -0,0 +1,27 @@ +# Tunnel +# +# https://mavlink.io/en/messages/common.html#TUNNEL + +uint8 target_system +uint8 target_component +uint16 payload_type +uint8 payload_length +uint8[128] payload + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('MAV_TUNNEL_PAYLOAD_TYPE', 'PAYLOAD_TYPE_', 16) +# ]]] +# MAV_TUNNEL_PAYLOAD_TYPE +uint16 PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown. +uint16 PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller. +# [[[end]]] (checksum: 3327b212af02c2d47d940cd6de049624) diff --git a/src/external/mavros_msgs/msg/VehicleInfo.msg b/src/external/mavros_msgs/msg/VehicleInfo.msg new file mode 100644 index 0000000..ea80b16 --- /dev/null +++ b/src/external/mavros_msgs/msg/VehicleInfo.msg @@ -0,0 +1,31 @@ +# Vehicle Info msg + +std_msgs/Header header + +uint8 HAVE_INFO_HEARTBEAT = 1 +uint8 HAVE_INFO_AUTOPILOT_VERSION = 2 +uint8 available_info # Bitmap shows what info is available + +# Vehicle address +uint8 sysid # SYSTEM ID +uint8 compid # COMPONENT ID + +# -*- Heartbeat info -*- +uint8 autopilot # MAV_AUTOPILOT +uint8 type # MAV_TYPE +uint8 system_status # MAV_STATE +uint8 base_mode +uint32 custom_mode +string mode # MAV_MODE string +uint32 mode_id # MAV_MODE number + +# -*- Autopilot version -*- +uint64 capabilities # MAV_PROTOCOL_CAPABILITY +uint32 flight_sw_version # Firmware version number +uint32 middleware_sw_version # Middleware version number +uint32 os_sw_version # Operating system version number +uint32 board_version # HW / board version (last 8 bytes should be silicon ID, if any) +string flight_custom_version # Custom version field, commonly from the first 8 bytes of the git hash +uint16 vendor_id # ID of the board vendor +uint16 product_id # ID of the product +uint64 uid # UID if provided by hardware diff --git a/src/external/mavros_msgs/msg/VfrHud.msg b/src/external/mavros_msgs/msg/VfrHud.msg new file mode 100644 index 0000000..81d0ba2 --- /dev/null +++ b/src/external/mavros_msgs/msg/VfrHud.msg @@ -0,0 +1,11 @@ +# Metrics typically displayed on a HUD for fixed wing aircraft +# +# VFR_HUD message + +std_msgs/Header header +float32 airspeed # m/s +float32 groundspeed # m/s +int16 heading # degrees 0..360 +float32 throttle # normalized to 0.0..1.0 +float32 altitude # MSL +float32 climb # current climb rate m/s diff --git a/src/external/mavros_msgs/msg/Vibration.msg b/src/external/mavros_msgs/msg/Vibration.msg new file mode 100644 index 0000000..d9170d0 --- /dev/null +++ b/src/external/mavros_msgs/msg/Vibration.msg @@ -0,0 +1,7 @@ +# VIBRATION message data +# @description: Vibration levels and accelerometer clipping + +std_msgs/Header header + +geometry_msgs/Vector3 vibration # 3-axis vibration levels +float32[3] clipping # Accelerometers clipping \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/Waypoint.msg b/src/external/mavros_msgs/msg/Waypoint.msg new file mode 100644 index 0000000..7afa95e --- /dev/null +++ b/src/external/mavros_msgs/msg/Waypoint.msg @@ -0,0 +1,45 @@ +# Waypoint.msg +# +# ROS representation of MAVLink MISSION_ITEM +# See mavlink documentation + + + +# see enum MAV_FRAME +uint8 frame +uint8 FRAME_GLOBAL = 0 +uint8 FRAME_LOCAL_NED = 1 +uint8 FRAME_MISSION = 2 +uint8 FRAME_GLOBAL_REL_ALT = 3 +uint8 FRAME_LOCAL_ENU = 4 +uint8 FRAME_GLOBAL_INT = 5 +uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 +uint8 FRAME_LOCAL_OFFSET_NED = 7 +uint8 FRAME_BODY_NED = 8 +uint8 FRAME_BODY_OFFSET_NED = 9 +uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 +uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +uint8 FRAME_BODY_FRD = 12 +uint8 FRAME_RESERVED_13 = 13 +uint8 FRAME_RESERVED_14 = 14 +uint8 FRAME_RESERVED_15 = 15 +uint8 FRAME_RESERVED_16 = 16 +uint8 FRAME_RESERVED_17 = 17 +uint8 FRAME_RESERVED_18 = 18 +uint8 FRAME_RESERVED_19 = 19 +uint8 FRAME_LOCAL_FRD = 20 +uint8 FRAME_LOCAL_FLU = 21 + +# see enum MAV_CMD and CommandCode.msg +uint16 command + +bool is_current +bool autocontinue +# meaning of this params described in enum MAV_CMD +float32 param1 +float32 param2 +float32 param3 +float32 param4 +float64 x_lat +float64 y_long +float64 z_alt diff --git a/src/external/mavros_msgs/msg/WaypointList.msg b/src/external/mavros_msgs/msg/WaypointList.msg new file mode 100644 index 0000000..de4111e --- /dev/null +++ b/src/external/mavros_msgs/msg/WaypointList.msg @@ -0,0 +1,9 @@ +# WaypointList.msg +# +# :current_seq: seq nr of currently active waypoint +# waypoints[current_seq].is_current == True +# +# :waypoints: list of waypoints + +uint16 current_seq +mavros_msgs/Waypoint[] waypoints diff --git a/src/external/mavros_msgs/msg/WaypointReached.msg b/src/external/mavros_msgs/msg/WaypointReached.msg new file mode 100644 index 0000000..6fc36a9 --- /dev/null +++ b/src/external/mavros_msgs/msg/WaypointReached.msg @@ -0,0 +1,7 @@ +# That message represent MISSION_ITEM_REACHED +# +# :wp_seq: index number of reached waypoint + +std_msgs/Header header + +uint16 wp_seq diff --git a/src/external/mavros_msgs/msg/WheelOdomStamped.msg b/src/external/mavros_msgs/msg/WheelOdomStamped.msg new file mode 100644 index 0000000..782c3ab --- /dev/null +++ b/src/external/mavros_msgs/msg/WheelOdomStamped.msg @@ -0,0 +1,6 @@ +# Stamped wheel odometry message +# +# For streaming timestamped data from FCU wheel encoders (RPM or WHEEL_DISTANCE). + +std_msgs/Header header +float64[] data diff --git a/src/external/mavros_msgs/package.xml b/src/external/mavros_msgs/package.xml new file mode 100644 index 0000000..9c5d8bc --- /dev/null +++ b/src/external/mavros_msgs/package.xml @@ -0,0 +1,46 @@ + + + + mavros_msgs + 2.9.0 + + mavros_msgs defines messages for MAVROS. + + + Vladimir Ermakov + + GPLv3 + LGPLv3 + BSD + + http://wiki.ros.org/mavros_msgs + https://github.com/mavlink/mavros.git + https://github.com/mavlink/mavros/issues + + Vladimir Ermakov + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + + + rcl_interfaces + geographic_msgs + geometry_msgs + sensor_msgs + + + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/external/mavros_msgs/srv/CommandAck.srv b/src/external/mavros_msgs/srv/CommandAck.srv new file mode 100644 index 0000000..f1d708e --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandAck.srv @@ -0,0 +1,11 @@ +# Generic COMMAND_ACK + +uint16 command +uint8 result +uint8 progress +uint32 result_param2 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandBool.srv b/src/external/mavros_msgs/srv/CommandBool.srv new file mode 100644 index 0000000..54eab1f --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandBool.srv @@ -0,0 +1,6 @@ +# Common type for switch commands + +bool value +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandHome.srv b/src/external/mavros_msgs/srv/CommandHome.srv new file mode 100644 index 0000000..89882c7 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandHome.srv @@ -0,0 +1,10 @@ +# request set new home position + +bool current_gps +float32 yaw +float32 latitude +float32 longitude +float32 altitude +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandInt.srv b/src/external/mavros_msgs/srv/CommandInt.srv new file mode 100644 index 0000000..f2242c7 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandInt.srv @@ -0,0 +1,19 @@ +# Generic COMMAND_INT + +bool broadcast # send this command in broadcast mode + +uint8 frame +uint16 command +uint8 current +uint8 autocontinue +float32 param1 +float32 param2 +float32 param3 +float32 param4 +int32 x # latitude in deg * 1E7 or local x * 1E4 m +int32 y # longitude in deg * 1E7 or local y * 1E4 m +float32 z # altitude +--- +bool success +# seems that this message don't produce andy COMMAND_ACK messages +# so no result field diff --git a/src/external/mavros_msgs/srv/CommandLong.srv b/src/external/mavros_msgs/srv/CommandLong.srv new file mode 100644 index 0000000..ca7f2bb --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandLong.srv @@ -0,0 +1,17 @@ +# Generic COMMAND_LONG + +bool broadcast # send this command in broadcast mode + +uint16 command +uint8 confirmation +float32 param1 +float32 param2 +float32 param3 +float32 param4 +float32 param5 # x_lat +float32 param6 # y_lon +float32 param7 # z_alt +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTOL.srv b/src/external/mavros_msgs/srv/CommandTOL.srv new file mode 100644 index 0000000..b76c85e --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTOL.srv @@ -0,0 +1,10 @@ +# Common type for Take Off and Landing + +float32 min_pitch # used by takeoff +float32 yaw +float32 latitude +float32 longitude +float32 altitude +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTOLLocal.srv b/src/external/mavros_msgs/srv/CommandTOLLocal.srv new file mode 100644 index 0000000..33d522f --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTOLLocal.srv @@ -0,0 +1,10 @@ +#Common type for LOCAL Take Off and Landing + +float32 min_pitch # used by takeoff +float32 offset # used by land (landing position accuracy) +float32 rate # speed of takeoff/land in m/s +float32 yaw # in radians +geometry_msgs/Vector3 position #(x,y,z) in meters +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTriggerControl.srv b/src/external/mavros_msgs/srv/CommandTriggerControl.srv new file mode 100644 index 0000000..fa3977b --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTriggerControl.srv @@ -0,0 +1,8 @@ +# Type for controlling onboard camera triggering system + +bool trigger_enable # Trigger enable/disable +bool sequence_reset # Reset the trigger sequence +bool trigger_pause # Pause triggering, but without switching the camera off or retracting it. +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTriggerInterval.srv b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv new file mode 100644 index 0000000..85af0f5 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv @@ -0,0 +1,7 @@ +# Type for controlling camera trigger interval and integration time + +float32 cycle_time # Trigger cycle_time (interval between to triggers) - set to 0 to ignore command +float32 integration_time # Camera shutter integration_time - set to 0 to ignore command +--- +bool success +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/CommandVtolTransition.srv b/src/external/mavros_msgs/srv/CommandVtolTransition.srv new file mode 100644 index 0000000..fce9a72 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandVtolTransition.srv @@ -0,0 +1,16 @@ + +# MAVLink command: DO_VTOL_TRANSITION +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_VTOL_TRANSITION + +std_msgs/Header header + +# MAV_VTOL_STATE +uint8 STATE_MC = 3 +uint8 STATE_FW = 4 + +uint8 state # See enum MAV_VTOL_STATE. + +--- +bool success +uint8 result # Raw result returned by COMMAND_ACK + \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/EndpointAdd.srv b/src/external/mavros_msgs/srv/EndpointAdd.srv new file mode 100644 index 0000000..c102adb --- /dev/null +++ b/src/external/mavros_msgs/srv/EndpointAdd.srv @@ -0,0 +1,14 @@ +# +# Adds endpoint to router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +string url # mavconn URL or topic prefix for TYPE_UAS +uint8 type # should be set to one of the TYPE_xxx +--- +bool successful # true if endpoint added and opened +string reason # returns error description if open fails +uint32 id # ID of new endpoint, should be > 0 if endpoint created diff --git a/src/external/mavros_msgs/srv/EndpointDel.srv b/src/external/mavros_msgs/srv/EndpointDel.srv new file mode 100644 index 0000000..04d1ff3 --- /dev/null +++ b/src/external/mavros_msgs/srv/EndpointDel.srv @@ -0,0 +1,17 @@ +# +# Removes endpoint from router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +# delete by ID, leave 0 for second option +uint32 id + +# delete by url+type pair +string url +uint8 type + +--- +bool successful diff --git a/src/external/mavros_msgs/srv/FileChecksum.srv b/src/external/mavros_msgs/srv/FileChecksum.srv new file mode 100644 index 0000000..75cb4be --- /dev/null +++ b/src/external/mavros_msgs/srv/FileChecksum.srv @@ -0,0 +1,12 @@ +# FTP::Checksum +# +# :file_path: file to calculate checksum +# :crc32: file checksum +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +uint32 crc32 +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileClose.srv b/src/external/mavros_msgs/srv/FileClose.srv new file mode 100644 index 0000000..b99419e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileClose.srv @@ -0,0 +1,10 @@ +# FTP::Close +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileList.srv b/src/external/mavros_msgs/srv/FileList.srv new file mode 100644 index 0000000..d589a9e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileList.srv @@ -0,0 +1,10 @@ +# FTP::List +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +mavros_msgs/FileEntry[] list +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileMakeDir.srv b/src/external/mavros_msgs/srv/FileMakeDir.srv new file mode 100644 index 0000000..c2067d9 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileMakeDir.srv @@ -0,0 +1,9 @@ +# FTP::MakeDir +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileOpen.srv b/src/external/mavros_msgs/srv/FileOpen.srv new file mode 100644 index 0000000..9ad26c3 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileOpen.srv @@ -0,0 +1,17 @@ +# FTP::Open +# +# :file_path: used as session id in read/write/close services +# :size: file size returned for MODE_READ +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +uint8 MODE_READ = 0 # open for read +uint8 MODE_WRITE = 1 # open for write +uint8 MODE_CREATE = 2 # do creat() + +string file_path +uint8 mode +--- +uint32 size +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRead.srv b/src/external/mavros_msgs/srv/FileRead.srv new file mode 100644 index 0000000..d8aa592 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRead.srv @@ -0,0 +1,13 @@ +# FTP::Read +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 offset +uint64 size +--- +uint8[] data +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRemove.srv b/src/external/mavros_msgs/srv/FileRemove.srv new file mode 100644 index 0000000..af0328f --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRemove.srv @@ -0,0 +1,9 @@ +# FTP::Remove +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRemoveDir.srv b/src/external/mavros_msgs/srv/FileRemoveDir.srv new file mode 100644 index 0000000..c95d52f --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRemoveDir.srv @@ -0,0 +1,9 @@ +# FTP::RemoveDir +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRename.srv b/src/external/mavros_msgs/srv/FileRename.srv new file mode 100644 index 0000000..0e367a5 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRename.srv @@ -0,0 +1,10 @@ +# FTP::Rename +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string old_path +string new_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileTruncate.srv b/src/external/mavros_msgs/srv/FileTruncate.srv new file mode 100644 index 0000000..4c78596 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileTruncate.srv @@ -0,0 +1,10 @@ +# FTP::Truncate +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 length +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileWrite.srv b/src/external/mavros_msgs/srv/FileWrite.srv new file mode 100644 index 0000000..d94815e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileWrite.srv @@ -0,0 +1,12 @@ +# FTP::Write +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 offset +uint8[] data +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/GimbalGetInformation.srv b/src/external/mavros_msgs/srv/GimbalGetInformation.srv new file mode 100644 index 0000000..c91bda2 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalGetInformation.srv @@ -0,0 +1,10 @@ +# MAVLink command: MAV_CMD_REQUEST_MESSAGE +# https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE +# Specifically used to request Information messages from Gimbal Device and Gimbal Manager +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv new file mode 100644 index 0000000..ef46dc9 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv @@ -0,0 +1,28 @@ +# MAVLink commands: CAMERA_TRACK_POINT, CAMERA_TRACK_RECTANGLE, CAMERA_STOP_TRACKING +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING + +uint8 mode # enumerator to indicate camera track mode setting - see CAMERA_TRACK_MODE +#CAMERA_TRACK_MODE +uint8 CAMERA_TRACK_MODE_POINT = 0 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. [CAMERA_TRACK_POINT] +uint8 CAMERA_TRACK_MODE_RECTANGLE = 1 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. [CAMERA_TRACK_RECTANGLE] +uint8 CAMERA_TRACK_MODE_STOP_TRACKING = 2 # Stops ongoing tracking. [CAMERA_STOP_TRACKING] + +#For CAMERA_TRACK_POINT +float32 x # Point to track x value (normalized 0..1, 0 is left, 1 is right). +float32 y # Point to track y value (normalized 0..1, 0 is top, 1 is bottom). +float32 radius # Point radius (normalized 0..1, 0 is image left, 1 is image right). + +#For CAMERA_TRACK_RECTANGLE +float32 top_left_x # Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 top_left_y # Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). +float32 bottom_right_x # Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 bottom_right_y # Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). + +#CAMERA_STOP_TRACKING doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv new file mode 100644 index 0000000..c0a2176 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv @@ -0,0 +1,32 @@ +# MAVLink command: DO_GIMBAL_MANAGER_CONFIGURE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE +# Note: default MAV_COMP_ID_ONBOARD_COMPUTER = 191, see MAV_COMPONENT documentation +# https://mavlink.io/en/messages/common.html#MAV_COMPONENT + +int16 sysid_primary # Sysid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_primary # Compid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 sysid_secondary # Sysid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_secondary # Compid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). + # Note: Default Mavlink gimbal device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv new file mode 100644 index 0000000..6e3c14e --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv @@ -0,0 +1,27 @@ +# MAVLink commands: DO_GIMBAL_MANAGER_PITCHYAW +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW + + +float32 pitch # Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode). (-180 to 180 deg) +float32 yaw # Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode). (-180 to 180 deg) +float32 pitch_rate # Pitch rate (positive to pitch up). (deg/s) +float32 yaw_rate # Yaw rate (positive to yaw to the right). (deg/s) + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv new file mode 100644 index 0000000..02904d8 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv @@ -0,0 +1,38 @@ +# MAVLink commands: DO_SET_ROI_LOCATION, DO_SET_ROI_WPNEXT_OFFSET, DO_SET_ROI_SYSID, DO_SET_ROI_NONE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE + +uint8 mode # enumerator to indicate ROI mode setting - see ROI_MODE +#ROI_MODE +uint8 ROI_MODE_LOCATION = 0 # Sets the region of interest (ROI) to a location. [DO_SET_ROI_LOCATION] +uint8 ROI_MODE_WP_NEXT_OFFSET = 1 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. [DO_SET_ROI_WPNEXT_OFFSET] +uint8 ROI_MODE_SYSID = 2 # Mount tracks system with specified system ID [DO_SET_ROI_SYSID] +uint8 ROI_MODE_NONE = 3 # Cancels any previous ROI setting and returns vehicle to defaults [DO_SET_ROI_NONE] + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +#For ROI_MODE_LOCATION +float32 latitude +float32 longitude +float32 altitude # Meters + +#For ROI_MODE_WP_NEXT_OFFSET +float32 pitch_offset # Pitch offset from next waypoint, positive pitching up +float32 roll_offset # Roll offset from next waypoint, positive rolling to the right +float32 yaw_offset # Yaw offset from next waypoint, positive yawing to the right + +#For ROI_MODE_SYSID +uint8 sysid # System ID to track (min: 1, max: 255) + +#ROI_MODE_NONE doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/LogRequestData.srv b/src/external/mavros_msgs/srv/LogRequestData.srv new file mode 100644 index 0000000..b306df6 --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestData.srv @@ -0,0 +1,11 @@ +# Request a chunk of a log +# +# :id: - log id from LogEntry message +# :offset: - offset into the log +# :count: - number of bytes to get + +uint16 id +uint32 offset +uint32 count +--- +bool success diff --git a/src/external/mavros_msgs/srv/LogRequestEnd.srv b/src/external/mavros_msgs/srv/LogRequestEnd.srv new file mode 100644 index 0000000..956e89f --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestEnd.srv @@ -0,0 +1,4 @@ +# Stop log transfer and resume normal logging + +--- +bool success diff --git a/src/external/mavros_msgs/srv/LogRequestList.srv b/src/external/mavros_msgs/srv/LogRequestList.srv new file mode 100644 index 0000000..14eb46c --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestList.srv @@ -0,0 +1,9 @@ +# Request a list of available logs +# +# :start: - first log id (0 for first available) +# :end: - last log id (0xffff for last available) + +uint16 start +uint16 end +--- +bool success diff --git a/src/external/mavros_msgs/srv/MessageInterval.srv b/src/external/mavros_msgs/srv/MessageInterval.srv new file mode 100644 index 0000000..5db5436 --- /dev/null +++ b/src/external/mavros_msgs/srv/MessageInterval.srv @@ -0,0 +1,7 @@ +# sets message interval +# See MAV_CMD_SET_MESSAGE_INTERVAL + +uint32 message_id +float32 message_rate +--- +bool success diff --git a/src/external/mavros_msgs/srv/MountConfigure.srv b/src/external/mavros_msgs/srv/MountConfigure.srv new file mode 100644 index 0000000..2b9afaf --- /dev/null +++ b/src/external/mavros_msgs/srv/MountConfigure.srv @@ -0,0 +1,28 @@ +# MAVLink message: DO_MOUNT_CONTROL +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONFIGURE + +std_msgs/Header header + +uint8 mode # See enum MAV_MOUNT_MODE. +#MAV_MOUNT_MODE +uint8 MODE_RETRACT = 0 +uint8 MODE_NEUTRAL = 1 +uint8 MODE_MAVLINK_TARGETING = 2 +uint8 MODE_RC_TARGETING = 3 +uint8 MODE_GPS_POINT = 4 + +bool stabilize_roll # stabilize roll? (1 = yes, 0 = no) +bool stabilize_pitch # stabilize pitch? (1 = yes, 0 = no) +bool stabilize_yaw # stabilize yaw? (1 = yes, 0 = no) +uint8 roll_input # roll input (See enum MOUNT_INPUT) +uint8 pitch_input # pitch input (See enum MOUNT_INPUT) +uint8 yaw_input # yaw input (See enum MOUNT_INPUT) + +#MOUNT_INPUT +uint8 INPUT_ANGLE_BODY_FRAME = 0 +uint8 INPUT_ANGULAR_RATE = 1 +uint8 INPUT_ANGLE_ABSOLUTE_FRAME = 2 +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/ParamGet.srv b/src/external/mavros_msgs/srv/ParamGet.srv new file mode 100644 index 0000000..cf092f0 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamGet.srv @@ -0,0 +1,8 @@ +# Request parameter from attached device + +# XXX DEPRECATED: use ros2 parameters api instead + +string param_id +--- +bool success +ParamValue value diff --git a/src/external/mavros_msgs/srv/ParamPull.srv b/src/external/mavros_msgs/srv/ParamPull.srv new file mode 100644 index 0000000..db11796 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamPull.srv @@ -0,0 +1,8 @@ +# Request parameters from device +# +# Returns success status and param_recived count + +bool force_pull +--- +bool success +uint32 param_received diff --git a/src/external/mavros_msgs/srv/ParamPush.srv b/src/external/mavros_msgs/srv/ParamPush.srv new file mode 100644 index 0000000..fe0d4f8 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamPush.srv @@ -0,0 +1,9 @@ +# Send current send +# +# Returns success status and param_transfered count + +# XXX DEPRECATED: not used. param plugin listen to parameter changes + +--- +bool success +uint32 param_transfered diff --git a/src/external/mavros_msgs/srv/ParamSet.srv b/src/external/mavros_msgs/srv/ParamSet.srv new file mode 100644 index 0000000..f5b64d9 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamSet.srv @@ -0,0 +1,9 @@ +# Request set parameter value + +# XXX DEPRECATED: replaced by ParamSetV2 + +string param_id +mavros_msgs/ParamValue value +--- +bool success +mavros_msgs/ParamValue value diff --git a/src/external/mavros_msgs/srv/ParamSetV2.srv b/src/external/mavros_msgs/srv/ParamSetV2.srv new file mode 100644 index 0000000..e06b85a --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamSetV2.srv @@ -0,0 +1,14 @@ +# Request set parameter value +# +# That interface allow to bypass some checks +# and send value as is to the FCU if force_set is true. +# +# Use that api only if ROS2 Parameter API is not sufficient +# for your application. + +bool force_set +string param_id +rcl_interfaces/ParameterValue value +--- +bool success +rcl_interfaces/ParameterValue value diff --git a/src/external/mavros_msgs/srv/SetMavFrame.srv b/src/external/mavros_msgs/srv/SetMavFrame.srv new file mode 100644 index 0000000..b12f76a --- /dev/null +++ b/src/external/mavros_msgs/srv/SetMavFrame.srv @@ -0,0 +1,36 @@ +# Set MAV_FRAME for setpoints + +# XXX DEPRECATED + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('MAV_FRAME', 'FRAME_') +# ]]] +# MAV_FRAME +uint8 FRAME_GLOBAL = 0 # Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). +uint8 FRAME_LOCAL_NED = 1 # NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. +uint8 FRAME_MISSION = 2 # NOT a coordinate frame, indicates a mission command. +uint8 FRAME_GLOBAL_RELATIVE_ALT = 3 # Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 FRAME_LOCAL_ENU = 4 # ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. +uint8 FRAME_GLOBAL_INT = 5 # Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude over mean sea level (MSL). +uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 FRAME_LOCAL_OFFSET_NED = 7 # NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle. +uint8 FRAME_BODY_NED = 8 # Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/accelaration values. +uint8 FRAME_BODY_OFFSET_NED = 9 # This is the same as MAV_FRAME_BODY_FRD. +uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 # Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 # Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 FRAME_BODY_FRD = 12 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. The forward axis is aligned to the front of the vehicle in the horizontal plane. +uint8 FRAME_RESERVED_13 = 13 # MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). +uint8 FRAME_RESERVED_14 = 14 # MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_15 = 15 # MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). +uint8 FRAME_RESERVED_16 = 16 # MAV_FRAME_VISION_NED - Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_17 = 17 # MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). +uint8 FRAME_RESERVED_18 = 18 # MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_19 = 19 # MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). +uint8 FRAME_LOCAL_FRD = 20 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +uint8 FRAME_LOCAL_FLU = 21 # FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +# [[[end]]] (checksum: c5ddb537c91e87c4efba8b24c9cde50e) + +uint8 mav_frame +--- +bool success diff --git a/src/external/mavros_msgs/srv/SetMode.srv b/src/external/mavros_msgs/srv/SetMode.srv new file mode 100644 index 0000000..2637c0e --- /dev/null +++ b/src/external/mavros_msgs/srv/SetMode.srv @@ -0,0 +1,22 @@ +# set FCU mode +# +# Known custom modes listed here: +# http://wiki.ros.org/mavros/CustomModes + +# basic modes from MAV_MODE +uint8 MAV_MODE_PREFLIGHT = 0 +uint8 MAV_MODE_STABILIZE_DISARMED = 80 +uint8 MAV_MODE_STABILIZE_ARMED = 208 +uint8 MAV_MODE_MANUAL_DISARMED = 64 +uint8 MAV_MODE_MANUAL_ARMED = 192 +uint8 MAV_MODE_GUIDED_DISARMED = 88 +uint8 MAV_MODE_GUIDED_ARMED = 216 +uint8 MAV_MODE_AUTO_DISARMED = 92 +uint8 MAV_MODE_AUTO_ARMED = 220 +uint8 MAV_MODE_TEST_DISARMED = 66 +uint8 MAV_MODE_TEST_ARMED = 194 + +uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != '' +string custom_mode # string mode representation or integer +--- +bool mode_sent # Mode known/parsed correctly and SET_MODE are sent diff --git a/src/external/mavros_msgs/srv/StreamRate.srv b/src/external/mavros_msgs/srv/StreamRate.srv new file mode 100644 index 0000000..a66ca54 --- /dev/null +++ b/src/external/mavros_msgs/srv/StreamRate.srv @@ -0,0 +1,17 @@ +# sets stream rate +# See REQUEST_DATA_STREAM message + +uint8 STREAM_ALL = 0 +uint8 STREAM_RAW_SENSORS = 1 +uint8 STREAM_EXTENDED_STATUS = 2 +uint8 STREAM_RC_CHANNELS = 3 +uint8 STREAM_RAW_CONTROLLER = 4 +uint8 STREAM_POSITION = 6 +uint8 STREAM_EXTRA1 = 10 +uint8 STREAM_EXTRA2 = 11 +uint8 STREAM_EXTRA3 = 12 + +uint8 stream_id +uint16 message_rate +bool on_off +--- diff --git a/src/external/mavros_msgs/srv/VehicleInfoGet.srv b/src/external/mavros_msgs/srv/VehicleInfoGet.srv new file mode 100644 index 0000000..9e32236 --- /dev/null +++ b/src/external/mavros_msgs/srv/VehicleInfoGet.srv @@ -0,0 +1,14 @@ +# Request the Vehicle Info +# use this to request the current target sysid / compid defined in mavros +# set get_all = True to request all available vehicles + +uint8 GET_MY_SYSID = 0 +uint8 GET_MY_COMPID = 0 + +uint8 sysid +uint8 compid +bool get_all +--- +bool success +mavros_msgs/VehicleInfo[] vehicles + diff --git a/src/external/mavros_msgs/srv/WaypointClear.srv b/src/external/mavros_msgs/srv/WaypointClear.srv new file mode 100644 index 0000000..ac3d34c --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointClear.srv @@ -0,0 +1,4 @@ +# Request clear waypoint + +--- +bool success diff --git a/src/external/mavros_msgs/srv/WaypointPull.srv b/src/external/mavros_msgs/srv/WaypointPull.srv new file mode 100644 index 0000000..2d7ac2c --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointPull.srv @@ -0,0 +1,7 @@ +# Requests waypoints from device +# +# Returns success status and received count + +--- +bool success +uint32 wp_received diff --git a/src/external/mavros_msgs/srv/WaypointPush.srv b/src/external/mavros_msgs/srv/WaypointPush.srv new file mode 100644 index 0000000..e29727b --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointPush.srv @@ -0,0 +1,11 @@ +# Send waypoints to device +# +# :start_index: will define a partial waypoint update. Set to 0 for full update +# +# Returns success status and transfered count + +uint16 start_index +mavros_msgs/Waypoint[] waypoints +--- +bool success +uint32 wp_transfered diff --git a/src/external/mavros_msgs/srv/WaypointSetCurrent.srv b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv new file mode 100644 index 0000000..b99612f --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv @@ -0,0 +1,7 @@ +# Request set current waypoint +# +# wp_seq - index in waypoint array + +uint16 wp_seq +--- +bool success From d2e90a93c91c371f7b074c848f83128b0851ac66 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 1 Dec 2025 21:15:57 +0800 Subject: [PATCH 079/146] Fix geographic_info branch to ros2 --- .gitmodules | 7 ++++--- README.md | 13 ++++++++----- src/external/geographic_info | 2 +- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/.gitmodules b/.gitmodules index 6630485..1742bc8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ -[submodule "src/external/geographic_info"] - path = src/external/geographic_info - url = https://github.com/ros-geographic-info/geographic_info.git [submodule "src/external/angles"] path = src/external/angles url = https://github.com/ros/angles.git +[submodule "src/external/geographic_info"] + path = src/external/geographic_info + url = https://github.com/ros-geographic-info/geographic_info.git + branch = ros2 diff --git a/README.md b/README.md index 4f9dff7..1cbbf37 100644 --- a/README.md +++ b/README.md @@ -7,12 +7,14 @@ === -必要相依套件 +必要相依套件 順便記錄我開發時的環境版本 Python -1. pymavlink +1. pymavlink -> Version: 2.4.42 2. conda-forge 中的 pyserial-asyncio -3. +3. importlib_metadata -> Version: 8.5.0 +4. setuptools -> Version: 58.2.0 (版本太新不行) + ROS2 1. source ~/ros2_humble/install/setup.bash @@ -25,7 +27,7 @@ ROS2 === 依賴的 ROS 庫 -1. https://github.com/ros-geographic-info/geographic_info.git +1. https://github.com/ros-geographic-info/geographic_info.git 記得要搞 ros2 版本的 2. https://github.com/ros/angles.git 3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的 @@ -36,7 +38,8 @@ cd ~/AirTrapMine git submodule init git submodule update # 2. build 需要的 package -colcon build --packages-select angles geographic_msgs mavros_msgs +colcon build --packages-select angles geographic_msgs +colcon build --packages-select mavros_msgs # 這個依賴前面的 ``` diff --git a/src/external/geographic_info b/src/external/geographic_info index bc73c05..24806ad 160000 --- a/src/external/geographic_info +++ b/src/external/geographic_info @@ -1 +1 @@ -Subproject commit bc73c05ee79c31a88b4a23b545a2fe55eae8089e +Subproject commit 24806adc767414eb3a34a58aefeb648ee415b09a From 0ce78b964a8744b2952c86ffe0d7a87bbefc2358 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 1 Dec 2025 21:24:56 +0800 Subject: [PATCH 080/146] before merge change nothing --- .../fc_network_adapter/mavlinkDevice.py | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py deleted file mode 100644 index 2d7ea06..0000000 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ /dev/null @@ -1,98 +0,0 @@ - -import os - -# 自定義的 import -from .utils import setup_logger - -# ====================== 分割線 ===================== - -logger = setup_logger(os.path.basename(__file__)) - -# 用來記錄每個 system 的資訊 -# 資料格式 { sysid : mavlink_device object } -mavlink_systems = {} - -# ====================== 分割線 ===================== - -# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component -class mavlink_device(): - def __init__(self): - self.socket_id = None # 記錄來自於哪個 socket - self.sysid = None - self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object - - def __str__(self): - p_str = '=====================\n' - p_str += f'object id : {id(self)}\n' # debug - p_str += f'socket_id : {self.socket_id}\n' - p_str += f'sysid : {self.sysid}\n' - p_str += 'has components : \n' - for compid in self.components: - p_str += f'compid : {compid}\n' - p_str += f'mav_type : {self.components[compid].mav_type}\n' - # p_str += '=====================\n' - return p_str - - ''' - 寫了半天 這個功能應該是 pymalink 本來就有的 - 去找 pymavlink_util.py 的 mavfile(object) - 算了 先擺著吧 之後再看看怎麼整合 - ''' - def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): - # 這段檢查遺失封包 - try: - 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)) # 因為初始化的之前 會有大量非 heartbeat 的訊息進來 這是正常現象 TODO 之後要幫這個類別加上初始化狀態 再進行這個判斷 - return - - if last_seq != None: - expected_seq = (last_seq + 1) % 256 - diff = current_seq - expected_seq - # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug - if diff < 0: - diff += 256 - self.components[compid].lost_packet_count += diff - - # 這段更新封包的基本資訊 - self.components[compid].msg_seq = current_seq - self.components[compid].last_msg_time = current_time - if current_type in self.components[compid].msg_count: - self.components[compid].msg_count[current_type] += 1 - else: - self.components[compid].msg_count[current_type] = 1 - - def resetComponentPacketCount(self, compid): - self.components[compid].msg_count = {} - self.components[compid].msg_seq = None - self.components[compid].lost_packet_count = 0 - - class mavlink_component(): - # 程式用不到的參數 但是做個記錄 - # paraEmitList = ['base_mode', 'flightMode_mode', - # 'battery_voltage', # from BATTERY_STATUS (147) - # 'gps_fix_type', # from GPS_RAW_INT (24) - # 'roll', 'pitch', 'yaw', # from ATTITUDE (30) - # 'position_latitude', 'position_longitude', 'position_altitude', # from GLOBAL_POSITION_INT (33) - # 'heading', # for VFR_HUD (74) - # ] - - def __init__(self): - self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) - self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) - self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) - - # 紀錄從這個 component 收到最後的訊息序號和時間 - self.msg_count = {} - self.msg_seq = None - self.lost_packet_count = 0 - self.last_msg_time = 0 - - # 存放該 emit component 參數的區域 - # 內容格式為 {param_name(字串) : param_value} - # param_name 請見上面 paraEmitList - self.emitParams = {} - # 用來存放每個 topic 的 publisher - # 內容格式 為 {topic_name(字串) : [publisher(物件), method(函式)]} (? - self.publishers = {} \ No newline at end of file From bcccdec9278ab3a2ff3a976cba246db9521addb3 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 11 Dec 2025 13:22:38 +0800 Subject: [PATCH 081/146] =?UTF-8?q?modify:=20mainOrchestrator.py=20?= =?UTF-8?q?=E6=94=B9=E5=96=84=E9=A1=AF=E7=A4=BA=E4=BB=8B=E9=9D=A2=20add:?= =?UTF-8?q?=20serialManager.py=20=E6=B7=BB=E5=8A=A0Serial=20=E9=80=A3?= =?UTF-8?q?=E7=B5=90=E5=8A=9F=E8=83=BD=20(=E6=9C=AA=E5=AE=8C=E6=88=90)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + .../fc_network_adapter/mainOrchestrator.py | 61 +-- .../fc_network_adapter/mavlinkObject.py | 96 ++-- .../fc_network_adapter/serialManager.py | 499 ++++++++++++++++++ .../fc_network_adapter/socketManager.py | 14 - .../fc_network_adapter/utils/acquirePort.py | 129 +++++ .../tests/demo_integration.py | 21 +- 7 files changed, 738 insertions(+), 83 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/serialManager.py delete mode 100644 src/fc_network_adapter/fc_network_adapter/socketManager.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py diff --git a/README.md b/README.md index 1cbbf37..ce0703a 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ Python 2. conda-forge 中的 pyserial-asyncio 3. importlib_metadata -> Version: 8.5.0 4. setuptools -> Version: 58.2.0 (版本太新不行) +5. pyserial-asyncio ROS2 diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index f15bad6..257f571 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -38,7 +38,8 @@ class PanelState: # 這邊是儲存關於 socket object 的資料 self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 self.socket_info_single = {"socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", - "target_sockets": "", "primary_socket_id": "", "InfoReady": False} # 暫存單一 socket 的資訊 + "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", + "InfoReady": False} # 暫存單一 socket 的資訊 def intoSTART(self): self.panel_status = "Running" @@ -139,9 +140,9 @@ class ControlPanel: obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), - MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), + MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), - MenuNode("Remove Target", "移除轉發目標(工程)", "MAVOBJ_REMOVE_TARGET"), + MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), MenuNode("返回", "回到列表", "BACK"), ]) # 將 socket_id 附加到每個子選單項目上 @@ -183,9 +184,10 @@ class ControlPanel: # 這裡顯示基本資訊 dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") - dialog_win.addstr(3, 2, f"Socket status : 運行中") + # dialog_win.addstr(3, 2, f"Socket status : 運行中") # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") + dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) @@ -397,7 +399,7 @@ class ControlPanel: # 操作說明 # help_line = start_line + len(current_menu.children) + 2 help_line = height - 2 - stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層 q退出", curses.A_DIM) + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM) stdscr.refresh() @@ -431,8 +433,12 @@ class ControlPanel: idx_stack[-1] = (current_idx + 1) % len(current_menu.children) elif ch == (ord('O')): - # 直接進入工程模式 + # 進入工程模式 state.intoENGINEER() + + elif ch == (ord('o')): + # 離開工程模式 + state.intoSTART() elif ch == curses.KEY_LEFT: # 返回上層 @@ -448,8 +454,9 @@ class ControlPanel: idx_stack.append(0) elif ch in (ord('q'), 27): - state.intoTERMINATION() - panel_shutdown() + if state.panel_status == "Engineer": + state.intoTERMINATION() + panel_shutdown() elif ch in (curses.KEY_ENTER, 10, 13): selected = current_menu.children[current_idx] @@ -537,22 +544,23 @@ class ControlPanel: # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 menu_stack.pop() idx_stack.pop() - # # 刷新列表頁面 - # if len(menu_stack) > 1: - # current_page = menu_stack[-1].current_page if hasattr(menu_stack[-1], 'current_page') else 0 - # menu_stack.pop() - # idx_stack.pop() - # time.sleep(0.1) # 等待物件被移除 - # object_list_menu = self.create_object_list_menu(state, page=current_page) - # menu_stack.append(object_list_menu) - # idx_stack.append(0) elif selected.action == "MAVOBJ_MAKE_LINK": # 建立轉發連結 if hasattr(selected, 'socket_id'): target_id = self.select_target_socket(stdscr, selected.socket_id, state) if target_id is not None: - cmd_q.put(("MAVOBJ_MAKE_LINK", selected.socket_id, target_id)) + # cmd_q.put(("MAVOBJ_MAKE_LINK", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 + + elif selected.action == "MAVOBJ_CANCEL_LINK": + # 取消轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) + if target_id is not None: + cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結 elif selected.action == "MAVOBJ_ADD_TARGET": # 添加目標端口 @@ -563,16 +571,6 @@ class ControlPanel: target_id = self.select_target_socket(stdscr, selected.socket_id, state) if target_id is not None: cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) - - elif selected.action == "MAVOBJ_REMOVE_TARGET": - # 移除目標端口 - if state.panel_status != "Engineer": - state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) - continue # 只有在工程模式下才能操作 - if hasattr(selected, 'socket_id'): - target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) - if target_id is not None: - cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) elif selected.action == "STOP_MANAGER": if state.panel_status != "Engineer": @@ -661,10 +659,6 @@ class Orchestrator: self.remove_target_from_object(s_id, socket_id) # 再移除該物件 self.delete_mavlink_object(socket_id) - elif action == "MAVOBJ_MAKE_LINK": - source_id, target_id = cmd[1], cmd[2] - self.add_target_to_object(source_id, target_id) - self.add_target_to_object(target_id, source_id) # 雙向連結 elif action == "MAVOBJ_ADD_TARGET": source_id, target_id = cmd[1], cmd[2] self.add_target_to_object(source_id, target_id) @@ -681,6 +675,9 @@ class Orchestrator: self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets + ip_info = mav_obj.mavlink_socket.port.getsockname() + self.panelState.socket_info_single["socket_connection_string"] = f"{ip_info[0]}:{ip_info[1]}" + # getattr(mav_obj.mavlink_socket, "connection_string", "") self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 elif cmd == "CREATE_UDP_INBOUND": diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index b4d747f..635f113 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,40 +1,35 @@ ''' -# 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 ring buffer 傳送到橋接器 -# 會有兩種 RingBuffer 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 -# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 +這個檔案是對 udp 進來的 mavlink 訊息做 "分流" "轉拋" 的地方 (並不會做 "分析") +概念上 把每個 udp 接口 視為一個獨立的 mavlink bus 針對 bus 來統籌管理 -# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 -# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 -# 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function +主要的重點部分: +1. stream_bridge_ring & return_packet_ring +2. mavlink_object & async_io_manager +3. mavlink_bridge -================= 改版記錄 ============================ -2025年 6月 20日 -1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 - 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 - 而 multiplexingToSwap 會完全被移除代替方式下一條描述 -2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 - (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) - 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 - 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 -3. mavlink_object 需要加上 state 去管理其狀態 -4. mavlink_object 需要加上 target port 去管理寫出的目標 -5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 -6. 基於第1,2項 updateMultiplexingList 會被完全移除 -7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object -8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 +stream_bridge_ring & return_packet_ring: + 這兩個 ring buffer 是用來做 mavlink 訊息的分流 + stream_bridge_ring 這邊的資訊比較是給會固定更新的資訊 (例如 HEARTBEAT, ATTITUDE 之類的) + return_packet_ring 比較是處理指令後的回應封包 (例如 PARAM_VALUE, MISSION_ITEM 等等) + +mavlink_object: + 每個 mavlink bus 都會有一個 mavlink_object + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 內容中沒有獨立的執行緒 只有一個個 asyncio function 會被放到 async_io_manager 裡面執行 + + 關於分流與轉拋的具體實現是在 process_data 這個 asyncio function 裡面 + +async_io_manager: + 首先它紀律並管理所有 mavlink_object 實例 + 有自己一個獨立的執行緒 執行 asyncio loop (mavlink_object 裡面的 asyncio function 都會被放到這個 loop 裡面執行) + +mavlink_bridge: + 專門處理 stream_bridge_ring 裡面的訊息流 + 會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖 + -2025年 11月 15日 -1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行 -2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式) -3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 -4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 -5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 -6. mavlink_object 優化 send_message 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 -7. 移除迴圈內的 try except 堆疊 增加效能 -8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 ''' @@ -61,7 +56,6 @@ from .mavlinkVehicleView import ( ) from .utils import RingBuffer, setup_logger - # ====================== 分割線 ===================== logger = setup_logger(os.path.basename(__file__)) @@ -331,8 +325,6 @@ class mavlink_bridge: mav_obj = mavlink_object.mavlinkObjects[socket_id] return mav_obj.send_message(message_bytes) -# ====================== 分割線 ===================== - # 定義 mavlink_object 的狀態 class MavlinkObjectState(Enum): INIT = auto() # 初始化狀態 @@ -560,6 +552,7 @@ class async_io_manager: start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop 然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務 """ + _instance = None _lock = threading.Lock() @@ -714,7 +707,7 @@ class async_io_manager: async def _async_add_mavlink_object(self, mavlink_obj): """在事件循環線程中同步執行""" socket_id = mavlink_obj.socket_id - + try: task = asyncio.create_task(mavlink_obj.process_data()) self.managed_objects[socket_id] = mavlink_obj @@ -789,3 +782,36 @@ class async_io_manager: if __name__ == '__main__': pass + + +''' +================= 改版記錄 ============================ + +2025年 6月 20日 +1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 + 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 + 而 multiplexingToSwap 會完全被移除代替方式下一條描述 +2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 + (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) + 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 + 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 +3. mavlink_object 需要加上 state 去管理其狀態 +4. mavlink_object 需要加上 target port 去管理寫出的目標 +5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 +6. 基於第1,2項 updateMultiplexingList 會被完全移除 +7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object +8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 + + +2025年 11月 15日 +1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行 +2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式) +3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 +4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 +5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 +6. mavlink_object 優化 send_message 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +7. 移除迴圈內的 try except 堆疊 增加效能 +8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 + +''' + diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py new file mode 100644 index 0000000..7de45bb --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -0,0 +1,499 @@ +''' + + +''' + +# 基礎功能的 import +import asyncio +import serial_asyncio + +import os +import sys +import serial +import signal +from enum import Enum, auto + +# # XBee 模組 +# from xbee.frame import APIFrame + +# 自定義的 import +from .utils import setup_logger + +# ====================== 分割線 ===================== + +logger = setup_logger(os.path.basename(__file__)) + +# ====================== 分割線 ===================== +class XBeeFrameHandler: + """XBee API Frame 處理器""" + + @staticmethod + def parse_at_command_response(frame: bytes) -> dict: + """解析 AT Command Response (0x88)""" + if len(frame) < 8: + return None + + frame_type = frame[3] + if frame_type != 0x88: + return None + + frame_id = frame[4] + at_command = frame[5:7] + status = frame[7] + data = frame[8:] if len(frame) > 8 else b'' + + return { + 'frame_id': frame_id, + 'command': at_command, + 'status': status, + 'data': data, + 'is_ok': status == 0x00 + } + + @staticmethod + def parse_receive_packet(frame: bytes) -> dict: + # """解析 RX Packet (0x90) - 未來擴展用""" + # if len(frame) < 15 or frame[3] != 0x90: + # return None + + # return { + # 'source_addr': frame[4:12], + # 'reserved': frame[12:14], + # 'options': frame[14], + # 'data': frame[15:-1] + # } + pass + + @staticmethod + def encapsulate_data(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes: + """ + 將數據封裝為 XBee API 傳輸幀 + + 使用 XBee API 格式封裝數據: + - 傳輸請求幀 (0x10) + - 使用廣播地址 + - 添加適當的頭部和校驗和 + """ + frame_type = 0x10 + 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) + + @staticmethod + def decapsulate_data(data: bytes): + # 這裡可以根據需要進行數據解封裝 + + # XBee API 幀格式: + # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) + + # 檢查幀起始符 (0x7E) + if not data or len(data) < 5 or data[0] != 0x7E: + return data + + # 獲取數據長度 (不包括校驗和) + # length = (data[1] << 8) + data[2] + length = (data[1] << 8) | data[2] + + # 檢查幀完整性 + if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 + return data + + # 提取API標識符和數據 + frame_type = data[3] + # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 + + # 根據不同的幀類型進行處理 + if frame_type == 0x90: # 例如，這是"接收數據包"類型 + rf_data_start = 3 + 12 + return data[rf_data_start:3 + length] + else: + return None + return data + + + +class ATCommandHandler: + """AT 指令回應處理器""" + + def __init__(self, serial_port: str): + self.serial_port = serial_port + self.handlers = { + b'DB': self._handle_rssi, + b'SH': self._handle_serial_high, + b'SL': self._handle_serial_low, + # 可擴展其他 AT 指令 + } + + def handle_response(self, response: dict): + """根據 AT 指令類型分派處理""" + if not response or not response['is_ok']: + if response: + print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}") + return + + command = response['command'] + handler = self.handlers.get(command) + + if handler: + handler(response['data']) + else: + print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}") + + def _handle_rssi(self, data: bytes): + """處理 DB (RSSI) 回應""" + if not data: + return + + rssi_value = data[0] + now = time.time() + + # 檢查是否最近有收到 MAVLink + last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0) + if now - last_mavlink_time > 0.5: + print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + return + + # 取得對應的 sysid + sysid = serial_to_sysid.get(self.serial_port) + if sysid is None: + print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + return + + # 記錄 RSSI + rssi_history[sysid].append(-rssi_value) + time_history[sysid].append(now) + # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + + def _handle_serial_high(self, data: bytes): + """處理 SH (Serial Number High) - 範例""" + if len(data) >= 4: + serial_high = int.from_bytes(data[:4], 'big') + print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + + def _handle_serial_low(self, data: bytes): + """處理 SL (Serial Number Low) - 範例""" + if len(data) >= 4: + serial_low = int.from_bytes(data[:4], 'big') + print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + + + +class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 + def __init__(self, udp_handler, serial_port_str): + self.udp_handler = udp_handler # UDP 的傳輸把手 + self.serial_port_str = serial_port_str + self.at_handler = ATCommandHandler(serial_port) + + self.buffer = bytearray() # 用於緩存接收到的資料 + self.transport = None # Serial 自己的傳輸物件 + # self.first_data = True # 標記是否為第一次收到資料 + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + if hasattr(self.udp_handler, 'set_serial_handler'): + self.udp_handler.set_serial_handler(self) + logger.info(f"Serial port {self.serial_port_str} connected.") + + # Serial 收到資料的處理過程 + def data_received(self, data): + # 1. 把收到的資料加入緩衝區 + self.buffer.extend(data) + + # 2. 需要完整的 header 才能解析 + while len(self.buffer) >= 3: + # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) + if self.buffer[0] != 0x7E: + self.buffer.pop(0) # 如果不是就丟掉 + continue + + # 4. 讀取 payload 長度 + length = (self.buffer[1] << 8) | self.buffer[2] + full_length = 3 + length + 1 + + # 5. 等待完整封包 + if len(self.buffer) < full_length: + break + + # 6. 提取完整 frame 並從緩衝區移除 + frame_payload = self.buffer[:full_length] + del self.buffer[:full_length] + + # 7. 判斷 frame 類型 + frame_type = frame[3] + + if frame_type == 0x88: + # 處理 AT Command 回應 + # response = XBeeFrameHandler.parse_at_command_response(frame) + # self.at_handler.handle_response(response) + pass # debug + + elif frame_type == 0x90: + # Receive Packet (RX) payload 先解碼 + processed_data = XBeeFrameHandler.decapsulate_data(bytes(frame_payload)) + # 轉換失敗就捨棄了 + if processed_data is None: + break + # 再透過 UDP 送出 + self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)) + + else: + # 其他類型的 frame 未來可擴展處理 現在忽略 + logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}") + + # # RSSI + # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包 + # # frame[5:7] == b'DB' -> API 封包的DB參數 + # status = frame[7] # + # if status == 0x00 and len(frame) > 8: # status == 0x00 -> 這個封包是有效封包 + # rssi_value = frame[8] + # now = time.time() + + # # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port + # last_time = serial_last_mavlink_time.get(self.serial_port, 0) + # if now - last_time <= 0.5: + # sysid = serial_to_sysid.get(self.serial_port, None) + # if sysid is not None: + # rssi_history[sysid].append(-rssi_value) + # time_history[sysid].append(now) + # # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + # else: + # print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + # else: + # print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + # else: + # print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}") + + + # def write_to_serial(self, data): + # # 在資料透過 Serial 發送之前進行處理 + # processed_data = self.encapsulate_data(data) + + # # 處理失敗就不發送 + # if processed_data not None: + # self.transport.write(processed_data) + + + +class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 + + LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP + + def __init__(self, target_port): + self.target_port = target_port # 目標 UDP 端口 + + self.serial_handler = None # Serial 的傳輸物件 + self.transport = None # UDP 自己的傳輸物件 + self.remote_addr = None # 儲存動態獲取的遠程地址 # debug + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + print("UDP transport ready. Waiting for serial data before sending...") + + def set_serial_handler(self, serial_handler): + self.serial_handler = serial_handler + + # UDP 收到資料的處理過程 + def datagram_received(self, data, addr): + # 儲存對方的地址（這樣就能向同一個來源回傳數據） + # self.remote_addr = addr + # print(f"Received UDP data from {addr}, setting as remote address") + + processed_data = XBeeFrameHandler.encapsulate_data(data) + + if self.serial_handler: + self.serial_handler.transport.write(processed_data) + + + # def send_udp(self, data): + # # 藉由 UDP 發送資料出去 + + # # 在透過 UDP 發送數據之前進行解封裝 + # decoded_data = self.decapsulate_data(data) + # if decoded_data is None: + # return + + # if self.transport: + # self.transport.sendto(decoded_data, (self.LOCAL_HOST_IP, self.target_port)) + +#================================================================== + +class SerialReceiverType(Enum): + """連接類型""" + TELEMETRY = auto() + XBEEAPI = auto() + OTHER = auto() + + +class serial_manager: + + class serial_object: + def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc + self.baudrate = baudrate + self.receiver_type = receiver_type + self.target_port = target_port # 指向的 UPD 端口 + + self.transport = None + self.protocol = None + self.udp_handler = None + self.serial_handler = None + + def __init__(self): + self.thread = None + self.loop = None + self.running = False + self.serial_count = 0 + self.serial_objects = {} # serial id num : serial object + + def __del__(self): + self.loop = None + self.thread = None + + def start(self): + + if self.running: + logger.warning("serial_manager already running") + return + + self.running = True + + # 啟動獨立線程 命名為 SerialManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="SerialManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 + self.thread.start() + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("serial_manager thread started <-") + return True + else: + logger.error("serial_manager failed to start") + return False + + def shutdown(self): + pass + + def _run_event_loop(self): + """在獨立線程中運行 asyncio 事件循環""" + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + # # 為每個 serial_object 建立連接 + # for serial_obj in self.serial_objects: + # coro = serial_asyncio.create_serial_connection( + # self.loop, + # lambda: SerialProtocol(serial_obj.receiver_type), + # serial_obj.serial_port, + # baudrate=serial_obj.baudrate + # ) + # transport, protocol = self.loop.run_until_complete(coro) + # serial_obj.transport = transport + # serial_obj.protocol = protocol + + try: + self.loop.run_forever() + finally: + self.loop.close() + + def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + + if self.loop is None: + logger.error("Event loop not running, cannot create serial link") + return False + + # 檢查 serial port 有效 + self.check_serial_port(serial_port) + + serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) + + # 建立 UDP 處理器 並指定目標端口位置 + serial_obj.udp_handler = UDPHandler(target_port) + # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 + try: + serial_obj.transport, serial_obj.protocol = await self.loop.create_datagram_endpoint( + lambda: serial_obj.udp_handler, + local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 + ) + except Exception as e: + logger.error(f"Cannot Create UDP Endpoint: {str(e)}") + return False + + # 建立 Serial 傳輸，將 UDP 處理器傳給它 + try: + serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler) + + _, serial_transport = await serial_asyncio.create_serial_connection( + self.loop, lambda: serial_obj.serial_handler, serial_port, baudrate=baudrate + ) + except Exception as e: + logger.error(f"Cannot Create Serial Connection: {str(e)}") + serial_obj.transport.close() + return + + # self.serial_objects.append(serial_obj) + self.serial_objects[serial_count+1] = serial_obj + serial_count += 1 + + async def _async_create_serial_link(self, serial_port, baudrate, target_port): + pass + + def remove_serial_link(serial_id): + pass + + async def _async_remove_serial_link(self, serial_id): + pass + + def check_serial_port(serial_port): + """檢查串口是否存在與可用""" + # 檢查設備是否存在 + if not os.path.exists(serial_port): + logger.error(f"Serial Device {serial_port} Not Found") + return False + + # 檢查是否有權限訪問設備 + try: + os.access(serial_port, os.R_OK | os.W_OK) + except Exception as e: + logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}") + return False + + # 檢查是否被占用 + try: + # 嘗試打開串口 + ser = serial.Serial(serial_port, SERIAL_BAUDRATE) + ser.close() # 打開成功後立即關閉 + return True + except serial.SerialException as e: + logger.error(f"Serial Device {serial_port} is Occupied or Inaccessible: {str(e)}") + return False + except Exception as e: + logger.error(f"Unknown Error: {str(e)}") + return False + + +if __main__ == '__main__': + sm = serial_manager() + sm.start() + + SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 + SERIAL_BAUDRATE = 115200 + UDP_REMOTE_IP = '127.0.0.1' + UDP_REMOTE_PORT = 14560 + sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_IP, SerialReceiverType.XBEEAPI) \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/socketManager.py b/src/fc_network_adapter/fc_network_adapter/socketManager.py deleted file mode 100644 index 52e8681..0000000 --- a/src/fc_network_adapter/fc_network_adapter/socketManager.py +++ /dev/null @@ -1,14 +0,0 @@ - -''' - -透過某個地方 得到 udp 或 uart 接口 -對於每個接口 視為一個獨立的物件 - -物件對於不同的接口 是為不同類型的物件 - -每個類型的物件 創建一個獨立的執行緒 來處理資料 -關於執行緒的實作 是寫在另一個模組 - -物件之間 也可以做資料轉換/轉拋 - -''' \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py new file mode 100644 index 0000000..0c6a873 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py @@ -0,0 +1,129 @@ +import socket +import random +import os + + +def get_used_ports(): + """ + 從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port + 直接讀取 Linux 系統資訊，避免暴力嘗試 + + Returns: + set: 已被占用的 port 號集合 + """ + used_ports = set() + + # 讀取 TCP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/tcp', '/proc/net/tcp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + # local_address 格式: "0100007F:1F90" (hex) + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + # 讀取 UDP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/udp', '/proc/net/udp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + return used_ports + + +def is_port_available(port): + """ + 測試指定 port 是否可用 (TCP 和 UDP 都測試) + + Args: + port (int): 要測試的 port 號 + + Returns: + bool: True 表示可用，False 表示被占用 + """ + # 測試 TCP + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + # 測試 UDP + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + return True + + +def find_available_port(start_port=1024, end_port=65535): + """ + 在指定的 port 區間內隨機找出一個未被占用的 port + 使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試 + 確保 TCP 和 UDP 都可用 + + Args: + start_port (int): 起始 port 號 (預設 1024) + end_port (int): 結束 port 號 (預設 65535) + + Returns: + int: 可用的 port 號，如果找不到則返回 None + """ + if start_port < 1 or end_port > 65535 or start_port >= end_port: + raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port") + + # 從系統讀取已占用的 port + used_ports = get_used_ports() + + # 計算可用的 port 列表 (排除已占用的) + available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports] + + if not available_ports: + return None + + # 隨機打亂順序 + random.shuffle(available_ports) + + # 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用 + for port in available_ports: + if is_port_available(port): + return port + + # 如果都不可用 + return None + + +if __name__ == "__main__": + # 使用範例 + port = find_available_port(8000, 9000) + if port: + print(f"找到可用的 port: {port}") + else: + print("找不到可用的 port") + + # 自訂範圍範例 + port = find_available_port(10000, 20000) + if port: + print(f"在 10000-20000 範圍找到可用的 port: {port}") diff --git a/src/fc_network_adapter/tests/demo_integration.py b/src/fc_network_adapter/tests/demo_integration.py index 7546f65..11ac1d7 100644 --- a/src/fc_network_adapter/tests/demo_integration.py +++ b/src/fc_network_adapter/tests/demo_integration.py @@ -19,7 +19,7 @@ from ..fc_network_adapter import mavlinkVehicleView as mvv # ====================== 分割線 ===================== -test_item = 10 +test_item = 1 running_time = 3 @@ -31,7 +31,24 @@ print('test_item : ', test_item) 測試項 1X 表示 mavlink_object 的功能 測試連線的能力 ''' -if test_item == 10: +if test_item == 1: + print('===> Start of Program .Test ', test_item) + + connection_string="udp:127.0.0.1:14591" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + # mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + time.sleep(1) + + print("mark A") + + # print("Socket IP:", mavlink_socket1.target_system) + print("Socket port:", mavlink_socket1.port.getsockname()) + + # print("=== ", dir(mavlink_socket1.port)) + + +elif test_item == 10: # 需要開啟一個 ardupilot 的模擬器 # 測試 mavlink_object 放入 ring buffer 的應用 print('===> Start of Program .Test ', test_item) From a8aefe785366f904445013726f3f86fe5c30b969 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 17 Dec 2025 16:08:54 +0800 Subject: [PATCH 082/146] =?UTF-8?q?1.=20serialManager=20=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E5=AE=8C=E6=88=90=202.=20mainOrchestrator=20=E6=8A=8Aserial?= =?UTF-8?q?=E7=AB=AF=E5=8F=A3=E6=95=B4=E5=90=88=E5=88=B0=E4=BB=8B=E9=9D=A2?= =?UTF-8?q?=E4=B8=AD=E4=BA=86=203.=20=E6=96=B0=E5=A2=9E=E7=8D=B2=E5=8F=96?= =?UTF-8?q?=E7=B3=BB=E7=B5=B1=20serial=20=E8=B3=87=E6=BA=90=E7=9A=84=20uti?= =?UTF-8?q?l=20=E5=B7=A5=E5=85=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 301 +++++++++++++++--- .../fc_network_adapter/mavlinkObject.py | 6 +- .../fc_network_adapter/serialManager.py | 232 ++++++++++---- .../fc_network_adapter/utils/acquireSerial.py | 96 ++++++ 4 files changed, 528 insertions(+), 107 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 257f571..0010a1b 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -20,7 +20,11 @@ from pymavlink import mavutil # 自定義的 import from . import mavlinkObject as mo +from . import serialManager as sm + from .utils import RingBuffer, setup_logger +from .utils import acquireSerial, acquirePort +from .utils.acquirePort import find_available_port @@ -32,11 +36,13 @@ class PanelState: termination_start_time = None self.mavlink_bridge_state = "Stopped" self.object_manager_state = "Stopped" + self.serial_manager_state = "Stopped" self.socket_object_list = [] self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 # 這邊是儲存關於 socket object 的資料 self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 + self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": True} # 暫存 Serial 設定資訊 self.socket_info_single = {"socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", "InfoReady": False} # 暫存單一 socket 的資訊 @@ -122,7 +128,7 @@ class ControlPanel: return user_input - def create_object_list_menu(self, state: PanelState, page=0, items_per_page=5): + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): """動態創建 mavlink_object 列表選單（支持分頁）""" children = [] @@ -166,7 +172,56 @@ class ControlPanel: menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) menu.current_page = page return menu - + + def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 serial port 列表選單（支持分頁）""" + children = [] + + # 獲取可用的 Serial 連接埠列表 + # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 + serial_ports = acquireSerial.get_serial_ports_with_filter('/dev/ttyUSB*') + + if not serial_ports: + children.append(MenuNode("(空)", "目前沒有串口設備", None)) + else: + total_items = len(serial_ports) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的串口 + for port in serial_ports[start_idx:end_idx]: + port_menu = MenuNode(f"{port}", children=[ + MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ + MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), + MenuNode("Xbee(AT-AT)", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + ]), + MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), + MenuNode("返回", "回到列表", "BACK"), + ]) + # 將 port 附加到每個子選單項目上 + for child in port_menu.children: + child.port = port + children.append(port_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("返回", "回到上層選單", "BACK")) + menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + def show_object_info(self, stdscr, socket_id, state: PanelState): """顯示物件詳細資訊的對話框""" height, width = stdscr.getmaxyx() @@ -280,12 +335,14 @@ class ControlPanel: MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), ]), + MenuNode("Serial InBound", action = "LIST_SERIAL_RES"), ]), MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), ]), MenuNode("Engineer Mode", "工程模式", children=[ - MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), #TODO: 尚未實作 - MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), #TODO: 尚未實作 + MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), + MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), + MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), ]), MenuNode("Shutdown", "關閉整個系統", children=[ MenuNode("Return", "繼續運行", "BACK"), @@ -304,11 +361,11 @@ class ControlPanel: curses.echo() curses.endwin() - def panel_shutdown(): + def pre_panel_shutdown(): # 先關閉所有模組 再關閉面板 cmd_q.put("SHUTDOWN_BRIDGE") cmd_q.put("SHUTDOWN_MANAGER") - + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") def draw_menu(screen): nonlocal stdscr @@ -325,9 +382,6 @@ class ControlPanel: state.intoSTART() # 設定狀態為運行中 while not stop_evt.is_set(): - # 檢查是否需要退出 - if stop_evt.is_set(): - break current_menu = menu_stack[-1] current_idx = idx_stack[-1] @@ -335,20 +389,27 @@ class ControlPanel: # 獲取終端機尺寸 height, width = stdscr.getmaxyx() # 簡單暴力的限制視窗的大小 - if height < 20 or width < 60: + MIN_HEIGHT = ( + 2 + # 邊界 + 6 + # 狀態列 操作說明列 一個空白 + 11+ # 最大選單 與 空白區 + 5 # 訊息區域 + ) + if height < MIN_HEIGHT or width < 60: logger.error("Terminal size too small for Control Panel.") break stdscr.clear() - stdscr.border() + + # 更新模組狀態顯示 stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") - stdscr.addstr(2, 2, f"mavlink Bridge State : {state.mavlink_bridge_state}") - stdscr.addstr(3, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") + stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") - # # 更新模組狀態顯示 # stdscr.addstr(2, 25, f"{state.mavlink_bridge_state}") # stdscr.addstr(3, 25, f"{state.object_manager_state}") # stdscr.addstr(4, 25, f"{len(state.socket_object_list)} ") @@ -363,6 +424,10 @@ class ControlPanel: desc = f"{child.desc} [{state.udp_info_temp['IP']}]" elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: desc = f"{child.desc} [{state.udp_info_temp['Port']}]" + elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]: + desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" + elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: + desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" line = f"{marker}{child.name:15s} – {desc}" attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL @@ -404,25 +469,29 @@ class ControlPanel: stdscr.refresh() # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 - # 驗證 bridge 跟 manager 狀態 兩者都停止後 就進入 STOPPED 狀態並跳出迴圈 + # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈 # 超過幾秒沒有反應就強制關閉 if state.panel_status == "Terminating": - if time.time() - state.termination_start_time > 3: + if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點 logger.warning("Control Panel forced shutdown after timeout.") state.intoSTOPPED() - stop_evt.set() - continue + # stop_evt.set() + # continue + break time.sleep(0.1) - if state.mavlink_bridge_state == "Stopped" and state.object_manager_state == "Stopped": + if (state.mavlink_bridge_state == "Stopped" and + state.object_manager_state == "Stopped" and + state.serial_manager_state == "Stopped"): state.intoSTOPPED() - stop_evt.set() + # stop_evt.set() + break continue # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt - stdscr.timeout(100) # 100ms timeout + stdscr.timeout(100) ch = stdscr.getch() - if ch == -1: # timeout，繼續檢查 stop_evt + if ch == -1: # 沒有操作 continue # 處理按鍵 @@ -456,7 +525,7 @@ class ControlPanel: elif ch in (ord('q'), 27): if state.panel_status == "Engineer": state.intoTERMINATION() - panel_shutdown() + pre_panel_shutdown() elif ch in (curses.KEY_ENTER, 10, 13): selected = current_menu.children[current_idx] @@ -473,7 +542,7 @@ class ControlPanel: elif selected.action == "QUIT": state.intoTERMINATION() - panel_shutdown() + pre_panel_shutdown() elif selected.action == "TEXT_UDP_IP": result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ") @@ -484,47 +553,105 @@ class ControlPanel: result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ") if result is not None: state.udp_info_temp["Port"] = result - + elif selected.action == "CREATE_UDP_INBOUND": cmd_q.put("CREATE_UDP_INBOUND") # 確認後回到上兩層 if len(menu_stack) > 1: menu_stack.pop() idx_stack.pop() - menu_stack.pop() - idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() elif selected.action == "CREATE_UDP_OUTBOUND": cmd_q.put("CREATE_UDP_OUTBOUND") # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() + + elif selected.action == "TEXT_BAUD_SERIAL": + result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ") + if result is not None: + try: + baud_rate = int(result) + except ValueError: + state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time())) + state.serial_info_temp["Baud"] = baud_rate + + elif selected.action == "SET_SERIAL_COMM_XBEE": + state.serial_info_temp["CommunicationType"] = "XBee(API-AT)" + menu_stack.pop() + idx_stack.pop() + elif selected.action == "SET_SERIAL_COMM_TELEMETRY": + state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)" + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "CREATE_SERIAL_PORT": + state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱 + cmd_q.put("CREATE_SERIAL_PORT") + # 確認後回到上兩層 if len(menu_stack) > 1: menu_stack.pop() idx_stack.pop() menu_stack.pop() idx_stack.pop() + + elif selected.action == "LIST_SERIAL_RES": + created_list_menu = self.create_serial_port_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) elif selected.action == "LIST_MAV_OBJECT": # 動態生成 mavlink_object 列表選單 - object_list_menu = self.create_object_list_menu(state, page=0) - menu_stack.append(object_list_menu) + created_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(created_list_menu) idx_stack.append(0) - elif selected.action == "PREV_PAGE": - # 上一頁 - if hasattr(selected, 'page'): - menu_stack.pop() - idx_stack.pop() - object_list_menu = self.create_object_list_menu(state, page=selected.page) - menu_stack.append(object_list_menu) - idx_stack.append(0) + # elif selected.action == "PREV_PAGE": + # # 上一頁 + # if hasattr(selected, 'page'): + # menu_stack.pop() + # idx_stack.pop() + # if menu_stack[-1].name == "Serial Port List": + # created_list_menu = self.create_serial_port_menu(state, page=selected.page) + # elif menu_stack[-1].name == "Object List": + # created_list_menu = self.create_object_list_menu(state, page=selected.page) + # menu_stack.append(created_list_menu) + # idx_stack.append(0) - elif selected.action == "NEXT_PAGE": - # 下一頁 + # elif selected.action == "NEXT_PAGE": + # # 下一頁 + # if hasattr(selected, 'page'): + # menu_stack.pop() + # idx_stack.pop() + # if menu_stack[-1].name == "Serial Port List": + # created_list_menu = self.create_serial_port_menu(state, page=selected.page) + # elif menu_stack[-1].name == "Object List": + # created_list_menu = self.create_object_list_menu(state, page=selected.page) + # menu_stack.append(created_list_menu) + # idx_stack.append(0) + elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): if hasattr(selected, 'page'): + current_list_menu = menu_stack[-1] menu_stack.pop() idx_stack.pop() - object_list_menu = self.create_object_list_menu(state, page=selected.page) - menu_stack.append(object_list_menu) + + # 依據選單種類 重新建立分頁 + if current_list_menu.name == "Serial Port List": + created_list_menu = self.create_serial_port_menu(state, page=selected.page) + elif current_list_menu.name == "Object List": + created_list_menu = self.create_object_list_menu(state, page=selected.page) + else: + # 不支援的選單類型，回到原本的選單 + menu_stack.append(current_list_menu) + idx_stack.append(0) + continue + + menu_stack.append(created_list_menu) idx_stack.append(0) elif selected.action == "INSPECT_MAV_OBJECT": @@ -583,6 +710,13 @@ class ControlPanel: state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) continue # 只有在工程模式下才能操作 cmd_q.put("SHUTDOWN_BRIDGE") + + elif selected.action == "STOP_SERIAL_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + elif callable(selected.action): # 執行函式 cmd_q.put(selected.action) @@ -592,14 +726,13 @@ class ControlPanel: except KeyboardInterrupt: pass finally: - stop_evt.set() cleanup() class Orchestrator: def __init__(self, stop_sig): - self.stop_evt = stop_sig + self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個) # === 1) 面板部分的準備 === self.cmd_q = queue.Queue() @@ -613,6 +746,9 @@ class Orchestrator: self.manager = mo.async_io_manager() self.bridge = mo.mavlink_bridge() + # === 3) serial_manager 部分的準備 === + self.plumber = sm.serial_manager() + def engageWholeSystem(self): """啟動整個系統""" # === 1) 面板部分的啟動 === @@ -623,10 +759,14 @@ class Orchestrator: self.manager.start() self.bridge.start() + # === 3) serial_manager 部分的啟動 === + self.plumber.start() + def mainLoop(self): logger.info("Main orchestrator started <-") try: - while not self.stop_evt.is_set(): + # while not self.stop_evt.is_set(): + while self.panel_thread.is_alive(): # A. 更新模組狀態 if self.manager.running: @@ -642,6 +782,11 @@ class Orchestrator: else: self.panelState.mavlink_bridge_state = 'Stopped' + if self.plumber.running: + self.panelState.serial_manager_state = 'Running' + else: + self.panelState.serial_manager_state = 'Stopped' + # 取出面板丟過來的「動作」 try: cmd = self.cmd_q.get_nowait() @@ -686,10 +831,14 @@ class Orchestrator: elif cmd == "CREATE_UDP_OUTBOUND": self.panelState.udp_info_temp["direction"] = "outbound" self.create_udp_object() + elif cmd == "CREATE_SERIAL_PORT": + self.create_serial_port_object() elif cmd == "SHUTDOWN_BRIDGE": self.bridge.stop() elif cmd == "SHUTDOWN_MANAGER": self.manager.shutdown() + elif cmd == "SHUTDOWN_SERIAL_MANAGER": + self.plumber.shutdown() except queue.Empty: pass except Exception as e: @@ -703,17 +852,31 @@ class Orchestrator: except Exception as e: logger.error(f"Unexpected error in main loop: {e}") finally: - logger.info("Main orchestrator END!") - # 關閉 mavlink_bridge (裡面有一個執行緒) - self.bridge.stop() + # 驗證並確保所有模組都被下達關閉訊號 + # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 + if self.bridge.thread.is_alive(): + if self.bridge.running: + self.bridge.stop() + self.bridge.thread.join(timeout=2) + + if self.manager.thread.is_alive(): + if self.manager.running: + self.manager.shutdown() + self.manager.thread.join(timeout=2) - # 關閉 async_io_manager (裡面有一個執行緒) - self.manager.shutdown() + if self.plumber.thread.is_alive(): + if self.plumber.running: + self.plumber.shutdown() + self.plumber.thread.join(timeout=2) # 關閉面板執行緒 if self.panel_thread.is_alive(): self.panel_thread.join(timeout=2) + + logger.info("Main orchestrator END!") + + # =============== 面板動作 - Mavlink Object =============== def create_udp_object(self): if self.panelState.udp_info_temp["direction"] == "inbound": @@ -761,6 +924,46 @@ class Orchestrator: else: self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + # =============== 面板動作 - Serial Manager =============== + + def create_serial_port_object(self): + # 獲取可用的 udp port + udp_port_tmp = find_available_port(19000, 20000) + + # 定義通訊類型映射表 + COMM_TYPE_MAP = { + "XBee(API-AT)": sm.CommunicationType.XBee_API_AT, + # "XBee(AT-AT)": sm.CommunicationType.XBee_AT_AT, # TODO: 之後再弄 + # 新增區 + } + + # 驗證輸入 + comm_type = self.panelState.serial_info_temp['CommunicationType'] + if not comm_type: + self.panelState.panel_info_msg_list.append( + ("Please select Communication Type first.", time.time()) + ) + return + + # 查找對應的通訊類型 + comm_type_tmp = COMM_TYPE_MAP.get(comm_type) + if comm_type_tmp is None: + self.panelState.panel_info_msg_list.append( + (f"Communication type '{comm_type}' not supported yet.", time.time()) + ) + return + + ret = self.plumber.create_serial_port( + port=self.panelState.serial_info_temp['Port'], + baudrate=self.panelState.serial_info_temp['Baud'], + target_port=udp_port_tmp, + communication_type=comm_type_tmp, + ) + + if not ret: + self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + return + def main(): stop_evt = threading.Event() diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 635f113..71760a0 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -128,7 +128,7 @@ class mavlink_bridge: """停止 mavlink_bridge 的運作""" self.running = False if self.thread and self.thread.is_alive(): - self.thread.join(timeout=3.0) + self.thread.join(timeout=5.0) # === Thread 區塊 === def _run_thread(self): @@ -632,12 +632,10 @@ class async_io_manager: # 等待線程結束 if self.thread and self.thread.is_alive(): - self.thread.join(timeout=10.0) + self.thread.join(timeout=5.0) if self.thread.is_alive(): logger.warning("async_io_manager thread did not stop gracefully") os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序 - - logger.info("async_io_manager thread END!") diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 7de45bb..1cf79e0 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -11,6 +11,9 @@ import os import sys import serial import signal +import time +import threading +import struct from enum import Enum, auto # # XBee 模組 @@ -63,9 +66,10 @@ class XBeeFrameHandler: # 'data': frame[15:-1] # } pass + return None @staticmethod - def encapsulate_data(data: bytes, dest_addr64: bytes, frame_id=0x01) -> bytes: + def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes: """ 將數據封裝為 XBee API 傳輸幀 @@ -117,7 +121,6 @@ class XBeeFrameHandler: return data - class ATCommandHandler: """AT 指令回應處理器""" @@ -171,16 +174,18 @@ class ATCommandHandler: # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") def _handle_serial_high(self, data: bytes): - """處理 SH (Serial Number High) - 範例""" - if len(data) >= 4: - serial_high = int.from_bytes(data[:4], 'big') - print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + # """處理 SH (Serial Number High) - 範例""" + # if len(data) >= 4: + # serial_high = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + pass def _handle_serial_low(self, data: bytes): - """處理 SL (Serial Number Low) - 範例""" - if len(data) >= 4: - serial_low = int.from_bytes(data[:4], 'big') - print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + # """處理 SL (Serial Number Low) - 範例""" + # if len(data) >= 4: + # serial_low = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + pass @@ -188,7 +193,7 @@ class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial def __init__(self, udp_handler, serial_port_str): self.udp_handler = udp_handler # UDP 的傳輸把手 self.serial_port_str = serial_port_str - self.at_handler = ATCommandHandler(serial_port) + self.at_handler = ATCommandHandler(serial_port_str) self.buffer = bytearray() # 用於緩存接收到的資料 self.transport = None # Serial 自己的傳輸物件 @@ -199,7 +204,7 @@ class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial self.transport = transport if hasattr(self.udp_handler, 'set_serial_handler'): self.udp_handler.set_serial_handler(self) - logger.info(f"Serial port {self.serial_port_str} connected.") + # logger.info(f"Serial port {self.serial_port_str} connected.") # debug # Serial 收到資料的處理過程 def data_received(self, data): @@ -222,27 +227,30 @@ class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial break # 6. 提取完整 frame 並從緩衝區移除 - frame_payload = self.buffer[:full_length] + an_frame = self.buffer[:full_length] del self.buffer[:full_length] # 7. 判斷 frame 類型 - frame_type = frame[3] + frame_type = an_frame[3] if frame_type == 0x88: # 處理 AT Command 回應 - # response = XBeeFrameHandler.parse_at_command_response(frame) + # response = XBeeFrameHandler.parse_at_command_response(an_frame) # self.at_handler.handle_response(response) pass # debug elif frame_type == 0x90: # Receive Packet (RX) payload 先解碼 - processed_data = XBeeFrameHandler.decapsulate_data(bytes(frame_payload)) + processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame)) # 轉換失敗就捨棄了 if processed_data is None: - break + continue # 再透過 UDP 送出 self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)) + elif frame_type == 0x8B: + pass + else: # 其他類型的 frame 未來可擴展處理 現在忽略 logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}") @@ -295,7 +303,7 @@ class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處 def connection_made(self, transport): self.transport = transport - print("UDP transport ready. Waiting for serial data before sending...") + # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug def set_serial_handler(self, serial_handler): self.serial_handler = serial_handler @@ -328,7 +336,7 @@ class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處 class SerialReceiverType(Enum): """連接類型""" TELEMETRY = auto() - XBEEAPI = auto() + XBEEAPI2AT = auto() OTHER = auto() @@ -388,7 +396,30 @@ class serial_manager: return False def shutdown(self): - pass + """停止 serial_manager 和其管理的所有 serial_object""" + # 自己在 running 狀態下才執行停止程序 + if not self.running: + logger.warning("serial_manager is not running") + return + + # 停止所有被管理的 serial_object + for serial_id in list(self.serial_objects.keys()): + self.remove_serial_link(serial_id) + + # 停止自己 + self.running = False + + # 解開事件循環的阻塞 + if self.loop: + self.loop.call_soon_threadsafe(self.loop.stop) + + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) + if self.thread.is_alive(): + logger.warning("serial_manager thread did not stop gracefully") + + logger.info("serial_manager thread END!") def _run_event_loop(self): """在獨立線程中運行 asyncio 事件循環""" @@ -414,53 +445,142 @@ class serial_manager: def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): - if self.loop is None: + if not self.running or not self.loop: logger.error("Event loop not running, cannot create serial link") return False # 檢查 serial port 有效 - self.check_serial_port(serial_port) + if not self.check_serial_port(serial_port, baudrate): + logger.error(f"Serial port {serial_port} validation failed") + return False - serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type), + self.loop + ) - # 建立 UDP 處理器 並指定目標端口位置 - serial_obj.udp_handler = UDPHandler(target_port) - # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 try: - serial_obj.transport, serial_obj.protocol = await self.loop.create_datagram_endpoint( - lambda: serial_obj.udp_handler, - local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 - ) + # 等待結果，設定合理的超時時間 + result = future.result(timeout=5.0) + if result: + logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}") + return True + except asyncio.TimeoutError: + logger.error(f"Timeout creating serial link for {serial_port}") + return False except Exception as e: - logger.error(f"Cannot Create UDP Endpoint: {str(e)}") + logger.error(f"Failed to create serial link for {serial_port}: {e}") return False - # 建立 Serial 傳輸，將 UDP 處理器傳給它 + + async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + """在事件循環線程中執行實際的連接創建""" try: - serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler) + # 創建 serial_object 實例 + serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) - _, serial_transport = await serial_asyncio.create_serial_connection( - self.loop, lambda: serial_obj.serial_handler, serial_port, baudrate=baudrate + # 建立 UDP 處理器並指定目標端口位置 + serial_obj.udp_handler = UDPHandler(target_port) + + # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 + udp_transport, udp_protocol = await self.loop.create_datagram_endpoint( + lambda: serial_obj.udp_handler, + local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 ) - except Exception as e: - logger.error(f"Cannot Create Serial Connection: {str(e)}") - serial_obj.transport.close() - return + serial_obj.transport = udp_transport + serial_obj.protocol = udp_protocol + + # logger.info(f"UDP endpoint created for {serial_port}") # debug - # self.serial_objects.append(serial_obj) - self.serial_objects[serial_count+1] = serial_obj - serial_count += 1 + # 建立 Serial 處理器，將 UDP 處理器傳給它 + serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port) - async def _async_create_serial_link(self, serial_port, baudrate, target_port): - pass + # 建立 Serial 連接 + serial_transport, _ = await serial_asyncio.create_serial_connection( + self.loop, + lambda: serial_obj.serial_handler, + serial_port, + baudrate=baudrate + ) + + # logger.info(f"Serial connection created for {serial_port}") # debug - def remove_serial_link(serial_id): - pass + # 將 serial_object 加入管理列表 + serial_id = self.serial_count + 1 + self.serial_objects[serial_id] = serial_obj + self.serial_count += 1 + + # logger.info(f"Serial object {serial_id} added to manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_create_serial_link for {serial_port}: {str(e)}") + # 清理已創建的資源 + if 'serial_obj' in locals(): + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + return False + + def remove_serial_link(self, serial_id): + """移除串口連接（線程安全方式）""" + # 確保事件循環正在運行 + if not self.loop: + logger.error("Event loop not running") + return False + + # 檢查 serial_id 是否存在 + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not found") + return False + + # 使用 run_coroutine_threadsafe 執行協程 + future = asyncio.run_coroutine_threadsafe( + self._async_remove_serial_link(serial_id), + self.loop + ) + + try: + result = future.result(timeout=3.0) + if result: + logger.info(f"Remove Serial Link {serial_id}") + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing serial link {serial_id}") + return False + except Exception as e: + logger.error(f"Failed to remove serial link {serial_id}: {e}") + return False async def _async_remove_serial_link(self, serial_id): - pass + """在事件循環線程中執行實際的連接移除""" + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not in managed list") + return False + + try: + serial_obj = self.serial_objects[serial_id] + + # 關閉 UDP transport + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + + # 關閉 Serial transport + if hasattr(serial_obj, 'serial_handler') and serial_obj.serial_handler: + if hasattr(serial_obj.serial_handler, 'transport') and serial_obj.serial_handler.transport: + serial_obj.serial_handler.transport.close() + + # 從管理列表中移除 + del self.serial_objects[serial_id] + # logger.info(f"Serial object {serial_id} removed from manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}") + return False - def check_serial_port(serial_port): + @staticmethod + def check_serial_port(serial_port, baudrate): """檢查串口是否存在與可用""" # 檢查設備是否存在 if not os.path.exists(serial_port): @@ -469,7 +589,9 @@ class serial_manager: # 檢查是否有權限訪問設備 try: - os.access(serial_port, os.R_OK | os.W_OK) + if not os.access(serial_port, os.R_OK | os.W_OK): + logger.error(f"No permission to access {serial_port}") + return False except Exception as e: logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}") return False @@ -477,7 +599,7 @@ class serial_manager: # 檢查是否被占用 try: # 嘗試打開串口 - ser = serial.Serial(serial_port, SERIAL_BAUDRATE) + ser = serial.Serial(serial_port, baudrate) ser.close() # 打開成功後立即關閉 return True except serial.SerialException as e: @@ -488,12 +610,14 @@ class serial_manager: return False -if __main__ == '__main__': +if __name__ == '__main__': sm = serial_manager() sm.start() SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 SERIAL_BAUDRATE = 115200 - UDP_REMOTE_IP = '127.0.0.1' - UDP_REMOTE_PORT = 14560 - sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_IP, SerialReceiverType.XBEEAPI) \ No newline at end of file + UDP_REMOTE_PORT = 14571 + sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT) + + time.sleep(10) + sm.shutdown() \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py new file mode 100644 index 0000000..703ae36 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py @@ -0,0 +1,96 @@ +""" +Serial Port Discovery Utility + +This module provides functions to discover available serial ports on the system. +It uses glob pattern matching to find serial device files in /dev/. +""" + +import glob +from typing import List + + +def get_serial_ports() -> List[str]: + """ + 獲取系統中所有可用的串口設備列表 + + 在 Linux 系統中，會搜尋以下模式的設備： + - /dev/ttyUSB* (USB 串口設備) + - /dev/ttyACM* (USB CDC ACM 設備) + - /dev/ttyS* (標準串口) + + Returns: + List[str]: 包含所有找到的串口設備路徑的列表 + + Example: + >>> ports = get_serial_ports() + >>> print(ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0'] + """ + serial_ports = [] + + # 搜尋不同類型的串口設備 + patterns = [ + '/dev/ttyUSB*', # USB 串口轉換器 + '/dev/ttyACM*', # USB CDC ACM 設備（如 Arduino） + '/dev/ttyS*', # 標準串口 + ] + + for pattern in patterns: + serial_ports.extend(glob.glob(pattern)) + + # 排序以便於閱讀 + serial_ports.sort() + + return serial_ports + + +def get_serial_ports_with_filter(filter_pattern: str = None) -> List[str]: + """ + 獲取串口設備列表，可選擇性地使用自訂篩選模式 + + Args: + filter_pattern (str, optional): 自訂的 glob 模式，例如 '/dev/ttyUSB*' + 如果為 None，則使用預設模式搜尋所有串口 + + Returns: + List[str]: 符合條件的串口設備路徑列表 + + Example: + >>> # 只搜尋 USB 串口 + >>> usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*') + >>> print(usb_ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1'] + """ + if filter_pattern: + serial_ports = glob.glob(filter_pattern) + serial_ports.sort() + return serial_ports + else: + return get_serial_ports() + + +if __name__ == "__main__": + # 使用範例 + print("=== Serial Port Discovery ===\n") + + # 1. 獲取所有串口設備 + all_ports = get_serial_ports() + print(f"找到 {len(all_ports)} 個串口設備:") + for port in all_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 2. 只搜尋 USB 串口 + usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*') + print(f"找到 {len(usb_ports)} 個 USB 串口設備:") + for port in usb_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 3. 只搜尋 ACM 設備 + acm_ports = get_serial_ports_with_filter('/dev/ttyACM*') + print(f"找到 {len(acm_ports)} 個 ACM 設備:") + for port in acm_ports: + print(f" - {port}") From f31cc8742a406cd4289b6dd99886d5fcb6ef57fe Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 18 Dec 2025 12:09:54 +0800 Subject: [PATCH 083/146] =?UTF-8?q?(Tested)=201.=20mainOrchestrator.py=20?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E5=91=BC=E5=8F=AB=E9=8C=AF=E8=AA=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 48 +++++-------------- 1 file changed, 12 insertions(+), 36 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 0010a1b..b6c5b1c 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -194,7 +194,7 @@ class ControlPanel: port_menu = MenuNode(f"{port}", children=[ MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), - MenuNode("Xbee(AT-AT)", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), ]), MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), @@ -323,7 +323,7 @@ class ControlPanel: def menu_tree(self): """建立多層選單結構""" return MenuNode("Main Menu", children=[ - MenuNode("MavLink Object", "控制 MavLink 物件", children=[ + MenuNode("MavLink Object", "MavLink 通道選項", children=[ MenuNode("New+", children=[ MenuNode("UDP InBound", children=[ MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), @@ -335,10 +335,13 @@ class ControlPanel: MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), ]), - MenuNode("Serial InBound", action = "LIST_SERIAL_RES"), ]), MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), ]), + MenuNode("Serial Manager", "Serial 連接埠選項", children=[ + MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), + MenuNode("ListAll", "顯示已連線的 Serial", action = "LIST_SERIAL_LINKS"), + ]), MenuNode("Engineer Mode", "工程模式", children=[ MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), @@ -410,10 +413,6 @@ class ControlPanel: stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") - # stdscr.addstr(2, 25, f"{state.mavlink_bridge_state}") - # stdscr.addstr(3, 25, f"{state.object_manager_state}") - # stdscr.addstr(4, 25, f"{len(state.socket_object_list)} ") - # 顯示當前選單項目 start_line = 6 for i, child in enumerate(current_menu.children): @@ -610,30 +609,7 @@ class ControlPanel: created_list_menu = self.create_object_list_menu(state, page=0) menu_stack.append(created_list_menu) idx_stack.append(0) - - # elif selected.action == "PREV_PAGE": - # # 上一頁 - # if hasattr(selected, 'page'): - # menu_stack.pop() - # idx_stack.pop() - # if menu_stack[-1].name == "Serial Port List": - # created_list_menu = self.create_serial_port_menu(state, page=selected.page) - # elif menu_stack[-1].name == "Object List": - # created_list_menu = self.create_object_list_menu(state, page=selected.page) - # menu_stack.append(created_list_menu) - # idx_stack.append(0) - - # elif selected.action == "NEXT_PAGE": - # # 下一頁 - # if hasattr(selected, 'page'): - # menu_stack.pop() - # idx_stack.pop() - # if menu_stack[-1].name == "Serial Port List": - # created_list_menu = self.create_serial_port_menu(state, page=selected.page) - # elif menu_stack[-1].name == "Object List": - # created_list_menu = self.create_object_list_menu(state, page=selected.page) - # menu_stack.append(created_list_menu) - # idx_stack.append(0) + elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): if hasattr(selected, 'page'): current_list_menu = menu_stack[-1] @@ -932,8 +908,8 @@ class Orchestrator: # 定義通訊類型映射表 COMM_TYPE_MAP = { - "XBee(API-AT)": sm.CommunicationType.XBee_API_AT, - # "XBee(AT-AT)": sm.CommunicationType.XBee_AT_AT, # TODO: 之後再弄 + "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT, + # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY, # TODO: 之後再弄 # 新增區 } @@ -953,11 +929,11 @@ class Orchestrator: ) return - ret = self.plumber.create_serial_port( - port=self.panelState.serial_info_temp['Port'], + ret = self.plumber.create_serial_link( + serial_port=self.panelState.serial_info_temp['Port'], baudrate=self.panelState.serial_info_temp['Baud'], target_port=udp_port_tmp, - communication_type=comm_type_tmp, + receiver_type=comm_type_tmp, ) if not ret: From 5134fa8466f0547757e8e7f5de8097afb036e1d1 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 19 Dec 2025 10:04:26 +0800 Subject: [PATCH 084/146] =?UTF-8?q?commit=20to=20merge=20serial=20port=20?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=E5=B7=B2=E9=A9=97=E8=AD=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 170 ++++++++++++------ .../fc_network_adapter/serialManager.py | 10 +- 2 files changed, 126 insertions(+), 54 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index b6c5b1c..e4c0cf0 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -37,7 +37,8 @@ class PanelState: self.mavlink_bridge_state = "Stopped" self.object_manager_state = "Stopped" self.serial_manager_state = "Stopped" - self.socket_object_list = [] + self.socket_object_list = [] # 已有的 mavlink object + self.linked_serial_dict = {} # 已連線的 serial 端口 self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 # 這邊是儲存關於 socket object 的資料 @@ -128,6 +129,8 @@ class ControlPanel: return user_input +# ================ 關於 mavlink object 的部份 =================== + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): """動態創建 mavlink_object 列表選單（支持分頁）""" children = [] @@ -155,56 +158,7 @@ class ControlPanel: for child in obj_menu.children: child.socket_id = socket_id children.append(obj_menu) - - # 添加分頁控制 - if total_pages > 1: - children.append(MenuNode("---", "---", None)) - if page > 0: - prev_node = MenuNode("◀ 上一頁", f"第 {page}/{total_pages} 頁", "PREV_PAGE") - prev_node.page = page - 1 - children.append(prev_node) - if page < total_pages - 1: - next_node = MenuNode("下一頁 ▶", f"第 {page + 2}/{total_pages} 頁", "NEXT_PAGE") - next_node.page = page + 1 - children.append(next_node) - - children.append(MenuNode("返回", "回到上層選單", "BACK")) - menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) - menu.current_page = page - return menu - def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 serial port 列表選單（支持分頁）""" - children = [] - - # 獲取可用的 Serial 連接埠列表 - # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 - serial_ports = acquireSerial.get_serial_ports_with_filter('/dev/ttyUSB*') - - if not serial_ports: - children.append(MenuNode("(空)", "目前沒有串口設備", None)) - else: - total_items = len(serial_ports) - total_pages = (total_items + items_per_page - 1) // items_per_page - start_idx = page * items_per_page - end_idx = min(start_idx + items_per_page, total_items) - - # 顯示當前頁的串口 - for port in serial_ports[start_idx:end_idx]: - port_menu = MenuNode(f"{port}", children=[ - MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ - MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), - # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), - ]), - MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), - MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), - MenuNode("返回", "回到列表", "BACK"), - ]) - # 將 port 附加到每個子選單項目上 - for child in port_menu.children: - child.port = port - children.append(port_menu) - # 添加分頁控制 if total_pages > 1: children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) @@ -216,9 +170,9 @@ class ControlPanel: next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") next_node.page = page + 1 children.append(next_node) - + children.append(MenuNode("返回", "回到上層選單", "BACK")) - menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) menu.current_page = page return menu @@ -319,7 +273,107 @@ class ControlPanel: stdscr.clear() stdscr.refresh() return None - + +# ================ 關於 serial link 的部份 =================== + + def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 serial port 列表選單（支持分頁）""" + children = [] + + # 獲取可用的 Serial 連接埠列表 + # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 + serial_ports = acquireSerial.get_serial_ports_with_filter('/dev/ttyUSB*') + + if not serial_ports: + children.append(MenuNode("(空)", "目前沒有串口設備", None)) + else: + total_items = len(serial_ports) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的串口 + for port in serial_ports[start_idx:end_idx]: + port_menu = MenuNode(f"{port}", children=[ + MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ + MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), + # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + ]), + MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), + MenuNode("返回", "回到列表", "BACK"), + ]) + # 將 port 附加到每個子選單項目上 + for child in port_menu.children: + child.port = port + children.append(port_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("返回", "回到上層選單", "BACK")) + menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能""" + children = [] + + if not state.linked_serial_dict: + children.append(MenuNode("(空)", "目前沒有連結口", None)) + else: + total_items = len(state.linked_serial_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + linked_serial_id_list = list(state.linked_serial_dict.keys()) + for serial_id in linked_serial_id_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), + MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), + # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), + MenuNode("返回", "回到列表", "BACK"), + ]) + # 將 serial_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.serial_id = serial_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("返回", "回到上層選單", "BACK")) + menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_linked_serial_info(self): + pass + +# ================ 關於 主要選單 的部份 =================== + def menu_tree(self): """建立多層選單結構""" return MenuNode("Main Menu", children=[ @@ -603,6 +657,11 @@ class ControlPanel: created_list_menu = self.create_serial_port_menu(state, page=0) menu_stack.append(created_list_menu) idx_stack.append(0) + + elif selected.action == "LIST_SERIAL_LINKS": + created_list_menu = self.create_linked_serial_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) elif selected.action == "LIST_MAV_OBJECT": # 動態生成 mavlink_object 列表選單 @@ -621,6 +680,8 @@ class ControlPanel: created_list_menu = self.create_serial_port_menu(state, page=selected.page) elif current_list_menu.name == "Object List": created_list_menu = self.create_object_list_menu(state, page=selected.page) + elif current_list_menu.name == "Linked Serial List": + created_list_menu = self.create_linked_serial_menu(state, page=selected.page) else: # 不支援的選單類型，回到原本的選單 menu_stack.append(current_list_menu) @@ -763,6 +824,9 @@ class Orchestrator: else: self.panelState.serial_manager_state = 'Stopped' + linked_serial_dict = self.plumber.get_serial_link() + self.panelState.linked_serial_dict = linked_serial_dict + # 取出面板丟過來的「動作」 try: cmd = self.cmd_q.get_nowait() diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 1cf79e0..e1f7a80 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -473,7 +473,6 @@ class serial_manager: logger.error(f"Failed to create serial link for {serial_port}: {e}") return False - async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): """在事件循環線程中執行實際的連接創建""" try: @@ -579,6 +578,12 @@ class serial_manager: logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}") return False + def get_serial_link(self): + ret = {} # serial id num : serial_port string + for key, obj in self.serial_objects.items(): + ret[key] = obj.serial_port + return ret + @staticmethod def check_serial_port(serial_port, baudrate): """檢查串口是否存在與可用""" @@ -619,5 +624,8 @@ if __name__ == '__main__': UDP_REMOTE_PORT = 14571 sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT) + linked_serial = sm.get_serial_link() + print(linked_serial) + time.sleep(10) sm.shutdown() \ No newline at end of file From 62356cc056d651fb6a8b5c8aac983a8a46e33d78 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Tue, 23 Dec 2025 14:45:19 +0800 Subject: [PATCH 085/146] =?UTF-8?q?(Tested)=20fix=20mainOrchestrator.py=20?= =?UTF-8?q?=E5=AE=8C=E5=96=84=20serial=20=E6=94=AF=E6=8F=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 181 ++++++++++++++++-- .../fc_network_adapter/serialManager.py | 6 +- 2 files changed, 171 insertions(+), 16 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index e4c0cf0..52655ff 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -38,15 +38,21 @@ class PanelState: self.object_manager_state = "Stopped" self.serial_manager_state = "Stopped" self.socket_object_list = [] # 已有的 mavlink object - self.linked_serial_dict = {} # 已連線的 serial 端口 + self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 - # 這邊是儲存關於 socket object 的資料 + # 關於創建通道時的暫存資訊 self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 - self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": True} # 暫存 Serial 設定資訊 - self.socket_info_single = {"socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", - "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", - "InfoReady": False} # 暫存單一 socket 的資訊 + self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊 + + # 關於顯示通道資訊 + self.socket_info_single = { + "socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", + "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", + "InfoReady": False} # 暫存單一 socket 的資訊 + self.serial_info_single = { + "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "", + "InfoReady": False} # 暫存單一 serial 連結的資訊 def intoSTART(self): self.panel_status = "Running" @@ -178,6 +184,15 @@ class ControlPanel: def show_object_info(self, stdscr, socket_id, state: PanelState): """顯示物件詳細資訊的對話框""" + + start = time.time() + while not state.socket_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + height, width = stdscr.getmaxyx() dialog_height = 15 dialog_width = min(70, width - 4) @@ -187,13 +202,10 @@ class ControlPanel: dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) dialog_win.border() dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) - - while not state.socket_info_single.get('InfoReady', False): - time.sleep(0.05) # 等待資訊準備好 # 這裡顯示基本資訊 dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") - # dialog_win.addstr(3, 2, f"Socket status : 運行中") + dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}") # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") @@ -300,6 +312,10 @@ class ControlPanel: # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), ]), MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Link to Middleware", "方便功能 可以直接建立 UDP object", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ + MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), + MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), + ]), MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), MenuNode("返回", "回到列表", "BACK"), ]) @@ -369,8 +385,55 @@ class ControlPanel: menu.current_page = page return menu - def show_linked_serial_info(self): - pass + def show_linked_serial_info(self, stdscr, serial_id, state: PanelState): + """顯示 Serial 連結詳細資訊的對話框""" + + start = time.time() + while not state.serial_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD) + + # 從 linked_serial_dict 獲取資訊 + serial_info = state.linked_serial_dict.get(serial_id, {}) + + if not serial_info: + dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊") + else: + # 顯示基本資訊 + dialog_win.addstr(2, 2, f"Serial ID : {serial_id}") + dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}") + dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}") + dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}") + dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}") + dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}") + + # 如果有額外資訊可以繼續添加 + if 'thread_alive' in serial_info: + thread_status = "Alive" if serial_info['thread_alive'] else "Stopped" + dialog_win.addstr(8, 2, f"Thread Status : {thread_status}") + + state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() # ================ 關於 主要選單 的部份 =================== @@ -481,6 +544,9 @@ class ControlPanel: desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" + elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP": + link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No" + desc = f"{child.desc} [{link_status}]" line = f"{marker}{child.name:15s} – {desc}" attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL @@ -643,6 +709,16 @@ class ControlPanel: menu_stack.pop() idx_stack.pop() + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES": + logger.info("mark A") + state.serial_info_temp["Go2Middleware"] = True + menu_stack.pop() + idx_stack.pop() + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO": + state.serial_info_temp["Go2Middleware"] = False + menu_stack.pop() + idx_stack.pop() + elif selected.action == "CREATE_SERIAL_PORT": state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱 cmd_q.put("CREATE_SERIAL_PORT") @@ -663,6 +739,24 @@ class ControlPanel: menu_stack.append(created_list_menu) idx_stack.append(0) + elif selected.action == "INSPECT_LINKED_SERIAL": + # 顯示 Serial 連結詳細資訊 + if hasattr(selected, 'serial_id'): + cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id)) + self.show_linked_serial_info(stdscr, selected.serial_id, state) + + elif selected.action == "REMOVE_LINKED_SERIAL": + # 移除 Serial 連結 + if hasattr(selected, 'serial_id'): + cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 一樣退兩層 + menu_stack.pop() + idx_stack.pop() + elif selected.action == "LIST_MAV_OBJECT": # 動態生成 mavlink_object 列表選單 created_list_menu = self.create_object_list_menu(state, page=0) @@ -714,7 +808,6 @@ class ControlPanel: if hasattr(selected, 'socket_id'): target_id = self.select_target_socket(stdscr, selected.socket_id, state) if target_id is not None: - # cmd_q.put(("MAVOBJ_MAKE_LINK", selected.socket_id, target_id)) cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 @@ -770,6 +863,7 @@ class ControlPanel: class Orchestrator: def __init__(self, stop_sig): self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個) + self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...] # === 1) 面板部分的準備 === self.cmd_q = queue.Queue() @@ -855,7 +949,7 @@ class Orchestrator: mav_obj = self.manager.managed_objects.get(socket_id, None) if mav_obj: self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type - # self.panelState.socket_info_single["socket_state"] = "Running" if mav_obj.running + self.panelState.socket_info_single["socket_state"] = mav_obj.state.name self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id @@ -864,6 +958,18 @@ class Orchestrator: self.panelState.socket_info_single["socket_connection_string"] = f"{ip_info[0]}:{ip_info[1]}" # getattr(mav_obj.mavlink_socket, "connection_string", "") self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "INSPECT_LINKED_SERIAL": + serial_id = cmd[1] + serial_obj = self.plumber.serial_objects.get(serial_id, None) + if serial_obj: + self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port + self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate + self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name + self.panelState.serial_info_single["target_port"] = serial_obj.target_port + self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "REMOVE_LINKED_SERIAL": + serial_id = cmd[1] + self.plumber.remove_serial_link(serial_id) elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" @@ -919,18 +1025,35 @@ class Orchestrator: # =============== 面板動作 - Mavlink Object =============== def create_udp_object(self): + # 監聽部分 if self.panelState.udp_info_temp["direction"] == "inbound": connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + # 監聽的 port 要先檢查是否被佔用 + ip = self.panelState.udp_info_temp['IP'] + port = int(self.panelState.udp_info_temp['Port']) + port_check_list = self.occupied_ip_ports.get(ip, []) + self.occupied_ip_ports.get("0.0.0.0", []) + if port in port_check_list: + self.panelState.panel_info_msg_list.append((f"Failed! Port {port} on IP {ip} occupied.", time.time()-1)) + return + # 再記錄被佔用的 port + if ip in self.occupied_ip_ports: + self.occupied_ip_ports[ip].append(port) + else: + self.occupied_ip_ports[ip] = [port] + # 外放資訊部分 elif self.panelState.udp_info_temp["direction"] == "outbound": connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" try: + # 檢測這個 connection_string 是否能成功建立 mavlink 連結 mavlink_socket = mavutil.mavlink_connection(connection_string) except Exception as e: self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) return + # mavlink 連結建立成功 把他丟到 mavlink_object mavlink_object = mo.mavlink_object(mavlink_socket) mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 self.manager.add_mavlink_object(mavlink_object) self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) @@ -967,6 +1090,14 @@ class Orchestrator: # =============== 面板動作 - Serial Manager =============== def create_serial_port_object(self): + # 先檢查是否已有相同的 Serial Port 被建立 + serial_port_strs = self.panelState.linked_serial_dict.values() # linked_serial_dict 會在上面的 mainLoop 被不斷更新 + if self.panelState.serial_info_temp['Port'] in serial_port_strs: + self.panelState.panel_info_msg_list.append( + (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time()) + ) + return + # 獲取可用的 udp port udp_port_tmp = find_available_port(19000, 20000) @@ -1004,6 +1135,16 @@ class Orchestrator: self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) return + self.panelState.panel_info_msg_list.append((f"Created Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + + # 自動建立對應的 UDP 監聽端口 + if self.panelState.serial_info_temp['Go2Middleware']: + self.panelState.udp_info_temp['IP'] = "127.0.0.1" + self.panelState.udp_info_temp['Port'] = str(udp_port_tmp) + self.panelState.udp_info_temp['direction'] = "inbound" + self.create_udp_object() + + def main(): stop_evt = threading.Event() @@ -1024,3 +1165,15 @@ if __name__ == "__main__": main() +''' +================= 改版記錄 ============================ + +2025.12.23 +1. 新增 serial 通道的資訊顯示完整化 +2. 新增 serial 通道刪除功能 +3. 新增 serial 直接順便開 ip object +4. 修改 避免 serial 與 ip port 重複建立相同的通道 +5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死 + + +''' diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index e1f7a80..c4e6dba 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -359,7 +359,7 @@ class serial_manager: self.loop = None self.running = False self.serial_count = 0 - self.serial_objects = {} # serial id num : serial object + self.serial_objects = {} # serial id num : serial_object def __del__(self): self.loop = None @@ -626,6 +626,8 @@ if __name__ == '__main__': linked_serial = sm.get_serial_link() print(linked_serial) - time.sleep(10) + + sm.remove_serial_link(1) + time.sleep(3) sm.shutdown() \ No newline at end of file From e00880c6def8323ea4e366dbd0b871743596a1e7 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 26 Dec 2025 12:17:00 +0800 Subject: [PATCH 086/146] =?UTF-8?q?1.=20(modify)=20acquireSerial.py=20?= =?UTF-8?q?=E6=92=88=E5=8F=96=E5=A4=9A=E5=80=8B=E7=AB=AF=E5=8F=A3=E5=AD=97?= =?UTF-8?q?=E4=B8=B2=E5=8A=9F=E8=83=BD=202.=20(modify)=20=E8=AA=BF?= =?UTF-8?q?=E6=95=B4=E9=9D=A2=E6=9D=BF=E7=9A=84=E9=A1=AF=E7=A4=BA=E5=AD=97?= =?UTF-8?q?=E4=B8=B2=203.=20(remove)=20serial=5Fudp=5Fbitrans.py=20?= =?UTF-8?q?=E7=94=A8=E4=B8=8D=E5=88=B0=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 71 +++- .../fc_network_adapter/serialManager.py | 26 +- .../fc_network_adapter/serial_udp_bitrans.py | 340 ------------------ .../fc_network_adapter/utils/acquireSerial.py | 39 +- 4 files changed, 88 insertions(+), 388 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 52655ff..4a9f16a 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -21,6 +21,7 @@ from pymavlink import mavutil # 自定義的 import from . import mavlinkObject as mo from . import serialManager as sm +from . import mavlinkVehicleView as mvv from .utils import RingBuffer, setup_logger from .utils import acquireSerial, acquirePort @@ -142,7 +143,7 @@ class ControlPanel: children = [] if not state.socket_object_list: - children.append(MenuNode("(空)", "目前沒有連結口", None)) + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) else: total_items = len(state.socket_object_list) total_pages = (total_items + items_per_page - 1) // items_per_page @@ -158,7 +159,7 @@ class ControlPanel: MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), - MenuNode("返回", "回到列表", "BACK"), + MenuNode("GoUp", "回到列表", "BACK"), ]) # 將 socket_id 附加到每個子選單項目上 for child in obj_menu.children: @@ -177,7 +178,7 @@ class ControlPanel: next_node.page = page + 1 children.append(next_node) - children.append(MenuNode("返回", "回到上層選單", "BACK")) + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) menu.current_page = page return menu @@ -294,10 +295,10 @@ class ControlPanel: # 獲取可用的 Serial 連接埠列表 # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 - serial_ports = acquireSerial.get_serial_ports_with_filter('/dev/ttyUSB*') + serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) if not serial_ports: - children.append(MenuNode("(空)", "目前沒有串口設備", None)) + children.append(MenuNode("(Empty)", "目前沒有串口設備", None)) else: total_items = len(serial_ports) total_pages = (total_items + items_per_page - 1) // items_per_page @@ -312,12 +313,12 @@ class ControlPanel: # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), ]), MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), - MenuNode("Link to Middleware", "方便功能 可以直接建立 UDP object", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ + MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), ]), MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), - MenuNode("返回", "回到列表", "BACK"), + MenuNode("GoUp", "回到列表", "BACK"), ]) # 將 port 附加到每個子選單項目上 for child in port_menu.children: @@ -336,7 +337,7 @@ class ControlPanel: next_node.page = page + 1 children.append(next_node) - children.append(MenuNode("返回", "回到上層選單", "BACK")) + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) menu.current_page = page return menu @@ -346,7 +347,7 @@ class ControlPanel: children = [] if not state.linked_serial_dict: - children.append(MenuNode("(空)", "目前沒有連結口", None)) + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) else: total_items = len(state.linked_serial_dict) total_pages = (total_items + items_per_page - 1) // items_per_page @@ -361,7 +362,7 @@ class ControlPanel: MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), - MenuNode("返回", "回到列表", "BACK"), + MenuNode("GoUp", "回到列表", "BACK"), ]) # 將 serial_id 附加到每個子選單項目上 for child in obj_menu.children: @@ -380,7 +381,7 @@ class ControlPanel: next_node.page = page + 1 children.append(next_node) - children.append(MenuNode("返回", "回到上層選單", "BACK")) + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) menu.current_page = page return menu @@ -435,6 +436,49 @@ class ControlPanel: stdscr.clear() stdscr.refresh() +# ================ 關於載具檢視的部份 =================== + + def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線載具 列表選單（支持分頁）""" + children = [] + + if not state.connected_vehicles_dict: + children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None)) + else: + total_items = len(state.connected_vehicles_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + vehicle_id_list = list(state.connected_vehicles_dict.keys()) + # 顯示當前頁的物件 + for vehicle_id in vehicle_id_list[start_idx:end_idx]: + vehicle_menu = MenuNode(f"Vehicle #{vehicle_id}", f"載具 {vehicle_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_VEHICLE"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 vehicle_id 附加到每個子選單項目上 + for child in vehicle_menu.children: + child.vehicle_id = vehicle_id + children.append(vehicle_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + # ================ 關於 主要選單 的部份 =================== def menu_tree(self): @@ -457,8 +501,9 @@ class ControlPanel: ]), MenuNode("Serial Manager", "Serial 連接埠選項", children=[ MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), - MenuNode("ListAll", "顯示已連線的 Serial", action = "LIST_SERIAL_LINKS"), + MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), ]), + MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), MenuNode("Engineer Mode", "工程模式", children=[ MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), @@ -865,6 +910,8 @@ class Orchestrator: self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個) self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...] + self.vehicle_registry = mvv.vehicle_registry + # === 1) 面板部分的準備 === self.cmd_q = queue.Queue() self.panelState = PanelState() # 面板的狀態儲存 diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index c4e6dba..02588e1 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -237,7 +237,7 @@ class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial # 處理 AT Command 回應 # response = XBeeFrameHandler.parse_at_command_response(an_frame) # self.at_handler.handle_response(response) - pass # debug + pass elif frame_type == 0x90: # Receive Packet (RX) payload 先解碼 @@ -279,16 +279,6 @@ class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial # print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}") - # def write_to_serial(self, data): - # # 在資料透過 Serial 發送之前進行處理 - # processed_data = self.encapsulate_data(data) - - # # 處理失敗就不發送 - # if processed_data not None: - # self.transport.write(processed_data) - - - class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP @@ -311,25 +301,13 @@ class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處 # UDP 收到資料的處理過程 def datagram_received(self, data, addr): # 儲存對方的地址（這樣就能向同一個來源回傳數據） - # self.remote_addr = addr + # self.remote_addr = addr # debug # print(f"Received UDP data from {addr}, setting as remote address") processed_data = XBeeFrameHandler.encapsulate_data(data) if self.serial_handler: self.serial_handler.transport.write(processed_data) - - - # def send_udp(self, data): - # # 藉由 UDP 發送資料出去 - - # # 在透過 UDP 發送數據之前進行解封裝 - # decoded_data = self.decapsulate_data(data) - # if decoded_data is None: - # return - - # if self.transport: - # self.transport.sendto(decoded_data, (self.LOCAL_HOST_IP, self.target_port)) #================================================================== diff --git a/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py b/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py deleted file mode 100644 index cb4373c..0000000 --- a/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py +++ /dev/null @@ -1,340 +0,0 @@ -import asyncio -import serial_asyncio - -# 附加驗證功能 -import os -import sys -import serial -import signal - -# XBee 模組 -from xbee.frame import APIFrame - -# ========================= - -SERIAL_PORT = '/dev/ttyACM0' # serial port -SERIAL_BAUDRATE = 57600 # serial baudrate - -UDP_REMOTE_IP = '127.0.0.1' # UDP Target IP -UDP_REMOTE_PORT = 14550 # UDP Target port - -# 測試用 只會吃一次資料 -DEBUG_MODE = False - -# ========================= - -def check_serial_port(): - """檢查串口是否存在與可用""" - # 檢查設備是否存在 - if not os.path.exists(SERIAL_PORT): - print(f"錯誤：串口設備 {SERIAL_PORT} 不存在") - return False - - # 檢查是否有權限訪問設備 - try: - os.access(SERIAL_PORT, os.R_OK | os.W_OK) - except Exception as e: - print(f"錯誤：無法訪問串口設備 {SERIAL_PORT}：{str(e)}") - return False - - # 檢查是否被占用 - try: - # 嘗試打開串口 - ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE) - ser.close() # 打開成功後立即關閉 - return True - except serial.SerialException as e: - print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}") - return False - except Exception as e: - print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}") - return False - -# ========================= - -class SerialToUDP(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 - def __init__(self, udp_protocol): - self.udp_protocol = udp_protocol - self.first_data = True # 標記是否為第一次收到資料 - self.has_processed = False # 測試模式用 處理數據旗標 # debug - - def connection_made(self, transport): - self.transport = transport - if hasattr(self.udp_protocol, 'set_serial_transport'): - self.udp_protocol.set_serial_transport(self) - print(f"Serial connection established on {SERIAL_PORT}") - - ## ===================================== - - # Serial 收到資料，轉發到 UDP - def data_received(self, data): - # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 # debug - if DEBUG_MODE and self.has_processed: - return - - # 標記首次收到資料 - if hasattr(self.udp_protocol, 'send_udp'): - if self.first_data: - print(f"First data received from serial, forwarding to UDP: {data[:20]}...") - self.first_data = False - - # 轉發數據 - self.udp_protocol.send_udp(data) - - if DEBUG_MODE: # 測試模式用 # debug - self.has_processed = True - print("[DEBUG] SerialToUDP Mark") - - def write_to_serial(self, data): - # 在資料透過 Serial 發送之前進行處理 - processed_data = self.encapsulate_data(data) - - # 處理失敗就不發送 - if processed_data not None: - self.transport.write(processed_data) - - def encapsulate_data(self, data): - - """ - 將數據封裝為 XBee API 傳輸幀 - - 使用 XBee API 格式封裝數據: - - 傳輸請求幀 (0x10) - - 使用廣播地址 - - 添加適當的頭部和校驗和 - """ - - ## 方法一 - # 設置幀參數 - frame_id = 0x01 # 幀識別碼，用於確認 - frame_type = 0x10 # 幀類型：傳輸請求 - dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 64位目標地址 (廣播) - dest_addr16 = b'\xFF\xFE' # 16位目標地址 (未知/廣播) - broadcast_radius = 0x00 # 廣播半徑 (0 = 最大) - options = 0x00 # 傳輸選項 - - # 構建幀數據部分 - frame_data = bytearray() - frame_data.append(frame_type) # 添加幀類型 - frame_data.append(frame_id) # 添加幀 ID - frame_data.extend(dest_addr64) # 添加 64 位目標地址 - frame_data.extend(dest_addr16) # 添加 16 位目標地址 - frame_data.append(broadcast_radius) # 添加廣播半徑 - frame_data.append(options) # 添加選項 - frame_data.extend(data) # 添加實際數據內容 - - # 計算校驗和 (0xFF 減去所有字節的總和的最低 8 位) - checksum = 0xFF - (sum(frame_data) & 0xFF) - - # 構建完整的 API 幀 - # 起始分隔符 + 長度 (兩字節) + 幀數據 + 校驗和 - complete_frame = bytearray() - complete_frame.append(0x7E) # 添加起始分隔符 - complete_frame.extend(struct.pack(">H", len(frame_data))) # 添加長度 (高位優先) - complete_frame.extend(frame_data) # 添加幀數據 - complete_frame.append(checksum) # 添加校驗和 - - return bytes(complete_frame) - - ## 方法二 - # frame_id=0x01 - # frame_type = 0x10 - # dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 - # 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) - - -class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 - def __init__(self): - self.serial_transport = None - self.transport = None - self.remote_addr = None # 儲存動態獲取的遠程地址 - self.has_processed = False # 測試模式用 處理數據旗標 # debug - - def connection_made(self, transport): - self.transport = transport - print("UDP transport ready. Waiting for serial data before sending...") - - def set_serial_transport(self, serial_transport): - self.serial_transport = serial_transport - - ## ===================================== - - # UDP 收到資料 - def datagram_received(self, data, addr): - # 儲存對方的地址（這樣就能向同一個來源回傳數據） - # self.remote_addr = addr - # print(f"Received UDP data from {addr}, setting as remote address") - - # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 - if DEBUG_MODE and self.has_processed: - return - - if self.serial_transport: - self.serial_transport.write_to_serial(data) - - if DEBUG_MODE: # 測試模式用 - self.has_processed = True - print("[DEBUG] UDPHandler Mark") - - def send_udp(self, data): - # 發送資料到 UDP - - # if self.transport: - # # 如果有已知的回應地址，使用該地址 - # if self.remote_addr: - # self.transport.sendto(data, self.remote_addr) - # # print(f"Sending to dynamic address: {self.remote_addr}") - # else: - # # 否則使用預設地址 - # self.transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) - # print(f"Sending first UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") - - if self.transport: - # 只有第一次或地址改變時才顯示 - # if not hasattr(self, '_last_sent_addr') or self._last_sent_addr != (UDP_REMOTE_IP, UDP_REMOTE_PORT): - # print(f"Sending UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") - # self._last_sent_addr = (UDP_REMOTE_IP, UDP_REMOTE_PORT) - - # 在透過 UDP 發送數據之前進行解封裝 - decoded_data = self.decapsulate_data(data) - self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) - - def decapsulate_data(self, data): - # 這裡可以根據需要進行數據解封裝 - - ## 方法一 - try: - # 創建一個 API 幀處理器 - api_frame = APIFrame(escaped=True) - - # 嘗試解析數據 - api_frame.fill(data) - - # 如果數據不完整，直接返回原始數據 - if not api_frame.is_complete(): - return data - - # 解析幀內容 - parsed_data = api_frame.parse() - - # 提取有用數據 - if parsed_data: - frame_data = parsed_data.get('rf_data', None) - if frame_data: - return frame_data - - return data - - ## 方法二 - 手動解析 - # try: - # # XBee API 幀格式: - # # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) - - # # 檢查幀起始符 (0x7E) - # if not data or len(data) < 5 or data[0] != 0x7E: - # return data - - # # 獲取數據長度 (不包括校驗和) - # length = (data[1] << 8) + data[2] - - # # 檢查幀完整性 - # if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 - # return data - - # # 提取API標識符和數據 - # frame_type = data[3] - # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 - - # # 根據不同的幀類型進行處理 - # if frame_type == 0x90: # 例如，這是"接收數據包"類型 - # # 提取實際有效載荷 - # # 對於接收數據包，格式通常是： - # # API標識符(1) + 64位地址(8) + 16位地址(2) + 選項(1) + 數據 - # if len(frame_data) >= 12: # 確保數據長度足夠 - # payload = frame_data[11:] # 前11字節是地址和選項 - # return payload - # return data - - # 如果無法提取 則回傳 None - except Exception as e: - print(f"手動解析 XBee 數據錯誤: {e}") - return None - -async def main(): - # 先檢查串口是否可用 - if not check_serial_port(): - print("程式終止：串口檢查失敗") - return - - loop = asyncio.get_running_loop() - - # 設置終止處理 - for sig in (signal.SIGINT, signal.SIGTERM): - loop.add_signal_handler( - sig, - lambda: asyncio.create_task(shutdown(loop)) - ) - - # 建立單一 UDP 端點處理收發 - udp_handler = UDPHandler() - - # 建立 UDP 傳輸，不指定接收端口，讓系統自動分配 - try: - udp_transport, protocol = await loop.create_datagram_endpoint( - lambda: udp_handler, - local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 - ) - except Exception as e: - print(f"無法創建 UDP 端點：{str(e)}") - return - - # 獲取系統分配的本地端口 - sock = udp_transport.get_extra_info('socket') - local_addr = sock.getsockname() - print(f"UDP listening on {local_addr[0]}:{local_addr[1]}") - - # 建立 Serial 傳輸，將 UDP 處理器傳給它 - try: - serial_proto = SerialToUDP(udp_handler) - _, serial_transport = await serial_asyncio.create_serial_connection( - loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE - ) - except Exception as e: - print(f"無法建立串口連接：{str(e)}") - udp_transport.close() - return - - print(f"Waiting for data from serial port to initiate UDP communication...") - - # 保持運行 - try: - await asyncio.Future() - except asyncio.CancelledError: - pass - -async def shutdown(loop): - """關閉程序""" - print("Shutting down...") - tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()] - - for task in tasks: - task.cancel() - - await asyncio.gather(*tasks, return_exceptions=True) - loop.stop() - -if __name__ == '__main__': - try: - asyncio.run(main()) - except KeyboardInterrupt: - print("程式被使用者中斷") - except Exception as e: - print(f"程式執行錯誤：{str(e)}") \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py index 703ae36..1e1a73f 100644 --- a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py @@ -6,7 +6,7 @@ It uses glob pattern matching to find serial device files in /dev/. """ import glob -from typing import List +from typing import List, Union def get_serial_ports() -> List[str]: @@ -44,29 +44,44 @@ def get_serial_ports() -> List[str]: return serial_ports -def get_serial_ports_with_filter(filter_pattern: str = None) -> List[str]: +def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]: """ 獲取串口設備列表，可選擇性地使用自訂篩選模式 Args: - filter_pattern (str, optional): 自訂的 glob 模式，例如 '/dev/ttyUSB*' - 如果為 None，則使用預設模式搜尋所有串口 + filter_patterns (Union[str, List[str]], optional): + 單一或多個 glob 模式 + - 字串: '/dev/ttyUSB*' + - 列表: ['/dev/ttyUSB*', '/dev/ttyACM*'] + 如果為 None，則使用預設模式搜尋所有串口 Returns: List[str]: 符合條件的串口設備路徑列表 Example: - >>> # 只搜尋 USB 串口 - >>> usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*') - >>> print(usb_ports) + >>> # 單一 pattern + >>> ports = get_serial_ports_with_filter('/dev/ttyUSB*') + >>> print(ports) ['/dev/ttyUSB0', '/dev/ttyUSB1'] + + >>> # 多個 patterns + >>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + >>> print(ports) + ['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1'] """ - if filter_pattern: - serial_ports = glob.glob(filter_pattern) - serial_ports.sort() - return serial_ports - else: + if filter_patterns is None: return get_serial_ports() + + # 統一轉成 list 處理 + if isinstance(filter_patterns, str): + filter_patterns = [filter_patterns] + + serial_ports = [] + for pattern in filter_patterns: + serial_ports.extend(glob.glob(pattern)) + + serial_ports.sort() + return serial_ports if __name__ == "__main__": From f8f5ff5a152878f4af61cddafd0febc769934694 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 29 Dec 2025 12:58:38 +0800 Subject: [PATCH 087/146] =?UTF-8?q?Tested=20(modify)=201.=20=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=E5=B7=B2=E9=80=A3=E7=B7=9A=E8=BC=89=E5=85=B7=E7=9A=84?= =?UTF-8?q?=E5=9F=BA=E7=A4=8E=E8=B3=87=E8=A8=8A=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mainOrchestrator.py | 345 +++++++++++++++++- .../fc_network_adapter/mavlinkVehicleView.py | 2 + 2 files changed, 332 insertions(+), 15 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 4a9f16a..ed78fbd 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -55,6 +55,22 @@ class PanelState: "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "", "InfoReady": False} # 暫存單一 serial 連結的資訊 + # 關於顯示載具資訊 + self.connected_vehicles_dict = {} # {(sysid, compid): {...基本資訊...}} + self.vehicle_info_single = { + "sysid": 0, + "compid": 0, + "vehicle_type": "", + "component_type": "", + "mav_autopilot": "", + "socket_id": None, + "connection_type": "", + "packet_stats": {}, + "msg_type_counts": {}, + "prev_stats": {}, # 用於計算變化率 + "InfoReady": False + } + def intoSTART(self): self.panel_status = "Running" @@ -439,7 +455,9 @@ class ControlPanel: # ================ 關於載具檢視的部份 =================== def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 已連線載具 列表選單（支持分頁）""" + """動態創建 已連線載具 列表選單（支持分頁） + 每個 vehicle-component 組合都是獨立的選單項目 + """ children = [] if not state.connected_vehicles_dict: @@ -450,16 +468,19 @@ class ControlPanel: start_idx = page * items_per_page end_idx = min(start_idx + items_per_page, total_items) - vehicle_id_list = list(state.connected_vehicles_dict.keys()) + # vehicle_id_list 現在是 (sysid, compid) 的元組列表 + vehicle_comp_list = list(state.connected_vehicles_dict.keys()) + # 顯示當前頁的物件 - for vehicle_id in vehicle_id_list[start_idx:end_idx]: - vehicle_menu = MenuNode(f"Vehicle #{vehicle_id}", f"載具 {vehicle_id}", None, children=[ - MenuNode("Info", "查看詳細資訊", "INSPECT_VEHICLE"), - MenuNode("GoUp", "回到列表", "BACK"), - ]) - # 將 vehicle_id 附加到每個子選單項目上 - for child in vehicle_menu.children: - child.vehicle_id = vehicle_id + for sysid, compid in vehicle_comp_list[start_idx:end_idx]: + # 建立顯示名稱 + display_name = f"Vehicle #{sysid} - Comp #{compid}" + desc = f"載具 {sysid} 組件 {compid}" + + vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") + # 將 sysid 和 compid 附加到選單項目上 + vehicle_menu.sysid = sysid + vehicle_menu.compid = compid children.append(vehicle_menu) # 添加分頁控制 @@ -479,6 +500,172 @@ class ControlPanel: menu.current_page = page return menu + def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): + """顯示載具組件詳細資訊（動態更新，顯示變化率）""" + + # 等待資訊準備 + start = time.time() + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) + return + time.sleep(0.05) + + info = state.vehicle_info_single + height, width = stdscr.getmaxyx() + dialog_height = min(22, height - 4) + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 + dialog_win.keypad(True) + + # MAV_TYPE 名稱對應 + MAV_TYPE_NAMES = { + 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", + 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", + 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" + } + + # 動態更新迴圈 + last_update = time.time() + while True: + current_time = time.time() + + # 每 1 秒重新請求資料 + if current_time - last_update >= 1.0: + # 觸發資料更新（透過 Orchestrator） + cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) + # 等待新資料準備好 + wait_start = time.time() + state.vehicle_info_single['InfoReady'] = False + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - wait_start > 0.5: # 最多等 0.5 秒 + break + time.sleep(0.01) + # 更新 info 參照 + info = state.vehicle_info_single + last_update = current_time + + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) + + # === 基礎資訊 === + dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) + dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) + + # # MAV Type # 這個用不到 + # mav_type = info.get('vehicle_type', 'N/A') + # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) + # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") + + # Component Type + dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") + + # Autopilot Type + if info.get('mav_autopilot') is not None: + dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") + + # Connection Info + dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") + dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") + + # === 封包統計 === + stats = info.get('packet_stats', {}) + dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) + + received = stats.get('received', 0) + lost = stats.get('lost', 0) + loss_rate = stats.get('loss_rate', 0.0) + last_seq = stats.get('last_seq', 'N/A') + + # 計算變化 + received_delta = stats.get('received_delta', 0) + lost_delta = stats.get('lost_delta', 0) + + # 顯示變化率 + recv_str = f"{received:6d}" + if received_delta > 0: + recv_str += f" (+{received_delta}↑)" + + lost_str = f"{lost:4d}" + if lost_delta > 0: + lost_str += f" (+{lost_delta}↑)" + + dialog_win.addstr(8, 2, f"Received : {recv_str}") + dialog_win.addstr(8, 32, f"Lost : {lost_str}") + dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") + dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") + + # 最後接收時間 + last_msg_time = stats.get('last_msg_time') + if last_msg_time: + time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) + elapsed = current_time - last_msg_time + dialog_win.addstr(10, 2, f"Last Time : {time_str}") + dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") + else: + dialog_win.addstr(10, 2, "Last Time : N/A") + + # === 訊息類型分佈 === + dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) + + msg_counts = info.get('msg_type_counts', {}) + + # MAVLink 訊息名稱對應（縮寫版本） + MSG_NAMES = { + 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", + 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", + 74: "VFR_HUD", 147: "BATT_ST" + } + + # 顯示前 12 個最常見的訊息類型（兩列各 6 個） + msg_items = list(msg_counts.items())[:12] + line = 13 + for i, (msg_id, count) in enumerate(msg_items): + msg_name = MSG_NAMES.get(msg_id, "???") + delta = stats.get(f'msg_delta_{msg_id}', 0) + + # 格式化數據 + if delta > 0: + data_str = f"{count}(+{delta}↑)" + else: + data_str = f"{count}" + + # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) + display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" + + # 左列(偶數索引)或右列(奇數索引) + col = 2 if i % 2 == 0 else 36 + row = line + (i // 2) + + if row >= dialog_height - 3: # 避免超出邊界 + break + + dialog_win.addstr(row, col, display_str) + + # 確保跳過顯示區域 + line = line + 6 + + dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") + dialog_win.refresh() + + # 檢查是否有按鍵（非阻塞） + ch = dialog_win.getch() + if ch != -1: # 有按鍵則退出 + break + + # 短暫延遲 + time.sleep(0.1) + + state.vehicle_info_single['InfoReady'] = False + del dialog_win + stdscr.clear() + stdscr.refresh() + # ================ 關於 主要選單 的部份 =================== def menu_tree(self): @@ -821,6 +1008,8 @@ class ControlPanel: created_list_menu = self.create_object_list_menu(state, page=selected.page) elif current_list_menu.name == "Linked Serial List": created_list_menu = self.create_linked_serial_menu(state, page=selected.page) + elif current_list_menu.name == "Connected Vehicles": + created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) else: # 不支援的選單類型，回到原本的選單 menu_stack.append(current_list_menu) @@ -892,6 +1081,19 @@ class ControlPanel: continue # 只有在工程模式下才能操作 cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + elif selected.action == "INSPECT_VEHICLES": + # 進入載具檢視選單 + cmd_q.put("UPDATE_VEHICLES_LIST") + created_list_menu = self.create_vehicles_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_VEHICLE": + # 顯示載具詳細資訊 + if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): + cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) + self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) + elif callable(selected.action): # 執行函式 cmd_q.put(selected.action) @@ -968,6 +1170,9 @@ class Orchestrator: linked_serial_dict = self.plumber.get_serial_link() self.panelState.linked_serial_dict = linked_serial_dict + # B. 更新載具列表（從 vehicle_registry 獲取） + self._update_vehicles_list() + # 取出面板丟過來的「動作」 try: cmd = self.cmd_q.get_nowait() @@ -1018,6 +1223,12 @@ class Orchestrator: serial_id = cmd[1] self.plumber.remove_serial_link(serial_id) + elif action == "INSPECT_VEHICLE": + sysid, compid = cmd[1], cmd[2] + self._prepare_vehicle_info(sysid, compid) + elif action == "UPDATE_VEHICLES_LIST": + self._update_vehicles_list() + elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" self.create_udp_object() @@ -1071,7 +1282,7 @@ class Orchestrator: # =============== 面板動作 - Mavlink Object =============== - def create_udp_object(self): + def create_udp_object(self, socket_type:str = ""): # 監聽部分 if self.panelState.udp_info_temp["direction"] == "inbound": connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" @@ -1097,11 +1308,18 @@ class Orchestrator: except Exception as e: self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) return - # mavlink 連結建立成功 把他丟到 mavlink_object + + # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句 mavlink_object = mo.mavlink_object(mavlink_socket) - mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() - # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 + # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句 self.manager.add_mavlink_object(mavlink_object) + + # 設定一下 mavlink_object 的類型描述 + if socket_type == "": + mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + else: + mavlink_object.socket_type = socket_type + self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) def delete_mavlink_object(self, socket_id): @@ -1134,6 +1352,103 @@ class Orchestrator: else: self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + # =============== 面板動作 - Vehicle Inspector =============== + + def _update_vehicles_list(self): + """更新已連線載具列表（從 vehicle_registry 獲取）""" + vehicles_dict = {} + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = self.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + # 遍歷每個載具的所有組件 + for compid, component in vehicle.components.items(): + # 使用 (sysid, compid) 作為 key + vehicles_dict[(sysid, compid)] = { + 'sysid': sysid, + 'compid': compid, + 'vehicle_type': vehicle.vehicle_type, + 'component_type': component.type.value, + 'connection_via': vehicle.connected_via.value, + 'socket_id': vehicle.custom_meta.get('socket_id', 'N/A') + } + + self.panelState.connected_vehicles_dict = vehicles_dict + + def _prepare_vehicle_info(self, sysid, compid): + """準備載具組件的詳細資訊（包含變化率計算）""" + vehicle = self.vehicle_registry.get(sysid) + if not vehicle: + logger.warning(f"Vehicle {sysid} not found") + return + + socket_id = vehicle.custom_meta.get('socket_id', 'N/A') + + component = vehicle.get_component(compid) + if not component: + logger.warning(f"Component {compid} not found in vehicle {sysid}") + return + + stats = component.packet_stats + + # 取得之前的統計資料（用於計算變化） + prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {}) + prev_received = prev_stats.get('received', 0) + prev_lost = prev_stats.get('lost', 0) + prev_msg_counts = prev_stats.get('msg_counts', {}) + + # 計算變化率 + received_delta = stats.received_count - prev_received + lost_delta = stats.lost_count - prev_lost + + # 準備訊息類型計數（排序後取前幾個） + sorted_msg_counts = dict(sorted( + stats.msg_type_count.items(), + key=lambda x: x[1], + reverse=True + )[:12]) # 取前 12 個最常見的 + + # 計算每種訊息類型的變化 + msg_deltas = {} + for msg_id, count in sorted_msg_counts.items(): + prev_count = prev_msg_counts.get(msg_id, 0) + msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count + + # 更新 vehicle_info_single + socket_type = "N/A" + socket_obj = self.manager.managed_objects.get(socket_id, None) + if socket_obj: + socket_type = socket_obj.socket_type + + self.panelState.vehicle_info_single = { + "sysid": sysid, + "compid": compid, + # "vehicle_type": vehicle.vehicle_type, # 這個用不到 + "component_type": component.type.value, + "mav_autopilot": component.mav_autopilot, + "socket_id": socket_id, + "connection_type": socket_type, + "packet_stats": { + "received": stats.received_count, + "lost": stats.lost_count, + "loss_rate": (stats.lost_count / stats.received_count * 100 + if stats.received_count > 0 else 0), + "last_seq": stats.last_seq, + "last_msg_time": stats.last_msg_time, + "received_delta": received_delta, + "lost_delta": lost_delta, + **msg_deltas # 展開訊息類型的變化 + }, + "msg_type_counts": sorted_msg_counts, + "prev_stats": { # 保存當前數據用於下次計算變化 + "received": stats.received_count, + "lost": stats.lost_count, + "msg_counts": dict(stats.msg_type_count) + }, + "InfoReady": True + } + # =============== 面板動作 - Serial Manager =============== def create_serial_port_object(self): @@ -1189,7 +1504,7 @@ class Orchestrator: self.panelState.udp_info_temp['IP'] = "127.0.0.1" self.panelState.udp_info_temp['Port'] = str(udp_port_tmp) self.panelState.udp_info_temp['direction'] = "inbound" - self.create_udp_object() + self.create_udp_object("SERIAL_XBEE") def main(): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index 86c0478..f59eb17 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -292,6 +292,8 @@ class VehicleView: 4. 可擴充 (支援 RF 模組狀態) """ + # TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了 + def __init__(self, sysid: int): # Meta 資訊 self.sysid = sysid From 14b70f6e2e96b34c1b833fdde8229358866156b1 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 30 Dec 2025 17:25:06 +0800 Subject: [PATCH 088/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/communication.py | 511 +++++++++++ src/unitdev01/unitdev01/gui.py | 1022 ++++++++++++---------- 2 files changed, 1073 insertions(+), 460 deletions(-) create mode 100644 src/unitdev01/unitdev01/communication.py diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py new file mode 100644 index 0000000..9584a62 --- /dev/null +++ b/src/unitdev01/unitdev01/communication.py @@ -0,0 +1,511 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 主题检测定时器 + 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+_\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(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + 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 + ), + '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 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_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) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + 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) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + '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.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } \ No newline at end of file diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index a227321..5eac6a3 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -1,405 +1,16 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) -from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal +from PyQt6.QtCore import Qt, QTimer from PyQt6.QtWebEngineWidgets import QWebEngineView -import math -import re import sys -from threading import Lock -from geometry_msgs.msg import Point, Vector3 -from sensor_msgs.msg import BatteryState, NavSatFix, Imu -from std_msgs.msg import Float64 -from mavros_msgs.msg import State, VfrHud -from mavros_msgs.srv import CommandBool, CommandTOL, SetMode import asyncio -from functools import partial -import threading -import websockets -import json -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.arm_clients = {} - self.takeoff_clients = {} - self.setpoint_pubs = {} - self.selected_drones = set() - self.latest_data = {} - - # 定義需要過濾的模式 - self.filtered_modes = ['Mode(0x000000c0)'] - - # 定義多個 WebSocket 連接配置 - self.websocket_connections = [ - { - 'name': 'local_1', - 'url': 'ws://192.168.137.1:5163', - 'enabled': True - }, - { - 'name': 'local_2', - 'url': 'ws://0.0.0.0:8756', - 'enabled': True # 新增的端口 - }, - { - 'name': 'remote_8756', - 'url': 'ws://192.168.50.48:8756', - 'enabled': False # 可選擇啟用 - } - ] - - # 啟動多個 WebSocket client 執行緒 - for connection in self.websocket_connections: - if connection['enabled']: - threading.Thread( - target=self.start_ws_client, - args=(connection,), - daemon=True, - name=f"WebSocket-{connection['name']}" - ).start() - - # 主题检测定时器 - self.create_timer(1.0, self.scan_topics) - - def start_ws_client(self, connection_config): - """啟動單個 WebSocket 客戶端""" - asyncio.set_event_loop(asyncio.new_event_loop()) - asyncio.get_event_loop().run_until_complete( - self.ws_client_loop(connection_config) - ) - - async def ws_client_loop(self, connection_config): - """單個 WebSocket 連接的主循環""" - retry_count = 0 - max_retries = 5 - base_delay = 1.0 - connection_name = connection_config['name'] - url = connection_config['url'] - - print(f"Starting WebSocket client for {connection_name} at {url}") - - while retry_count < max_retries: - try: - async with websockets.connect(url) as websocket: - print(f"WebSocket {connection_name} connected to {url}") - retry_count = 0 # 重置重試計數 - - async for message in websocket: - try: - data = json.loads(message) - # 添加連接來源信息 - data['_connection_source'] = connection_name - self.process_websocket_message(data) - except json.JSONDecodeError as e: - print(f"WebSocket {connection_name} JSON decode error: {e}") - except Exception as e: - print(f"WebSocket {connection_name} message processing error: {e}") - - except websockets.exceptions.ConnectionClosedError: - print(f"WebSocket {connection_name} connection closed") - break - except Exception as e: - retry_count += 1 - delay = base_delay * (2 ** min(retry_count, 4)) # 指數退避 - print(f"WebSocket {connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})") - await asyncio.sleep(delay) - - print(f"WebSocket client {connection_name} stopped after maximum retries") - - def process_websocket_message(self, data): - try: - drone_id = data.get('system_id') - drone_id = f"s9_{drone_id}" if drone_id else None - if not drone_id: - return - - # 模式 - if 'mode' in data: - self.signals.update_signal.emit('state', drone_id, { - 'mode': data['mode'], - }) - - # 電池 - if 'battery' in data: - # 這裡假設 battery 是百分比 - self.signals.update_signal.emit('battery', drone_id, { - 'percentage': data['battery'] - }) - - # 位置 - if 'position' in data: - pos = data['position'] - self.signals.update_signal.emit('gps', drone_id, { - 'lat': pos.get('lat', 0), - 'lon': pos.get('lon', 0) - }) - - self.signals.update_signal.emit('local_pose', drone_id, { - 'x': round(pos.get('lat', 0), 6), - 'y': round(pos.get('lon', 0), 6), - }) - - # 航向 - if 'heading' in data: - self.signals.update_signal.emit('hud', drone_id, { - 'heading': data['heading'], - }) - - except Exception as e: - print(f"WebSocket message processing error: {e}") - - def scan_topics(self): - topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\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(drone_id) - - def setup_drone(self, drone_id): - base_topic = f'/MavLinkBus/{drone_id}' - - # Add service clients - self.arm_clients[drone_id] = self.create_client( - CommandBool, - f'{base_topic}/cmd/arming' - ) - self.takeoff_clients[drone_id] = self.create_client( - CommandTOL, - f'{base_topic}/cmd/takeoff' - ) - - # Add setpoint publisher - self.setpoint_pubs[drone_id] = self.create_publisher( - Point, - f'{base_topic}/setpoint_position/local', - 10 - ) - - 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 - ), - '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 - ), - 'loss_rate': self.create_subscription( - Float64, - f'{base_topic}/packet_loss_rate', - lambda msg, did=drone_id: self.loss_rate_callback(did, msg), - 10 - ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), - 'ping': self.create_subscription( - Float64, - f'{base_topic}/ping', - lambda msg, did=drone_id: self.ping_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) - - async def arm_drone(self, drone_id, arm): - if drone_id not in self.arm_clients: - return False - - client = self.arm_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandBool.Request() - request.value = arm - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Arm service call failed: {e}') - return False - - async def takeoff_drone(self, drone_id, altitude=10.0): - if drone_id not in self.takeoff_clients: - return False - - client = self.takeoff_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False - - request = CommandTOL.Request() - request.altitude = altitude - request.min_pitch = 0.0 - request.yaw = 0.0 - - future = client.call_async(request) - try: - response = await future - return response.success - except Exception as e: - self.get_logger().error(f'Takeoff service call failed: {e}') - return False - - def send_setpoint(self, drone_id, x, y, z): - """Send setpoint position command""" - if drone_id not in self.setpoint_pubs: - return False - - msg = Point() - msg.x = float(x) - msg.y = float(y) - msg.z = float(z) - - self.setpoint_pubs[drone_id].publish(msg) - return True - - 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) - - # callbacks - def attitude_callback(self, drone_id, msg): - if hasattr(msg, 'orientation'): - roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) - self.latest_data[(drone_id, 'attitude')] = { - '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.latest_data[(drone_id, 'battery')] = { - 'voltage': msg.voltage - } - - def state_callback(self, drone_id, msg): - mode = msg.mode - if mode in self.filtered_modes: - return - self.latest_data[(drone_id, 'state')] = { - 'mode': msg.mode, - 'armed': msg.armed - } - - def gps_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'gps')] = { - 'lat': msg.latitude, - 'lon': msg.longitude, - 'alt': msg.altitude - } - - def local_vel_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'velocity')] = { - 'vx': msg.x, - 'vy': msg.y, - 'vz': msg.z - } - - def altitude_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'altitude')] = { - 'altitude': msg.data - } - - def local_pose_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'local_pose')] = { - 'x': msg.x, - 'y': msg.y, - 'z': msg.z - } - - def hud_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'hud')] = { - 'airspeed': msg.airspeed, - 'groundspeed': msg.groundspeed, - 'heading': msg.heading, - 'throttle': msg.throttle, - 'alt': msg.altitude, - 'climb': msg.climb - } - - def loss_rate_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'loss_rate')] = { - 'loss_rate': msg.data - } - - def ping_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'ping')] = { - 'ping': msg.data - } +# 從 communication.py 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver class ControlStationUI(QMainWindow): def __init__(self): @@ -423,7 +34,7 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} - self.socket_groups = {} # 新增：用於儲存 socket 分組 + self.socket_groups = {} self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", "Roll", "Pitch", "Yaw", "丟包", "延遲"] self.info_type_map = { @@ -442,27 +53,29 @@ class ControlStationUI(QMainWindow): "ping": 12 } self.socket_colors = { - '0': '#00BFFF', # DeepSkyBlue - '1': '#FFD700', # Gold - '2': '#FF69B4', # HotPink - '9': '#7CFC00', # LawnGreen + '0': '#00BFFF', + '1': '#FFD700', + '2': "#FF6969", + '8': '#7CFC00', + '9': '#FF8C00', 'default': '#AAAAAA' } self.drone_positions = {} self.drone_headings = {} self.map_loaded = False - # 添加地圖更新節流定時器 self.map_update_timer = QTimer() self.map_update_timer.timeout.connect(self.update_map_positions) - self.map_update_timer.start(200) # 每 200ms 更新一次地圖 - # 添加待更新的無人機位置緩存 + self.map_update_timer.start(200) self.pending_map_updates = {} - self.init_ui() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.init_ui() def init_ui(self): - # 只呼叫一次 main_splitter = QSplitter(Qt.Orientation.Horizontal) @@ -482,17 +95,193 @@ class ControlStationUI(QMainWindow): scroll.setWidgetResizable(True) self.left_tab.addTab(scroll, "無人載具") - # 底部控制按鈕區域 - bottom_control = QWidget() - bottom_layout = QVBoxLayout(bottom_control) + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_tab = QWidget() + comm_layout = QVBoxLayout(self.comm_tab) + comm_layout.setContentsMargins(10, 10, 10, 10) + comm_layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP MAVLink") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + comm_layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self.handle_add_udp_connection) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + comm_layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + comm_layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + comm_layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + comm_layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self.handle_add_ws_connection) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + comm_layout.addWidget(add_ws_widget) + comm_layout.addStretch() + + self.left_tab.addTab(self.comm_tab, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + # 上方按鈕區域 upper_buttons = QHBoxLayout() select_all_btn = QPushButton("全選") select_all_btn.clicked.connect(self.handle_select_all) - arm_all_btn = QPushButton("批次解鎖") + arm_all_btn = QPushButton("解鎖") arm_all_btn.clicked.connect(self.handle_arm_selected) - takeoff_all_btn = QPushButton("批次起飛") + takeoff_all_btn = QPushButton("起飛") takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: btn.setStyleSheet(""" @@ -500,7 +289,7 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -509,29 +298,27 @@ class ControlStationUI(QMainWindow): upper_buttons.addWidget(btn) upper_buttons.addStretch() - # --- 模式切換區域 --- + # 模式切換區域 mode_layout = QHBoxLayout() - mode_layout.setContentsMargins(0, 0, 0, 0) - mode_layout.setSpacing(8) mode_label = QLabel("模式：") mode_label.setStyleSheet("color: #DDD; min-width: 40px;") from PyQt6.QtWidgets import QComboBox self.mode_combo = QComboBox() self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) - self.mode_combo.setCurrentIndex(1) # 預設 GUIDED + self.mode_combo.setCurrentIndex(1) self.mode_combo.setStyleSheet(""" QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} """) - batch_mode_btn = QPushButton("批次切換模式") + batch_mode_btn = QPushButton("切換") batch_mode_btn.clicked.connect(self.handle_batch_mode_change) batch_mode_btn.setStyleSheet(""" QPushButton { background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } @@ -542,10 +329,9 @@ class ControlStationUI(QMainWindow): mode_layout.addWidget(batch_mode_btn) mode_layout.addStretch() - # Add coordinate inputs + # 座標輸入區域 coord_widget = QWidget() coord_layout = QHBoxLayout(coord_widget) - coord_layout.setContentsMargins(0, 0, 0, 0) self.x_input = QLineEdit() self.y_input = QLineEdit() @@ -563,14 +349,13 @@ class ControlStationUI(QMainWindow): } """) - coord_layout.addWidget(QLabel("X:")) + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.x_input) - coord_layout.addWidget(QLabel("Y:")) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.y_input) - coord_layout.addWidget(QLabel("Z:")) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) coord_layout.addWidget(self.z_input) - # Add buttons to control layout setpoint_btn = QPushButton("Setpoint") setpoint_btn.clicked.connect(self.handle_setpoint_selected) setpoint_btn.setStyleSheet(""" @@ -578,44 +363,27 @@ class ControlStationUI(QMainWindow): background-color: #444; color: #DDD; border: none; - padding: 5px 10px; + padding: 8px 12px; border-radius: 4px; min-width: 80px; } QPushButton:hover { background-color: #555; } """) - # 下方座標輸入區域 lower_control = QHBoxLayout() lower_control.addWidget(coord_widget) lower_control.addWidget(setpoint_btn) lower_control.addStretch() - - # 加入底部控制區 - bottom_layout.addLayout(upper_buttons) - bottom_layout.addLayout(mode_layout) - bottom_layout.addLayout(lower_control) - - # — 分頁 2：Overview Table - self.overview_table = QTableWidget() - # 一開始只有一欄 - self.overview_table.setColumnCount(1) - self.overview_table.setRowCount(len(self.info_types)) - self.overview_table.setHorizontalHeaderLabels(["資訊"]) - header = self.overview_table.horizontalHeader() - # 只有第一欄自動收縮到內容寬度，其它欄不動 - header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) - self.overview_table.verticalHeader().setVisible(False) - for i, txt in enumerate(self.info_types): - item = QTableWidgetItem(txt) - item.setFlags(Qt.ItemFlag.ItemIsEnabled) - item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) - self.overview_table.setItem(i, 0, item) - - self.left_tab.addTab(self.overview_table, "總覽") + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) - # 右侧地图区域 + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 self.map_view = QWebEngineView() inline_html = ''' @@ -671,8 +439,9 @@ class ControlStationUI(QMainWindow): function getColorBySocketId(id) { if (id.startsWith("s0_")) return "#00BFFF"; if (id.startsWith("s1_")) return "#FFD700"; - if (id.startsWith("s2_")) return "#FF69B4"; - if (id.startsWith("s9_")) return "#7CFC00"; + if (id.startsWith("s2_")) return "#FF6969"; + if (id.startsWith("s8_")) return "#7CFC00"; + if (id.startsWith("s9_")) return "#FF8C00"; return "#AAAAAA"; } @@ -803,23 +572,15 @@ class ControlStationUI(QMainWindow): ''' self.map_view.setHtml(inline_html) self.map_view.loadFinished.connect(self.on_map_loaded) - - # Create left container and its layout - left_container = QWidget() - left_layout = QVBoxLayout(left_container) - left_layout.setContentsMargins(0, 0, 0, 0) - - # Add tab widget and bottom control to left container - left_layout.addWidget(self.left_tab) - left_layout.addWidget(bottom_control) + right_layout.addWidget(self.map_view) + # Add widgets to splitter - main_splitter.addWidget(left_container) - main_splitter.addWidget(self.map_view) + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) - def on_map_loaded(self, ok: bool): if ok: self.map_loaded = True @@ -1079,6 +840,339 @@ class ControlStationUI(QMainWindow): main_layout.addWidget(control_widget) return panel + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_udp_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_udp_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.toggle_ws_connection(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.remove_ws_connection(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def handle_add_ws_connection(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.statusBar().showMessage("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴（如果用戶沒有輸入協議前綴） + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + # 檢查是否包含 host:port 格式 + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.statusBar().showMessage("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.statusBar().showMessage("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 啟動接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_ws_connection_panel(new_conn) + self.ws_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.ws_url_input.clear() + + self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) + + def handle_add_udp_connection(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.statusBar().showMessage("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.statusBar().showMessage("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.statusBar().showMessage("連接已存在", 3000) + return + + # 創建新連接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 啟動接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + conn_panel = self.create_udp_connection_panel(new_conn) + self.udp_list_layout.addWidget(conn_panel) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 連接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 從列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 從 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" @@ -1593,6 +1687,14 @@ class ControlStationUI(QMainWindow): print(f"ROS spin error: {e}") def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + self.monitor.destroy_node() self.executor.shutdown() rclpy.shutdown() From c12959d964d99a474c52a1dd8eed572a0cbc0676 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 8 Jan 2026 13:00:00 +0800 Subject: [PATCH 089/146] =?UTF-8?q?(Sort=20Out)=201.=20=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=20fc=5Fnetwork=5Fadapter.md=20=E6=95=B4=E7=90=86=E4=B8=A6?= =?UTF-8?q?=E8=A8=98=E9=8C=84=E5=B0=88=E6=A1=88=E7=B5=90=E6=A7=8B=20?= =?UTF-8?q?=E7=A8=8B=E5=BC=8F=E5=8A=9F=E8=83=BD=E7=84=A1=E6=94=B9=E5=8B=95?= =?UTF-8?q?=20=E4=BF=AE=E6=AD=A3=E6=8E=92=E7=89=88=E8=88=87=E5=91=BD?= =?UTF-8?q?=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/fc_network_adapter.md | 166 ++ .../fc_network_adapter/mainOrchestrator.py | 1673 ++++++++--------- .../fc_network_adapter/mavlinkObject.py | 16 +- .../fc_network_adapter/serialManager.py | 8 +- 4 files changed, 1013 insertions(+), 850 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md new file mode 100644 index 0000000..98dba3a --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -0,0 +1,166 @@ + +這個檔案整理 此專案下 程式代碼的流程與思路 +只會挑出重要的變數與方法描述 +以利後續開發使用 + + + +# 檔案結構 + +特別注意: +1. 有標註 [async method] 都是不該被直接呼叫的內部方法 + +- *valuable* 這個是變數 **沒有括號** +- *method (parameters...)* 這個是方法 **有括號** + +## mainOrchestrator.py : 程式進入點 + +### **[Class]** Orchestrator + 最上層的發配資源與啟動終端機面板的調配者 +- *self.manager* 存放 async_io_manager 實例 +- *self.bridge* 存放 mavlink_bridge 實例 +- *self.plumber* 存放 serial_manager 實例 +- *self.vehicle_registry* 存放 vehicle_registry 實例 + +- *self.panel_thread* 面板的執行緒 +- *self.panelState* 暫存面板與調配者互動的資料流動區 + - 面板運行狀態 + - 面板操作結果 + - 其他模組的運行狀態 +--- +- *mainLoop()* 核心方法 + - 更新個模組狀態到 *self.panelState* + - 對應面板來的操作指令 +--- + 對於 async_io_manager 控制實現 +- *create_udp_object()* +- *delete_udp_object()* +- *add_target_to_object()* +- *remove_target_from_object()* +--- + 關於載具管理與檢視 +- *_update_vehicles_list()* +- *_prepare_vehicle_info* +--- + 關於 serial_manager 控制實現 +- *create_serial_port_object()* + + +### **[Class]** ControlPanel + 面板的核心運行物件 + 把自己的變數 獨立出來都放到 PanelState 去 +- *panel_thread()* 核心方法 + - 主選單的引入 + - 主選單下所有的按鍵操作 + - 定義所有人為操作後續面板執行緒行為 +- *menu_tree()* 基礎選單的定義檔 +--- + 關於 udp object 的操作 +- *create_object_list_menu()* object 選單的定義檔 +- *show_object_info()* 顯示 object 資訊 +- *select_target_socket()* object 對於轉拋功能的操作 +--- + 關於 serial 的操作 +- *create_serial_port_menu()* +- *create_linked_serial_menu()* +- *show_linked_serial_info()* +--- + 關於載具檢視與操作 +- *create_vehicles_list_menu()* +- *show_vehicle_info()* + +### **[Class]** PanelState + 作為面板執行緒(ControlPanel)與調配者(Orchestrator)溝通的管道 + 不包含具體實作方法 是 ControlPanel 的延伸 +- *self.panel_info_msg_list* 顯示在面板上的資訊訊息 + +## mavlinkObject.py + +### 全域變數 +- *stream_bridge_ring* +- *return_packet_ring* + +### **[Class]** mavlink_bridge + 唯一實例 + 實際去解析 mavlink 封包的地方 + 接收 stream_bridge_ring 與 return_packet_ring 的資料 +- *self.thread* 自己的執行緒 +--- +- *_run_thread()* 核心方法 +- *_handle_XXXXX()* 每一種單項 mavlink 封包的解析 + +### **[Class]** async_io_manager + 唯一實例 + 異步 event loop + 管理 mavlink_object 的地方 + 沒有核心方法 +- *self.thread* 自己的執行緒 +- *self.managed_objects* 資料結構 socket_id: mavlink_object +--- +- *add_mavlink_object(mavlink_object)* [call method] 把一個 mavlink_object 物件加入管理 +- *_async_add_mavlink_object(mavlink_object)* [async method] 對應上面的內部方法 不該直接使用 +- *remove_mavlink_object(socket_id)* [call method] 從管理區把指定 mavlink_object 移除 + +### **[Class]** mavlink_object + 儲存 mavlink socket + 處理 mavlink 封包分流的地方 +- *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) } +- *self.mavlink_socket* 從 pymavlink 繼承的socket物件 +- *self.state* 描述這個 socket 物件的狀態 +--- +- *process_data()* [async method] 核心方法 +- *remove_target_socket()* *add_target_socket()* +- *send_message()* + +## serialManager.py + 看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件 + +### **[Class]** serial_manager + 異步 event loop + 管理 mavlink_object 的地方 +- *self.thread* 自己的執行緒 +- *self.loop* 自己的事件迴圈 +- *self.serial_objects* 存放管理的物件 serial id num : serial_object +--- +- *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通 +- *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序 +- *remove_serial_link()* [call method] 關閉指定的 serial 端口 +- *_async_remove_serial_link()* [async method] + +### **[Class]** serial_object + 被塞在 serial_manager 裡面 + 只是一個變數物件 + 用來被儲存 serial 的資訊 +- *self.transport* +- *self.protocol* +- *self.udp_handler* UDP 端口物件 +- *self.serial_handler* Serial 端口物件 + +### **[Class]** UDPHandler + 處理 UDP 收發的端口 作為一個端口物件 + 作為 UDP OutBound 使用 所以不會佔用系統監聽資源 +- *self.transport* 自己的傳輸物件 +--- +- *datagram_received()* 先加碼成 Xbee 再呼叫 Serial 端口物件送出 + +### **[Class]** SerialHandler + 處理 Serial 收發的端口 作為一個端口物件 +- *self.transport* 自己的傳輸物件 +--- +- *data_received()* 先組合 Serial 封包 再解碼 再呼叫 UDP 端口物件送出 + +## mavlinkVehicleView.py + 這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用 + +# 開發記錄 + +## 已實現功能 +1. mavlink 分流解析 +2. mavlink socket 建立 +3. mavlink socket 轉拋 +4. 連結 Serial 轉 UDP +5. 各單元模組化 +6. 終端機介面控制 +7. 基礎載具流量觀測 + + diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index ed78fbd..f77d1ad 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -27,9 +27,8 @@ from .utils import RingBuffer, setup_logger from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port - - logger = setup_logger(os.path.basename(__file__)) +VERSION_NO = "v0.56" class PanelState: def __init__(self): @@ -84,8 +83,8 @@ class PanelState: def intoSTOPPED(self): self.panel_status = "Stopped" - def set_user_input(self, text): - self.user_input = text + # def set_user_input(self, text): + # self.user_input = text class MenuNode: def __init__(self, name, desc="", action=None, children=None): @@ -152,726 +151,213 @@ class ControlPanel: return user_input -# ================ 關於 mavlink object 的部份 =================== - - def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 mavlink_object 列表選單（支持分頁）""" - children = [] - - if not state.socket_object_list: - children.append(MenuNode("(Empty)", "目前沒有連結口", None)) - else: - total_items = len(state.socket_object_list) - total_pages = (total_items + items_per_page - 1) // items_per_page - start_idx = page * items_per_page - end_idx = min(start_idx + items_per_page, total_items) - - # 顯示當前頁的物件 - for socket_id in state.socket_object_list[start_idx:end_idx]: - # 為每個 socket 創建子選單 - obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ - MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), - MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), - MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), - MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), - MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), - MenuNode("GoUp", "回到列表", "BACK"), - ]) - # 將 socket_id 附加到每個子選單項目上 - for child in obj_menu.children: - child.socket_id = socket_id - children.append(obj_menu) + # ================ 關於 主要選單 的部份 =================== - # 添加分頁控制 - if total_pages > 1: - children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) - if page > 0: - prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") - prev_node.page = page - 1 - children.append(prev_node) - if page < total_pages - 1: - next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") - next_node.page = page + 1 - children.append(next_node) + def menu_tree(self): + """建立多層選單結構""" + return MenuNode("Main Menu", children=[ + MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[ + MenuNode("New+", children=[ + MenuNode("UDP InBound", children=[ + MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"), + ]), + MenuNode("UDP OutBound", children=[ + MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), + ]), + ]), + MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), + ]), + MenuNode("Serial Manager", "Serial 連接埠選項", children=[ + MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), + MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), + ]), + MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), + MenuNode("Engineer Mode", "工程模式", children=[ + MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), + MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), + MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), + ]), + MenuNode("Shutdown", "關閉整個系統", children=[ + MenuNode("Return", "繼續運行", "BACK"), + MenuNode("Confirm", "關閉系統", "QUIT"), + ]), + ]) - children.append(MenuNode("GoUp", "回到上層選單", "BACK")) - menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) - menu.current_page = page - return menu + def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event): + stdscr = None + + def cleanup(): + """清理 curses 狀態""" + if stdscr: + stdscr.keypad(False) + curses.nocbreak() + curses.echo() + curses.endwin() - def show_object_info(self, stdscr, socket_id, state: PanelState): - """顯示物件詳細資訊的對話框""" + def pre_panel_shutdown(): + # 先關閉所有模組 再關閉面板 + cmd_q.put("SHUTDOWN_BRIDGE") + cmd_q.put("SHUTDOWN_MANAGER") + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") - start = time.time() - while not state.socket_info_single.get('InfoReady', False): - # 太久沒有回應 - if time.time() - start > 2: - state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time())) - return - time.sleep(0.05) # 等待資訊準備好 + def draw_menu(screen): + nonlocal stdscr + stdscr = screen + + curses.curs_set(0) + stdscr.nodelay(False) # 阻塞讀鍵 + stdscr.keypad(True) - height, width = stdscr.getmaxyx() - dialog_height = 15 - dialog_width = min(70, width - 4) - start_y = (height - dialog_height) // 2 - start_x = (width - dialog_width) // 2 - - dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) - dialog_win.border() - dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) - - # 這裡顯示基本資訊 - dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") - dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}") - # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) - dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") - dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") - show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) - dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") - show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) - dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}") - dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}") - show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', ''))) - dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}") + # 選單導航狀態 + menu_stack = [self.menu_tree()] # 選單堆疊 + idx_stack = [0] # 索引堆疊 - state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用 - - dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") - dialog_win.refresh() - - dialog_win.getch() - del dialog_win - stdscr.clear() - stdscr.refresh() - - def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False): - """選擇目標 socket 的對話框""" - height, width = stdscr.getmaxyx() - dialog_height = min(15, len(state.socket_object_list) + 5) - dialog_width = min(50, width - 4) - start_y = (height - dialog_height) // 2 - start_x = (width - dialog_width) // 2 - - dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) - dialog_win.keypad(True) - - title = "選擇要移除的目標" if remove_mode else "選擇轉發目標" - available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id] - - if not available_sockets: - dialog_win.border() - dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) - dialog_win.addstr(2, 2, "沒有可用的目標") - dialog_win.addstr(4, 2, "按任意鍵返回...") - dialog_win.refresh() - dialog_win.getch() - del dialog_win - stdscr.clear() - stdscr.refresh() - return None - - selected_idx = 0 - - while True: - dialog_win.clear() - dialog_win.border() - dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) - - for i, socket_id in enumerate(available_sockets): - marker = "➤" if i == selected_idx else " " - attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL - dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr) - - dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消") - dialog_win.refresh() - - ch = dialog_win.getch() + state.intoSTART() # 設定狀態為運行中 - if ch in (curses.KEY_UP, ord('k')): - selected_idx = (selected_idx - 1) % len(available_sockets) - elif ch in (curses.KEY_DOWN, ord('j')): - selected_idx = (selected_idx + 1) % len(available_sockets) - elif ch in (curses.KEY_ENTER, 10, 13): - result = available_sockets[selected_idx] - del dialog_win - stdscr.clear() - stdscr.refresh() - return result - elif ch == 27: # ESC - del dialog_win + while not stop_evt.is_set(): + + current_menu = menu_stack[-1] + current_idx = idx_stack[-1] + + # 獲取終端機尺寸 + height, width = stdscr.getmaxyx() + # 簡單暴力的限制視窗的大小 + MIN_HEIGHT = ( + 2 + # 邊界 + 6 + # 狀態列 操作說明列 一個空白 + 11+ # 最大選單 與 空白區 + 5 # 訊息區域 + ) + if height < MIN_HEIGHT or width < 60: + logger.error("Terminal size too small for Control Panel.") + break + stdscr.clear() - stdscr.refresh() - return None + stdscr.border() -# ================ 關於 serial link 的部份 =================== + # 更新模組狀態顯示 + stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") + stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") + stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") - def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 serial port 列表選單（支持分頁）""" - children = [] - - # 獲取可用的 Serial 連接埠列表 - # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 - serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) - - if not serial_ports: - children.append(MenuNode("(Empty)", "目前沒有串口設備", None)) - else: - total_items = len(serial_ports) - total_pages = (total_items + items_per_page - 1) // items_per_page - start_idx = page * items_per_page - end_idx = min(start_idx + items_per_page, total_items) - - # 顯示當前頁的串口 - for port in serial_ports[start_idx:end_idx]: - port_menu = MenuNode(f"{port}", children=[ - MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ - MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), - # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), - ]), - MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), - MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ - MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), - MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), - ]), - MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), - MenuNode("GoUp", "回到列表", "BACK"), - ]) - # 將 port 附加到每個子選單項目上 - for child in port_menu.children: - child.port = port - children.append(port_menu) - - # 添加分頁控制 - if total_pages > 1: - children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) - if page > 0: - prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") - prev_node.page = page - 1 - children.append(prev_node) - if page < total_pages - 1: - next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") - next_node.page = page + 1 - children.append(next_node) - - children.append(MenuNode("GoUp", "回到上層選單", "BACK")) - menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) - menu.current_page = page - return menu + # 顯示當前選單項目 + start_line = 6 + for i, child in enumerate(current_menu.children): + marker = "➤ " if i == current_idx else " " + # 動態顯示已輸入的值 + desc = child.desc + if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]: + desc = f"{child.desc} [{state.udp_info_temp['IP']}]" + elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: + desc = f"{child.desc} [{state.udp_info_temp['Port']}]" + elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]: + desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" + elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: + desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" + elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP": + link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No" + desc = f"{child.desc} [{link_status}]" + + line = f"{marker}{child.name:15s} – {desc}" + attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL + stdscr.addstr(start_line + i, 4, line, attr) - def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能""" - children = [] - - if not state.linked_serial_dict: - children.append(MenuNode("(Empty)", "目前沒有連結口", None)) - else: - total_items = len(state.linked_serial_dict) - total_pages = (total_items + items_per_page - 1) // items_per_page - start_idx = page * items_per_page - end_idx = min(start_idx + items_per_page, total_items) - - # 顯示當前頁的物件 - linked_serial_id_list = list(state.linked_serial_dict.keys()) - for serial_id in linked_serial_id_list[start_idx:end_idx]: - # 為每個 socket 創建子選單 - obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[ - MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), - MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), - # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), - MenuNode("GoUp", "回到列表", "BACK"), - ]) - # 將 serial_id 附加到每個子選單項目上 - for child in obj_menu.children: - child.serial_id = serial_id - children.append(obj_menu) - - # 添加分頁控制 - if total_pages > 1: - children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) - if page > 0: - prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") - prev_node.page = page - 1 - children.append(prev_node) - if page < total_pages - 1: - next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") - next_node.page = page + 1 - children.append(next_node) - - children.append(MenuNode("GoUp", "回到上層選單", "BACK")) - menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) - menu.current_page = page - return menu + # 顯示訊息區域 + # info_start_line = start_line + len(current_menu.children) + 1 + info_start_line = height - 8 + max_msg_lines = 5 # 最多顯示 5 行訊息 + current_time = time.time() + + # 清理過時的訊息 + state.panel_info_msg_list = [ + (msg, timestamp) for msg, timestamp in state.panel_info_msg_list + if current_time - timestamp < 2.0 #秒數 + ] + + # 只顯示最新的 max_msg_lines 條訊息 + display_msgs = state.panel_info_msg_list[-max_msg_lines:] + + for i, msg_data in enumerate(display_msgs): + if info_start_line + i >= help_line - 1: # 避免超出邊界 + break + msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data + # 截斷過長的訊息 + max_msg_width = width - 6 + if len(msg) > max_msg_width: + msg = msg[:max_msg_width-3] + "..." - def show_linked_serial_info(self, stdscr, serial_id, state: PanelState): - """顯示 Serial 連結詳細資訊的對話框""" + stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD) - start = time.time() - while not state.serial_info_single.get('InfoReady', False): - # 太久沒有回應 - if time.time() - start > 2: - state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time())) - return - time.sleep(0.05) # 等待資訊準備好 + + + # 操作說明 + # help_line = start_line + len(current_menu.children) + 2 + help_line = height - 2 + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM) + stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM) - height, width = stdscr.getmaxyx() - dialog_height = 15 - dialog_width = min(70, width - 4) - start_y = (height - dialog_height) // 2 - start_x = (width - dialog_width) // 2 - - dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) - dialog_win.border() - dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD) + stdscr.refresh() - # 從 linked_serial_dict 獲取資訊 - serial_info = state.linked_serial_dict.get(serial_id, {}) - - if not serial_info: - dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊") - else: - # 顯示基本資訊 - dialog_win.addstr(2, 2, f"Serial ID : {serial_id}") - dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}") - dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}") - dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}") - dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}") - dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}") - - # 如果有額外資訊可以繼續添加 - if 'thread_alive' in serial_info: - thread_status = "Alive" if serial_info['thread_alive'] else "Stopped" - dialog_win.addstr(8, 2, f"Thread Status : {thread_status}") - - state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用 + # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 + # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈 + # 超過幾秒沒有反應就強制關閉 + if state.panel_status == "Terminating": + if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點 + logger.warning("Control Panel forced shutdown after timeout.") + state.intoSTOPPED() + # stop_evt.set() + # continue + break + time.sleep(0.1) + if (state.mavlink_bridge_state == "Stopped" and + state.object_manager_state == "Stopped" and + state.serial_manager_state == "Stopped"): + state.intoSTOPPED() + # stop_evt.set() + break + continue - dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") - dialog_win.refresh() - - dialog_win.getch() - del dialog_win - stdscr.clear() - stdscr.refresh() + # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt + stdscr.timeout(100) + ch = stdscr.getch() + + if ch == -1: # 沒有操作 + continue + + # 處理按鍵 + if ch in (curses.KEY_UP, ord('k')): + idx_stack[-1] = (current_idx - 1) % len(current_menu.children) + + elif ch in (curses.KEY_DOWN, ord('j')): + idx_stack[-1] = (current_idx + 1) % len(current_menu.children) -# ================ 關於載具檢視的部份 =================== + elif ch == (ord('O')): + # 進入工程模式 + state.intoENGINEER() - def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 已連線載具 列表選單（支持分頁） - 每個 vehicle-component 組合都是獨立的選單項目 - """ - children = [] - - if not state.connected_vehicles_dict: - children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None)) - else: - total_items = len(state.connected_vehicles_dict) - total_pages = (total_items + items_per_page - 1) // items_per_page - start_idx = page * items_per_page - end_idx = min(start_idx + items_per_page, total_items) - - # vehicle_id_list 現在是 (sysid, compid) 的元組列表 - vehicle_comp_list = list(state.connected_vehicles_dict.keys()) - - # 顯示當前頁的物件 - for sysid, compid in vehicle_comp_list[start_idx:end_idx]: - # 建立顯示名稱 - display_name = f"Vehicle #{sysid} - Comp #{compid}" - desc = f"載具 {sysid} 組件 {compid}" + elif ch == (ord('o')): + # 離開工程模式 + state.intoSTART() + + elif ch == curses.KEY_LEFT: + # 返回上層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() - vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") - # 將 sysid 和 compid 附加到選單項目上 - vehicle_menu.sysid = sysid - vehicle_menu.compid = compid - children.append(vehicle_menu) - - # 添加分頁控制 - if total_pages > 1: - children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) - if page > 0: - prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") - prev_node.page = page - 1 - children.append(prev_node) - if page < total_pages - 1: - next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") - next_node.page = page + 1 - children.append(next_node) - - children.append(MenuNode("GoUp", "回到上層選單", "BACK")) - menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children) - menu.current_page = page - return menu - - def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): - """顯示載具組件詳細資訊（動態更新，顯示變化率）""" - - # 等待資訊準備 - start = time.time() - while not state.vehicle_info_single.get('InfoReady', False): - if time.time() - start > 2: - state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) - return - time.sleep(0.05) - - info = state.vehicle_info_single - height, width = stdscr.getmaxyx() - dialog_height = min(22, height - 4) - dialog_width = min(70, width - 4) - start_y = (height - dialog_height) // 2 - start_x = (width - dialog_width) // 2 - - dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) - dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 - dialog_win.keypad(True) - - # MAV_TYPE 名稱對應 - MAV_TYPE_NAMES = { - 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", - 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", - 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" - } - - # 動態更新迴圈 - last_update = time.time() - while True: - current_time = time.time() - - # 每 1 秒重新請求資料 - if current_time - last_update >= 1.0: - # 觸發資料更新（透過 Orchestrator） - cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) - # 等待新資料準備好 - wait_start = time.time() - state.vehicle_info_single['InfoReady'] = False - while not state.vehicle_info_single.get('InfoReady', False): - if time.time() - wait_start > 0.5: # 最多等 0.5 秒 - break - time.sleep(0.01) - # 更新 info 參照 - info = state.vehicle_info_single - last_update = current_time - - dialog_win.clear() - dialog_win.border() - dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) - - # === 基礎資訊 === - dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) - dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) - - # # MAV Type # 這個用不到 - # mav_type = info.get('vehicle_type', 'N/A') - # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) - # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") - - # Component Type - dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") - - # Autopilot Type - if info.get('mav_autopilot') is not None: - dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") - - # Connection Info - dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") - dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") - - # === 封包統計 === - stats = info.get('packet_stats', {}) - dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) - - received = stats.get('received', 0) - lost = stats.get('lost', 0) - loss_rate = stats.get('loss_rate', 0.0) - last_seq = stats.get('last_seq', 'N/A') - - # 計算變化 - received_delta = stats.get('received_delta', 0) - lost_delta = stats.get('lost_delta', 0) - - # 顯示變化率 - recv_str = f"{received:6d}" - if received_delta > 0: - recv_str += f" (+{received_delta}↑)" - - lost_str = f"{lost:4d}" - if lost_delta > 0: - lost_str += f" (+{lost_delta}↑)" - - dialog_win.addstr(8, 2, f"Received : {recv_str}") - dialog_win.addstr(8, 32, f"Lost : {lost_str}") - dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") - dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") - - # 最後接收時間 - last_msg_time = stats.get('last_msg_time') - if last_msg_time: - time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) - elapsed = current_time - last_msg_time - dialog_win.addstr(10, 2, f"Last Time : {time_str}") - dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") - else: - dialog_win.addstr(10, 2, "Last Time : N/A") - - # === 訊息類型分佈 === - dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) - - msg_counts = info.get('msg_type_counts', {}) - - # MAVLink 訊息名稱對應（縮寫版本） - MSG_NAMES = { - 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", - 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", - 74: "VFR_HUD", 147: "BATT_ST" - } - - # 顯示前 12 個最常見的訊息類型（兩列各 6 個） - msg_items = list(msg_counts.items())[:12] - line = 13 - for i, (msg_id, count) in enumerate(msg_items): - msg_name = MSG_NAMES.get(msg_id, "???") - delta = stats.get(f'msg_delta_{msg_id}', 0) - - # 格式化數據 - if delta > 0: - data_str = f"{count}(+{delta}↑)" - else: - data_str = f"{count}" - - # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) - display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" - - # 左列(偶數索引)或右列(奇數索引) - col = 2 if i % 2 == 0 else 36 - row = line + (i // 2) - - if row >= dialog_height - 3: # 避免超出邊界 - break - - dialog_win.addstr(row, col, display_str) - - # 確保跳過顯示區域 - line = line + 6 - - dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") - dialog_win.refresh() - - # 檢查是否有按鍵（非阻塞） - ch = dialog_win.getch() - if ch != -1: # 有按鍵則退出 - break - - # 短暫延遲 - time.sleep(0.1) - - state.vehicle_info_single['InfoReady'] = False - del dialog_win - stdscr.clear() - stdscr.refresh() - -# ================ 關於 主要選單 的部份 =================== - - def menu_tree(self): - """建立多層選單結構""" - return MenuNode("Main Menu", children=[ - MenuNode("MavLink Object", "MavLink 通道選項", children=[ - MenuNode("New+", children=[ - MenuNode("UDP InBound", children=[ - MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), - MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"), - MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"), - ]), - MenuNode("UDP OutBound", children=[ - MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"), - MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), - MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), - ]), - ]), - MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), - ]), - MenuNode("Serial Manager", "Serial 連接埠選項", children=[ - MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), - MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), - ]), - MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), - MenuNode("Engineer Mode", "工程模式", children=[ - MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), - MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), - MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), - ]), - MenuNode("Shutdown", "關閉整個系統", children=[ - MenuNode("Return", "繼續運行", "BACK"), - MenuNode("Confirm", "關閉系統", "QUIT"), - ]), - ]) - - def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event): - stdscr = None - - def cleanup(): - """清理 curses 狀態""" - if stdscr: - stdscr.keypad(False) - curses.nocbreak() - curses.echo() - curses.endwin() - - def pre_panel_shutdown(): - # 先關閉所有模組 再關閉面板 - cmd_q.put("SHUTDOWN_BRIDGE") - cmd_q.put("SHUTDOWN_MANAGER") - cmd_q.put("SHUTDOWN_SERIAL_MANAGER") - - def draw_menu(screen): - nonlocal stdscr - stdscr = screen - - curses.curs_set(0) - stdscr.nodelay(False) # 阻塞讀鍵 - stdscr.keypad(True) - - # 選單導航狀態 - menu_stack = [self.menu_tree()] # 選單堆疊 - idx_stack = [0] # 索引堆疊 - - state.intoSTART() # 設定狀態為運行中 - - while not stop_evt.is_set(): - - current_menu = menu_stack[-1] - current_idx = idx_stack[-1] - - # 獲取終端機尺寸 - height, width = stdscr.getmaxyx() - # 簡單暴力的限制視窗的大小 - MIN_HEIGHT = ( - 2 + # 邊界 - 6 + # 狀態列 操作說明列 一個空白 - 11+ # 最大選單 與 空白區 - 5 # 訊息區域 - ) - if height < MIN_HEIGHT or width < 60: - logger.error("Terminal size too small for Control Panel.") - break - - stdscr.clear() - stdscr.border() - - # 更新模組狀態顯示 - stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) - stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") - stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") - stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") - stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") - stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") - - # 顯示當前選單項目 - start_line = 6 - for i, child in enumerate(current_menu.children): - marker = "➤ " if i == current_idx else " " - # 動態顯示已輸入的值 - desc = child.desc - if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]: - desc = f"{child.desc} [{state.udp_info_temp['IP']}]" - elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: - desc = f"{child.desc} [{state.udp_info_temp['Port']}]" - elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]: - desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" - elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: - desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" - elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP": - link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No" - desc = f"{child.desc} [{link_status}]" - - line = f"{marker}{child.name:15s} – {desc}" - attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL - stdscr.addstr(start_line + i, 4, line, attr) - - # 顯示訊息區域 - # info_start_line = start_line + len(current_menu.children) + 1 - info_start_line = height - 8 - max_msg_lines = 5 # 最多顯示 5 行訊息 - current_time = time.time() - - # 清理過時的訊息 - state.panel_info_msg_list = [ - (msg, timestamp) for msg, timestamp in state.panel_info_msg_list - if current_time - timestamp < 2.0 #秒數 - ] - - # 只顯示最新的 max_msg_lines 條訊息 - display_msgs = state.panel_info_msg_list[-max_msg_lines:] - - for i, msg_data in enumerate(display_msgs): - if info_start_line + i >= help_line - 1: # 避免超出邊界 - break - msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data - # 截斷過長的訊息 - max_msg_width = width - 6 - if len(msg) > max_msg_width: - msg = msg[:max_msg_width-3] + "..." - - stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD) - - - - # 操作說明 - # help_line = start_line + len(current_menu.children) + 2 - help_line = height - 2 - stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM) - - stdscr.refresh() - - # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 - # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈 - # 超過幾秒沒有反應就強制關閉 - if state.panel_status == "Terminating": - if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點 - logger.warning("Control Panel forced shutdown after timeout.") - state.intoSTOPPED() - # stop_evt.set() - # continue - break - time.sleep(0.1) - if (state.mavlink_bridge_state == "Stopped" and - state.object_manager_state == "Stopped" and - state.serial_manager_state == "Stopped"): - state.intoSTOPPED() - # stop_evt.set() - break - continue - - # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt - stdscr.timeout(100) - ch = stdscr.getch() - - if ch == -1: # 沒有操作 - continue - - # 處理按鍵 - if ch in (curses.KEY_UP, ord('k')): - idx_stack[-1] = (current_idx - 1) % len(current_menu.children) - - elif ch in (curses.KEY_DOWN, ord('j')): - idx_stack[-1] = (current_idx + 1) % len(current_menu.children) - - elif ch == (ord('O')): - # 進入工程模式 - state.intoENGINEER() - - elif ch == (ord('o')): - # 離開工程模式 - state.intoSTART() - - elif ch == curses.KEY_LEFT: - # 返回上層 - if len(menu_stack) > 1: - menu_stack.pop() - idx_stack.pop() - - elif ch == curses.KEY_RIGHT: - # 進入下層 (但不執行動作) - selected = current_menu.children[current_idx] - if selected.children: # 有子選單 - menu_stack.append(selected) - idx_stack.append(0) + elif ch == curses.KEY_RIGHT: + # 進入下層 (但不執行動作) + selected = current_menu.children[current_idx] + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) elif ch in (ord('q'), 27): if state.panel_status == "Engineer": @@ -942,7 +428,6 @@ class ControlPanel: idx_stack.pop() elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES": - logger.info("mark A") state.serial_info_temp["Go2Middleware"] = True menu_stack.pop() idx_stack.pop() @@ -966,144 +451,658 @@ class ControlPanel: menu_stack.append(created_list_menu) idx_stack.append(0) - elif selected.action == "LIST_SERIAL_LINKS": - created_list_menu = self.create_linked_serial_menu(state, page=0) - menu_stack.append(created_list_menu) - idx_stack.append(0) - - elif selected.action == "INSPECT_LINKED_SERIAL": - # 顯示 Serial 連結詳細資訊 - if hasattr(selected, 'serial_id'): - cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id)) - self.show_linked_serial_info(stdscr, selected.serial_id, state) + elif selected.action == "LIST_SERIAL_LINKS": + created_list_menu = self.create_linked_serial_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_LINKED_SERIAL": + # 顯示 Serial 連結詳細資訊 + if hasattr(selected, 'serial_id'): + cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id)) + self.show_linked_serial_info(stdscr, selected.serial_id, state) + + elif selected.action == "REMOVE_LINKED_SERIAL": + # 移除 Serial 連結 + if hasattr(selected, 'serial_id'): + cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 一樣退兩層 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_MAV_OBJECT": + # 動態生成 mavlink_object 列表選單 + created_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): + if hasattr(selected, 'page'): + current_list_menu = menu_stack[-1] + menu_stack.pop() + idx_stack.pop() + + # 依據選單種類 重新建立分頁 + if current_list_menu.name == "Serial Port List": + created_list_menu = self.create_serial_port_menu(state, page=selected.page) + elif current_list_menu.name == "Object List": + created_list_menu = self.create_object_list_menu(state, page=selected.page) + elif current_list_menu.name == "Linked Serial List": + created_list_menu = self.create_linked_serial_menu(state, page=selected.page) + elif current_list_menu.name == "Connected Vehicles": + created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) + else: + # 不支援的選單類型，回到原本的選單 + menu_stack.append(current_list_menu) + idx_stack.append(0) + continue + + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_MAV_OBJECT": + # 顯示物件詳細資訊 + if hasattr(selected, 'socket_id'): + cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id)) + self.show_object_info(stdscr, selected.socket_id, state) + + elif selected.action == "REMOVE_MAV_OBJECT": + # 移除物件 + if hasattr(selected, 'socket_id'): + cmd_q.put(("REMOVE_OBJECT", selected.socket_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "MAVOBJ_MAKE_LINK": + # 建立轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 + + elif selected.action == "MAVOBJ_CANCEL_LINK": + # 取消轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) + if target_id is not None: + cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結 + + elif selected.action == "MAVOBJ_ADD_TARGET": + # 添加目標端口 + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + + elif selected.action == "STOP_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_MANAGER") + + elif selected.action == "STOP_BRIDGE": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_BRIDGE") + + elif selected.action == "STOP_SERIAL_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + elif selected.action == "INSPECT_VEHICLES": + # 進入載具檢視選單 + cmd_q.put("UPDATE_VEHICLES_LIST") + created_list_menu = self.create_vehicles_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_VEHICLE": + # 顯示載具詳細資訊 + if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): + cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) + self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) + + elif callable(selected.action): + # 執行函式 + cmd_q.put(selected.action) + + try: + curses.wrapper(draw_menu) + except KeyboardInterrupt: + pass + finally: + cleanup() + + # ================ 關於 mavlink object 的部份 =================== + + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 mavlink_object 列表選單（支持分頁）""" + children = [] + + if not state.socket_object_list: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.socket_object_list) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + for socket_id in state.socket_object_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), + MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), + MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), + MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), + MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 socket_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.socket_id = socket_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_object_info(self, stdscr, socket_id, state: PanelState): + """顯示物件詳細資訊的對話框""" + + start = time.time() + while not state.socket_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) + + # 這裡顯示基本資訊 + dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") + dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}") + # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) + dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") + dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") + show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) + dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") + show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) + dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}") + dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}") + show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', ''))) + dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}") + + state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False): + """選擇目標 socket 的對話框""" + height, width = stdscr.getmaxyx() + dialog_height = min(15, len(state.socket_object_list) + 5) + dialog_width = min(50, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.keypad(True) + + title = "選擇要移除的目標" if remove_mode else "選擇轉發目標" + available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id] + + if not available_sockets: + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + dialog_win.addstr(2, 2, "沒有可用的目標") + dialog_win.addstr(4, 2, "按任意鍵返回...") + dialog_win.refresh() + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + selected_idx = 0 + + while True: + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + + for i, socket_id in enumerate(available_sockets): + marker = "➤" if i == selected_idx else " " + attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL + dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr) + + dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消") + dialog_win.refresh() + + ch = dialog_win.getch() + + if ch in (curses.KEY_UP, ord('k')): + selected_idx = (selected_idx - 1) % len(available_sockets) + elif ch in (curses.KEY_DOWN, ord('j')): + selected_idx = (selected_idx + 1) % len(available_sockets) + elif ch in (curses.KEY_ENTER, 10, 13): + result = available_sockets[selected_idx] + del dialog_win + stdscr.clear() + stdscr.refresh() + return result + elif ch == 27: # ESC + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + # ================ 關於 serial link 的部份 =================== - elif selected.action == "REMOVE_LINKED_SERIAL": - # 移除 Serial 連結 - if hasattr(selected, 'serial_id'): - cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id)) - # 返回上層（回到列表） - if len(menu_stack) > 1: - menu_stack.pop() - idx_stack.pop() - # 一樣退兩層 - menu_stack.pop() - idx_stack.pop() + def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 serial port 列表選單（支持分頁）""" + children = [] + + # 獲取可用的 Serial 連接埠列表 + # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 + serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + + if not serial_ports: + children.append(MenuNode("(Empty)", "目前沒有串口設備", None)) + else: + total_items = len(serial_ports) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的串口 + for port in serial_ports[start_idx:end_idx]: + port_menu = MenuNode(f"{port}", children=[ + MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ + MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), + # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + ]), + MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ + MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), + MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), + ]), + MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 port 附加到每個子選單項目上 + for child in port_menu.children: + child.port = port + children.append(port_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu - elif selected.action == "LIST_MAV_OBJECT": - # 動態生成 mavlink_object 列表選單 - created_list_menu = self.create_object_list_menu(state, page=0) - menu_stack.append(created_list_menu) - idx_stack.append(0) + def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能""" + children = [] + + if not state.linked_serial_dict: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.linked_serial_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + linked_serial_id_list = list(state.linked_serial_dict.keys()) + for serial_id in linked_serial_id_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), + MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), + # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 serial_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.serial_id = serial_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu - elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): - if hasattr(selected, 'page'): - current_list_menu = menu_stack[-1] - menu_stack.pop() - idx_stack.pop() - - # 依據選單種類 重新建立分頁 - if current_list_menu.name == "Serial Port List": - created_list_menu = self.create_serial_port_menu(state, page=selected.page) - elif current_list_menu.name == "Object List": - created_list_menu = self.create_object_list_menu(state, page=selected.page) - elif current_list_menu.name == "Linked Serial List": - created_list_menu = self.create_linked_serial_menu(state, page=selected.page) - elif current_list_menu.name == "Connected Vehicles": - created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) - else: - # 不支援的選單類型，回到原本的選單 - menu_stack.append(current_list_menu) - idx_stack.append(0) - continue - - menu_stack.append(created_list_menu) - idx_stack.append(0) - - elif selected.action == "INSPECT_MAV_OBJECT": - # 顯示物件詳細資訊 - if hasattr(selected, 'socket_id'): - cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id)) - self.show_object_info(stdscr, selected.socket_id, state) - - elif selected.action == "REMOVE_MAV_OBJECT": - # 移除物件 - if hasattr(selected, 'socket_id'): - cmd_q.put(("REMOVE_OBJECT", selected.socket_id)) - # 返回上層（回到列表） - if len(menu_stack) > 1: - menu_stack.pop() - idx_stack.pop() - # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 - menu_stack.pop() - idx_stack.pop() + def show_linked_serial_info(self, stdscr, serial_id, state: PanelState): + """顯示 Serial 連結詳細資訊的對話框""" - elif selected.action == "MAVOBJ_MAKE_LINK": - # 建立轉發連結 - if hasattr(selected, 'socket_id'): - target_id = self.select_target_socket(stdscr, selected.socket_id, state) - if target_id is not None: - cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) - cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 + start = time.time() + while not state.serial_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 - elif selected.action == "MAVOBJ_CANCEL_LINK": - # 取消轉發連結 - if hasattr(selected, 'socket_id'): - target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) - if target_id is not None: - cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) - cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結 + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD) - elif selected.action == "MAVOBJ_ADD_TARGET": - # 添加目標端口 - if state.panel_status != "Engineer": - state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) - continue # 只有在工程模式下才能操作 - if hasattr(selected, 'socket_id'): - target_id = self.select_target_socket(stdscr, selected.socket_id, state) - if target_id is not None: - cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + # 從 linked_serial_dict 獲取資訊 + serial_info = state.linked_serial_dict.get(serial_id, {}) + + if not serial_info: + dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊") + else: + # 顯示基本資訊 + dialog_win.addstr(2, 2, f"Serial ID : {serial_id}") + dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}") + dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}") + dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}") + dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}") + dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}") + + # 如果有額外資訊可以繼續添加 + if 'thread_alive' in serial_info: + thread_status = "Alive" if serial_info['thread_alive'] else "Stopped" + dialog_win.addstr(8, 2, f"Thread Status : {thread_status}") + + state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用 - elif selected.action == "STOP_MANAGER": - if state.panel_status != "Engineer": - state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) - continue # 只有在工程模式下才能操作 - cmd_q.put("SHUTDOWN_MANAGER") + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() - elif selected.action == "STOP_BRIDGE": - if state.panel_status != "Engineer": - state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) - continue # 只有在工程模式下才能操作 - cmd_q.put("SHUTDOWN_BRIDGE") + # ================ 關於載具檢視的部份 =================== - elif selected.action == "STOP_SERIAL_MANAGER": - if state.panel_status != "Engineer": - state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) - continue # 只有在工程模式下才能操作 - cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線載具 列表選單（支持分頁） + 每個 vehicle-component 組合都是獨立的選單項目 + """ + children = [] + + if not state.connected_vehicles_dict: + children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None)) + else: + total_items = len(state.connected_vehicles_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # vehicle_id_list 現在是 (sysid, compid) 的元組列表 + vehicle_comp_list = list(state.connected_vehicles_dict.keys()) + + # 顯示當前頁的物件 + for sysid, compid in vehicle_comp_list[start_idx:end_idx]: + # 建立顯示名稱 + display_name = f"Vehicle #{sysid} - Comp #{compid}" + desc = f"載具 {sysid} 組件 {compid}" + + vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") + # 將 sysid 和 compid 附加到選單項目上 + vehicle_menu.sysid = sysid + vehicle_menu.compid = compid + children.append(vehicle_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu - elif selected.action == "INSPECT_VEHICLES": - # 進入載具檢視選單 - cmd_q.put("UPDATE_VEHICLES_LIST") - created_list_menu = self.create_vehicles_list_menu(state, page=0) - menu_stack.append(created_list_menu) - idx_stack.append(0) + def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): + """顯示載具組件詳細資訊（動態更新，顯示變化率）""" + + # 等待資訊準備 + start = time.time() + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) + return + time.sleep(0.05) + + info = state.vehicle_info_single + height, width = stdscr.getmaxyx() + dialog_height = min(22, height - 4) + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 + dialog_win.keypad(True) + + # MAV_TYPE 名稱對應 + MAV_TYPE_NAMES = { + 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", + 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", + 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" + } + + # 動態更新迴圈 + last_update = time.time() + while True: + current_time = time.time() + + # 每 1 秒重新請求資料 + if current_time - last_update >= 1.0: + # 觸發資料更新（透過 Orchestrator） + cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) + # 等待新資料準備好 + wait_start = time.time() + state.vehicle_info_single['InfoReady'] = False + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - wait_start > 0.5: # 最多等 0.5 秒 + break + time.sleep(0.01) + # 更新 info 參照 + info = state.vehicle_info_single + last_update = current_time + + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) + + # === 基礎資訊 === + dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) + dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) + + # # MAV Type # 這個用不到 + # mav_type = info.get('vehicle_type', 'N/A') + # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) + # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") + + # Component Type + dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") + + # Autopilot Type + if info.get('mav_autopilot') is not None: + dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") + + # Connection Info + dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") + dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") + + # === 封包統計 === + stats = info.get('packet_stats', {}) + dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) + + received = stats.get('received', 0) + lost = stats.get('lost', 0) + loss_rate = stats.get('loss_rate', 0.0) + last_seq = stats.get('last_seq', 'N/A') + + # 計算變化 + received_delta = stats.get('received_delta', 0) + lost_delta = stats.get('lost_delta', 0) + + # 顯示變化率 + recv_str = f"{received:6d}" + if received_delta > 0: + recv_str += f" (+{received_delta}↑)" + + lost_str = f"{lost:4d}" + if lost_delta > 0: + lost_str += f" (+{lost_delta}↑)" + + dialog_win.addstr(8, 2, f"Received : {recv_str}") + dialog_win.addstr(8, 32, f"Lost : {lost_str}") + dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") + dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") + + # 最後接收時間 + last_msg_time = stats.get('last_msg_time') + if last_msg_time: + time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) + elapsed = current_time - last_msg_time + dialog_win.addstr(10, 2, f"Last Time : {time_str}") + dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") + else: + dialog_win.addstr(10, 2, "Last Time : N/A") + + # === 訊息類型分佈 === + dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) + + msg_counts = info.get('msg_type_counts', {}) + + # MAVLink 訊息名稱對應（縮寫版本） + MSG_NAMES = { + 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", + 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", + 74: "VFR_HUD", 147: "BATT_ST" + } + + # 顯示前 12 個最常見的訊息類型（兩列各 6 個） + msg_items = list(msg_counts.items())[:12] + line = 13 + for i, (msg_id, count) in enumerate(msg_items): + msg_name = MSG_NAMES.get(msg_id, "???") + delta = stats.get(f'msg_delta_{msg_id}', 0) + + # 格式化數據 + if delta > 0: + data_str = f"{count}(+{delta}↑)" + else: + data_str = f"{count}" + + # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) + display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" + + # 左列(偶數索引)或右列(奇數索引) + col = 2 if i % 2 == 0 else 36 + row = line + (i // 2) + + if row >= dialog_height - 3: # 避免超出邊界 + break - elif selected.action == "INSPECT_VEHICLE": - # 顯示載具詳細資訊 - if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): - cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) - self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) - - elif callable(selected.action): - # 執行函式 - cmd_q.put(selected.action) - - try: - curses.wrapper(draw_menu) - except KeyboardInterrupt: - pass - finally: - cleanup() + dialog_win.addstr(row, col, display_str) + + # 確保跳過顯示區域 + line = line + 6 + + dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") + dialog_win.refresh() + + # 檢查是否有按鍵（非阻塞） + ch = dialog_win.getch() + if ch != -1: # 有按鍵則退出 + break + + # 短暫延遲 + time.sleep(0.1) + + state.vehicle_info_single['InfoReady'] = False + del dialog_win + stdscr.clear() + stdscr.refresh() @@ -1189,7 +1188,7 @@ class Orchestrator: if socket_id in s_obj.target_sockets: self.remove_target_from_object(s_id, socket_id) # 再移除該物件 - self.delete_mavlink_object(socket_id) + self.delete_udp_object(socket_id) elif action == "MAVOBJ_ADD_TARGET": source_id, target_id = cmd[1], cmd[2] self.add_target_to_object(source_id, target_id) @@ -1206,9 +1205,7 @@ class Orchestrator: self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets - ip_info = mav_obj.mavlink_socket.port.getsockname() - self.panelState.socket_info_single["socket_connection_string"] = f"{ip_info[0]}:{ip_info[1]}" - # getattr(mav_obj.mavlink_socket, "connection_string", "") + self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 elif action == "INSPECT_LINKED_SERIAL": serial_id = cmd[1] @@ -1322,7 +1319,7 @@ class Orchestrator: self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) - def delete_mavlink_object(self, socket_id): + def delete_udp_object(self, socket_id): """移除指定的 mavlink_object""" self.manager.remove_mavlink_object(socket_id) diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 71760a0..7b321f1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -663,16 +663,16 @@ class async_io_manager: self.running = False logger.info("async_io_manager event loop END!") - async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管 - """主任務協程 讓 async_io_manager 在執行緒中持續運作""" - logger.info("async_io_manager main task started") + # async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管 + # """主任務協程 讓 async_io_manager 在執行緒中持續運作""" + # logger.info("async_io_manager main task started") - while self.running and not self._stop_event.is_set(): - await asyncio.sleep(0.1) + # while self.running and not self._stop_event.is_set(): + # await asyncio.sleep(0.1) - logger.info("async_io_manager main task ended") + # logger.info("async_io_manager main task ended") - def add_mavlink_object(self, mavlink_obj): + def add_mavlink_object(self, mavlink_obj: mavlink_object): """添加 mavlink_object""" # 一個防呆 確保有 event loop 與 _main_task 正在運作 if not self.running or not self.loop: @@ -718,7 +718,7 @@ class async_io_manager: logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}") return False - def remove_mavlink_object(self, socket_id): + def remove_mavlink_object(self, socket_id: int): """移除 mavlink_object""" # 一個防呆 確保有 event loop 正在運作 diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 02588e1..731a950 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -187,11 +187,11 @@ class ATCommandHandler: # print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") pass - +# ====================== 分割線 ===================== class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 def __init__(self, udp_handler, serial_port_str): - self.udp_handler = udp_handler # UDP 的傳輸把手 + self.udp_handler = udp_handler # UDP 的傳輸物件 self.serial_port_str = serial_port_str self.at_handler = ATCommandHandler(serial_port_str) @@ -327,8 +327,8 @@ class serial_manager: self.receiver_type = receiver_type self.target_port = target_port # 指向的 UPD 端口 - self.transport = None - self.protocol = None + self.transport = None # TODO 這個變數可能沒有作用 + self.protocol = None # TODO 這個變數可能沒有作用 self.udp_handler = None self.serial_handler = None From 00eb6b512d052f849035b2868e842583c1bd9ece Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 8 Jan 2026 17:14:01 +0800 Subject: [PATCH 090/146] Upload files to 'src/unitdev01/unitdev01' --- src/unitdev01/unitdev01/comm_panel.py | 398 +++++++++ src/unitdev01/unitdev01/communication.py | 137 ++- src/unitdev01/unitdev01/drone_panel.py | 378 ++++++++ src/unitdev01/unitdev01/gui.py | 1043 ++-------------------- src/unitdev01/unitdev01/map_layout.py | 292 ++++++ 5 files changed, 1303 insertions(+), 945 deletions(-) create mode 100644 src/unitdev01/unitdev01/comm_panel.py create mode 100644 src/unitdev01/unitdev01/drone_panel.py create mode 100644 src/unitdev01/unitdev01/map_layout.py diff --git a/src/unitdev01/unitdev01/comm_panel.py b/src/unitdev01/unitdev01/comm_panel.py new file mode 100644 index 0000000..6259c8d --- /dev/null +++ b/src/unitdev01/unitdev01/comm_panel.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QLineEdit) +from PyQt6.QtCore import pyqtSignal + +class CommPanel(QWidget): + """通讯设置面板类""" + + # 定义信号 + udp_connection_added = pyqtSignal(str, int) # ip, port + udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + ws_connection_added = pyqtSignal(str) # url + ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + status_message = pyqtSignal(str, int) # message, timeout + + def __init__(self, parent=None): + super().__init__(parent) + self.udp_connections = [] + self.ws_connections = [] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self._handle_add_udp) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self._handle_add_ws) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + layout.addWidget(add_ws_widget) + layout.addStretch() + + def _handle_add_udp(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.status_message.emit("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.status_message.emit("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.udp_connection_added.emit(ip, port) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_udp_panel(self, conn): + """添加 UDP 連接面板到列表""" + panel = self.create_udp_connection_panel(conn) + self.udp_list_layout.addWidget(panel) + self.udp_connections.append(conn) + return panel + + def add_ws_panel(self, conn): + """添加 WebSocket 連接面板到列表""" + panel = self.create_ws_connection_panel(conn) + self.ws_list_layout.addWidget(panel) + self.ws_connections.append(conn) + return panel + + def remove_udp_connection_from_list(self, conn): + """從列表中移除 UDP 連接""" + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + def remove_ws_connection_from_list(self, conn): + """從列表中移除 WebSocket 連接""" + if conn in self.ws_connections: + self.ws_connections.remove(conn) \ No newline at end of file diff --git a/src/unitdev01/unitdev01/communication.py b/src/unitdev01/unitdev01/communication.py index 9584a62..2db56c1 100644 --- a/src/unitdev01/unitdev01/communication.py +++ b/src/unitdev01/unitdev01/communication.py @@ -130,6 +130,129 @@ class UDPMavlinkReceiver(threading.Thread): """停止接收器""" self.running = False +class SerialMavlinkReceiver(threading.Thread): + """串口 MAVLink 接收器""" + def __init__(self, port, baudrate, signals, connection_name): + super().__init__(daemon=True) + self.port = port + self.baudrate = baudrate + self.signals = signals + self.connection_name = connection_name + self.running = False + self.mav = None + + def run(self): + """執行串口接收循環""" + self.running = True + try: + print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") + + # 創建 MAVLink 串口連接 + self.mav = mavutil.mavlink_connection( + self.port, + baud=self.baudrate, + source_system=255 + ) + + print(f"Waiting for heartbeat from {self.port}...") + self.mav.wait_heartbeat() + print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") + + while self.running: + try: + msg = self.mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except Exception as e: + if self.running: + print(f"Error receiving MAVLink message from serial: {e}") + + except Exception as e: + print(f"Serial receiver error: {e}") + finally: + if self.mav: + try: + self.mav.close() + except: + pass + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message from serial: {e}") + + def stop(self): + """停止接收器""" + self.running = False + class WebSocketMavlinkReceiver(threading.Thread): """WebSocket MAVLink 接收器""" def __init__(self, url, signals, connection_name): @@ -266,6 +389,9 @@ class DroneMonitor(Node): # WebSocket 接收器列表 self.ws_receivers = [] + # 串口接收器列表 + self.serial_receivers = [] + # 主题检测定时器 self.create_timer(1.0, self.scan_topics) @@ -508,4 +634,13 @@ class DroneMonitor(Node): def ping_callback(self, drone_id, msg): self.latest_data[(drone_id, 'ping')] = { 'ping': msg.data - } \ No newline at end of file + } + + def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): + """啟動串口 MAVLink 連接""" + connection_name = f"Serial_{port.replace('/', '_')}" + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver.start() + self.serial_receivers.append(receiver) + print(f"Started serial connection on {port} at {baudrate} baud") + return receiver \ No newline at end of file diff --git a/src/unitdev01/unitdev01/drone_panel.py b/src/unitdev01/unitdev01/drone_panel.py new file mode 100644 index 0000000..a5a7bf5 --- /dev/null +++ b/src/unitdev01/unitdev01/drone_panel.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtCore import pyqtSignal + +class DronePanel(QWidget): + """單個無人機面板類別""" + + # 定義信號 + mode_change_requested = pyqtSignal(str) # drone_id + arm_requested = pyqtSignal(str) # drone_id + takeoff_requested = pyqtSignal(str) # drone_id + setpoint_requested = pyqtSignal(str) # drone_id + selection_changed = pyqtSignal(str, int) # drone_id, state + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.display_id = 's' + drone_id.split('_')[1] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"panel_{self.drone_id}") + self.setFixedHeight(140) + self.setStyleSheet(""" + background-color: #2A2A2A; + border-radius: 8px; + """) + + # 主佈局 + main_layout = QHBoxLayout(self) + main_layout.setContentsMargins(8, 8, 8, 8) + main_layout.setSpacing(0) + + # 創建內容容器（包含 info 和 control） + content_widget = QWidget() + content_widget.setStyleSheet("background-color: #333; border-radius: 6px;") + content_layout = QHBoxLayout(content_widget) + content_layout.setContentsMargins(8, 8, 8, 8) + content_layout.setSpacing(8) + + # 左側資訊區域 + info_widget = self._create_info_section() + + # 右側控制按鈕區域 + control_widget = self._create_control_section() + + # 將 info 和 control 加入內容容器 + content_layout.addWidget(info_widget) + content_layout.addWidget(control_widget) + + # 將內容容器加入主佈局 + main_layout.addWidget(content_widget) + + def _create_info_section(self): + """創建資訊顯示區域""" + info_widget = QWidget() + info_layout = QVBoxLayout(info_widget) + info_layout.setContentsMargins(0, 0, 0, 0) + info_layout.setSpacing(4) + + # 頂部標題欄 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # 勾選框 + self.checkbox = QCheckBox() + self.checkbox.setObjectName(f"{self.drone_id}_checkbox") + self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.stateChanged.connect( + lambda state: self.selection_changed.emit(self.drone_id, state) + ) + + # ID 顯示 + id_label = QLabel(self.display_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + header_layout.addWidget(self.checkbox) + header_layout.addWidget(id_label) + header_layout.addStretch() + + info_layout.addWidget(header) + + # 第一行：狀態 (模式 + ARM狀態) + status_row = self._create_status_row() + info_layout.addWidget(status_row) + + # 第二行：電池 + battery_row = self._create_battery_row() + info_layout.addWidget(battery_row) + + # 第三行：位置 + 高度 + position_row = self._create_position_row() + info_layout.addWidget(position_row) + + # 第四行：航向 + 速度 + nav_row = self._create_nav_row() + info_layout.addWidget(nav_row) + + return info_widget + + def _create_status_row(self): + """創建狀態行""" + status_row = QWidget() + status_layout = QHBoxLayout(status_row) + status_layout.setContentsMargins(0, 0, 0, 0) + + status_title = QLabel("狀態：") + status_title.setStyleSheet("color: #888; min-width: 50px;") + + self.mode_label = QLabel("--") + self.mode_label.setObjectName(f"{self.drone_id}_mode") + self.mode_label.setStyleSheet("color: #DDD;") + + self.armed_label = QLabel("--") + self.armed_label.setObjectName(f"{self.drone_id}_armed") + self.armed_label.setStyleSheet("color: #DDD;") + + status_layout.addWidget(status_title) + status_layout.addWidget(self.mode_label) + status_layout.addWidget(self.armed_label) + status_layout.addStretch() + + return status_row + + def _create_battery_row(self): + """創建電池行""" + battery_row = QWidget() + battery_layout = QHBoxLayout(battery_row) + battery_layout.setContentsMargins(0, 0, 0, 0) + + battery_title = QLabel("電池：") + battery_title.setStyleSheet("color: #888; min-width: 50px;") + + self.battery_label = QLabel("--") + self.battery_label.setObjectName(f"{self.drone_id}_battery") + self.battery_label.setStyleSheet("color: #DDD;") + + battery_layout.addWidget(battery_title) + battery_layout.addWidget(self.battery_label) + battery_layout.addStretch() + + return battery_row + + def _create_position_row(self): + """創建位置行""" + position_row = QWidget() + position_layout = QHBoxLayout(position_row) + position_layout.setContentsMargins(0, 0, 0, 0) + + position_title = QLabel("位置：") + position_title.setStyleSheet("color: #888; min-width: 50px;") + + self.local_label = QLabel("--") + self.local_label.setObjectName(f"{self.drone_id}_local") + self.local_label.setStyleSheet("color: #DDD;") + + altitude_title = QLabel("高度：") + altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.altitude_label = QLabel("--") + self.altitude_label.setObjectName(f"{self.drone_id}_altitude") + self.altitude_label.setStyleSheet("color: #DDD;") + + position_layout.addWidget(position_title) + position_layout.addWidget(self.local_label) + position_layout.addWidget(altitude_title) + position_layout.addWidget(self.altitude_label) + position_layout.addStretch() + + return position_row + + def _create_nav_row(self): + """創建導航行""" + nav_row = QWidget() + nav_layout = QHBoxLayout(nav_row) + nav_layout.setContentsMargins(0, 0, 0, 0) + + heading_title = QLabel("航向：") + heading_title.setStyleSheet("color: #888; min-width: 50px;") + + self.heading_label = QLabel("--") + self.heading_label.setObjectName(f"{self.drone_id}_heading") + self.heading_label.setStyleSheet("color: #DDD;") + + speed_title = QLabel("速度：") + speed_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.groundspeed_label = QLabel("--") + self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") + self.groundspeed_label.setStyleSheet("color: #DDD;") + + nav_layout.addWidget(heading_title) + nav_layout.addWidget(self.heading_label) + nav_layout.addWidget(speed_title) + nav_layout.addWidget(self.groundspeed_label) + nav_layout.addStretch() + + return nav_row + + def _create_control_section(self): + """創建控制按鈕區域""" + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + control_layout.setSpacing(6) + + control_widget.setFixedWidth(80) + + btn_style = """ + QPushButton { + background-color: #444; + color: #DDD; + border: none; + border-radius: 4px; + font-size: 11px; + } + QPushButton:hover { + background-color: #555; + } + """ + # 模式切換按鈕 + mode_btn = QPushButton("切換模式") + mode_btn.setStyleSheet(btn_style) + mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) + + # 解鎖按鈕 + arm_btn = QPushButton("解鎖") + arm_btn.setStyleSheet(btn_style) + arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) + + # 起飛按鈕 + takeoff_btn = QPushButton("起飛") + takeoff_btn.setStyleSheet(btn_style) + takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) + + # Setpoint 按鈕 + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.setStyleSheet(btn_style) + setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) + + control_layout.addWidget(mode_btn) + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addWidget(setpoint_btn) + + return control_widget + + def update_field(self, field, text, color=None): + """更新指定欄位的值""" + label = self.findChild(QLabel, f"{self.drone_id}_{field}") + if label and label.text() != text: + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def get_checkbox(self): + """獲取勾選框""" + return self.checkbox + + def set_checked(self, checked): + """設置勾選狀態""" + self.checkbox.setChecked(checked) + + def is_checked(self): + """獲取勾選狀態""" + return self.checkbox.isChecked() + +class SocketGroupPanel(QWidget): + # 定義信號 + group_selection_changed = pyqtSignal(str, int) # socket_id, state + + def __init__(self, socket_id, color='#AAAAAA', parent=None): + super().__init__(parent) + self.socket_id = socket_id + self.color = color + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"socket_group_{self.socket_id}") + self.setStyleSheet(""" + background-color: #1E1E1E; + border-radius: 12px; + """) + + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(6) + + # Socket 分組標題行 - 包含勾選框 + title_row = QWidget() + title_layout = QHBoxLayout(title_row) + title_layout.setContentsMargins(0, 0, 0, 0) + + # 分組勾選框 + self.group_checkbox = QCheckBox() + self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox") + self.group_checkbox.setStyleSheet(f""" + QCheckBox {{ color: #DDD; }} + QCheckBox::indicator {{ + width: 14px; + height: 14px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + }} + QCheckBox::indicator:checked {{ + background-color: {self.color}; + border: 2px solid #888888; + }} + QCheckBox::indicator:indeterminate {{ + background-color: #666; + border: 2px solid #888888; + }} + """) + self.group_checkbox.stateChanged.connect( + lambda state: self.group_selection_changed.emit(self.socket_id, state) + ) + + # Socket 分組標題 + title_label = QLabel(f"Socket {self.socket_id}") + title_label.setStyleSheet(f""" + font-weight: bold; + font-size: 16px; + color: {self.color}; + margin-bottom: 8px; + padding: 4px 8px; + border-radius: 6px; + """) + + title_layout.addWidget(self.group_checkbox) + title_layout.addWidget(title_label) + title_layout.addStretch() + + layout.addWidget(title_row) + + # 創建子容器用於放置該 socket 下的所有無人機面板 + self.drones_container = QWidget() + self.drones_layout = QVBoxLayout(self.drones_container) + self.drones_layout.setContentsMargins(0, 0, 0, 0) + self.drones_layout.setSpacing(4) + + layout.addWidget(self.drones_container) + + def add_drone_panel(self, panel): + """添加無人機面板到分組""" + self.drones_layout.addWidget(panel) + + def clear_drones(self): + """清空所有無人機面板""" + while self.drones_layout.count(): + item = self.drones_layout.takeAt(0) + if item.widget(): + item.widget().setParent(None) + + def get_checkbox(self): + """獲取分組勾選框""" + return self.group_checkbox + + def set_checked(self, checked): + """設置分組勾選狀態""" + self.group_checkbox.setChecked(checked) + + def set_check_state(self, state): + """設置分組勾選狀態（支持半選）""" + self.group_checkbox.setCheckState(state) \ No newline at end of file diff --git a/src/unitdev01/unitdev01/gui.py b/src/unitdev01/unitdev01/gui.py index 5eac6a3..615e9ff 100644 --- a/src/unitdev01/unitdev01/gui.py +++ b/src/unitdev01/unitdev01/gui.py @@ -5,12 +5,14 @@ from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit) from PyQt6.QtCore import Qt, QTimer -from PyQt6.QtWebEngineWidgets import QWebEngineView import sys import asyncio -# 從 communication.py 導入分離的類別 +# 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver +from map_layout import DroneMap +from drone_panel import DronePanel, SocketGroupPanel +from comm_panel import CommPanel class ControlStationUI(QMainWindow): def __init__(self): @@ -52,26 +54,30 @@ class ControlStationUI(QMainWindow): "loss_rate": 11, "ping": 12 } + self.socket_colors = { - '0': '#00BFFF', - '1': '#FFD700', - '2': "#FF6969", - '8': '#7CFC00', - '9': '#FF8C00', - 'default': '#AAAAAA' + '0': '#00BFFF', # 天藍色 (DeepSkyBlue) + '1': '#FFD700', # 金色 (Gold) + '2': '#FF6969', # 淺紅色 (Light Red) + '3': '#FF69B4', # 熱粉紅 (HotPink) + '4': '#00FA9A', # 中春綠 (MediumSpringGreen) + '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 + '6': '#FFA500', # 橙色 (Orange) + '7': '#20B2AA', # 淺海綠 (LightSeaGreen) + '8': '#7CFC00', # 草綠色 (LawnGreen) + '9': '#FF8C00', # 深橙色 (DarkOrange) + 'default': '#AAAAAA' # 灰色 } + self.drone_positions = {} self.drone_headings = {} - self.map_loaded = False - self.map_update_timer = QTimer() - self.map_update_timer.timeout.connect(self.update_map_positions) - self.map_update_timer.start(200) - self.pending_map_updates = {} - + # 初始化地圖 + self.drone_map = DroneMap() # 初始化連接列表 self.udp_receivers = [] self.udp_connections = [] self.ws_connections = [] + self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) self.init_ui() @@ -112,147 +118,16 @@ class ControlStationUI(QMainWindow): self.left_tab.addTab(self.overview_table, "總覽") # — 分頁 3：通訊設定 - self.comm_tab = QWidget() - comm_layout = QVBoxLayout(self.comm_tab) - comm_layout.setContentsMargins(10, 10, 10, 10) - comm_layout.setSpacing(10) - - # ========== UDP MAVLink 區域 ========== - udp_title = QLabel("UDP MAVLink") - udp_title.setStyleSheet(""" - color: #DDD; - font-size: 14px; - font-weight: bold; - padding: 5px; - background-color: #333; - border-radius: 4px; - """) - comm_layout.addWidget(udp_title) - - # UDP 連接列表容器 - self.udp_list_widget = QWidget() - self.udp_list_layout = QVBoxLayout(self.udp_list_widget) - self.udp_list_layout.setContentsMargins(0, 0, 0, 0) - self.udp_list_layout.setSpacing(5) - comm_layout.addWidget(self.udp_list_widget) - - # UDP 添加新連接區域 - add_udp_widget = QWidget() - add_udp_layout = QHBoxLayout(add_udp_widget) - add_udp_layout.setContentsMargins(0, 0, 0, 0) - - self.udp_ip_input = QLineEdit() - self.udp_ip_input.setText("127.0.0.1") - self.udp_ip_input.setPlaceholderText("IP") - self.udp_ip_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) + self.comm_panel = CommPanel() + self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) + self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) + self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) + self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) - self.udp_port_input = QLineEdit() - self.udp_port_input.setText("14540") - self.udp_port_input.setPlaceholderText("Port") - self.udp_port_input.setFixedWidth(80) - self.udp_port_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) - - add_udp_btn = QPushButton("添加") - add_udp_btn.clicked.connect(self.handle_add_udp_connection) - add_udp_btn.setStyleSheet(""" - QPushButton { - background-color: #4CAF50; - color: white; - border: none; - padding: 6px 12px; - border-radius: 4px; - min-width: 30px; - } - QPushButton:hover { background-color: #45a049; } - """) - - add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) - add_udp_layout.addWidget(self.udp_ip_input) - add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) - add_udp_layout.addWidget(self.udp_port_input) - add_udp_layout.addWidget(add_udp_btn) - - comm_layout.addWidget(add_udp_widget) - - # 分隔線 - separator = QWidget() - separator.setFixedHeight(20) - comm_layout.addWidget(separator) - - # ========== WebSocket 區域 ========== - ws_title = QLabel("WebSocket") - ws_title.setStyleSheet(""" - color: #DDD; - font-size: 14px; - font-weight: bold; - padding: 5px; - background-color: #333; - border-radius: 4px; - """) - comm_layout.addWidget(ws_title) - - # WebSocket 連接列表容器 - self.ws_list_widget = QWidget() - self.ws_list_layout = QVBoxLayout(self.ws_list_widget) - self.ws_list_layout.setContentsMargins(0, 0, 0, 0) - self.ws_list_layout.setSpacing(5) - comm_layout.addWidget(self.ws_list_widget) - - # WebSocket 添加新連接區域 - add_ws_widget = QWidget() - add_ws_layout = QHBoxLayout(add_ws_widget) - add_ws_layout.setContentsMargins(0, 0, 0, 0) - - self.ws_url_input = QLineEdit() - self.ws_url_input.setPlaceholderText("host") - self.ws_url_input.setStyleSheet(""" - QLineEdit { - background-color: #333; - color: #DDD; - border: 1px solid #555; - border-radius: 4px; - padding: 5px; - } - """) - - add_ws_btn = QPushButton("添加") - add_ws_btn.clicked.connect(self.handle_add_ws_connection) - add_ws_btn.setStyleSheet(""" - QPushButton { - background-color: #4CAF50; - color: white; - border: none; - padding: 6px 12px; - border-radius: 4px; - min-width: 30px; - } - QPushButton:hover { background-color: #45a049; } - """) - - add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) - add_ws_layout.addWidget(self.ws_url_input) - add_ws_layout.addWidget(add_ws_btn) - - comm_layout.addWidget(add_ws_widget) - comm_layout.addStretch() - - self.left_tab.addTab(self.comm_tab, "通訊") + self.left_tab.addTab(self.comm_panel, "通訊") # 右侧容器 right_container = QWidget() @@ -384,647 +259,54 @@ class ControlStationUI(QMainWindow): right_layout.addLayout(batch_control_layout) # 添加地圖 - self.map_view = QWebEngineView() - - inline_html = ''' - - - - - - - - - - -
-
- -
- - - - - ''' - self.map_view.setHtml(inline_html) - self.map_view.loadFinished.connect(self.on_map_loaded) - - right_layout.addWidget(self.map_view) + right_layout.addWidget(self.drone_map.get_widget()) + self.drone_map.get_gps_signal().connect(self.handle_map_click) + # Add widgets to splitter main_splitter.addWidget(self.left_tab) main_splitter.addWidget(right_container) main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) - def on_map_loaded(self, ok: bool): - if ok: - self.map_loaded = True - else: - print("⚠️ 地图页加载失败") - - # 修改2: 添加地圖更新節流方法 - def update_map_positions(self): - """批量更新地圖上的無人機位置""" - if not self.map_loaded or not self.pending_map_updates: - return - - # 批量執行所有待更新的位置 - js_commands = [] - for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): - js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") - - if js_commands: - combined_js = "\n".join(js_commands) - self.map_view.page().runJavaScript(combined_js) - - # 清空待更新緩存 - self.pending_map_updates.clear() - - def create_drone_panel(self, drone_id): - panel = QWidget() - panel.setObjectName(f"panel_{drone_id}") - panel.setFixedHeight(140) # 根據需要調整高度 - 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) - - # 主佈局改為水平佈局 - main_layout = QHBoxLayout(panel) - main_layout.setContentsMargins(8, 8, 8, 8) - main_layout.setSpacing(8) - - # 左側資訊區域 - info_widget = QWidget() - info_layout = QVBoxLayout(info_widget) - info_layout.setContentsMargins(0, 0, 0, 0) - info_layout.setSpacing(4) - - # 顶部标题栏 - header = QWidget() - header_layout = QHBoxLayout(header) - header_layout.setContentsMargins(0, 0, 0, 0) - - # 在標題列加入勾選框 - checkbox = QCheckBox() - checkbox.setObjectName(f"{drone_id}_checkbox") - checkbox.setStyleSheet("QCheckBox { color: #DDD; }") - checkbox.stateChanged.connect(lambda state: self.handle_drone_selection(drone_id, state)) - - header_layout.insertWidget(0, checkbox) # 插入到最前面 - - # ID显示 - 修改這裡：將 s0_4 格式改為 s4 格式 - display_id = 's' + drone_id.split('_')[1] - id_label = QLabel(display_id) - id_label.setStyleSheet(""" - font-weight: bold; - font-size: 14px; - color: #7FFFD4; - min-width: 80px; - """) - - header_layout.addWidget(id_label) - header_layout.addStretch() - - info_layout.addWidget(header) - - # 第一行：狀態 (模式 + ARM狀態) - status_row = QWidget() - status_layout = QHBoxLayout(status_row) - status_layout.setContentsMargins(0, 0, 0, 0) - - status_title = QLabel("狀態：") - status_title.setStyleSheet("color: #888; min-width: 50px;") - - mode_label = QLabel("--") - mode_label.setObjectName(f"{drone_id}_mode") - - armed_label = QLabel("--") - armed_label.setObjectName(f"{drone_id}_armed") - - status_layout.addWidget(status_title) - status_layout.addWidget(mode_label) - status_layout.addWidget(armed_label) - status_layout.addStretch() - - info_layout.addWidget(status_row) - - # 第二行：電池 - battery_row = QWidget() - battery_layout = QHBoxLayout(battery_row) - battery_layout.setContentsMargins(0, 0, 0, 0) - - battery_title = QLabel("電池：") - battery_title.setStyleSheet("color: #888; min-width: 50px;") - - battery_label = QLabel("--") - battery_label.setObjectName(f"{drone_id}_battery") - - battery_layout.addWidget(battery_title) - battery_layout.addWidget(battery_label) - battery_layout.addStretch() - - info_layout.addWidget(battery_row) - - # 第三行：位置 + 高度 - position_row = QWidget() - position_layout = QHBoxLayout(position_row) - position_layout.setContentsMargins(0, 0, 0, 0) - - position_title = QLabel("位置：") - position_title.setStyleSheet("color: #888; min-width: 50px;") - - local_label = QLabel("--") - local_label.setObjectName(f"{drone_id}_local") - - altitude_title = QLabel("高度：") - altitude_title.setStyleSheet("color: #888; margin-left: 10px;") - - altitude_label = QLabel("--") - altitude_label.setObjectName(f"{drone_id}_altitude") - - position_layout.addWidget(position_title) - position_layout.addWidget(local_label) - position_layout.addWidget(altitude_title) - position_layout.addWidget(altitude_label) - position_layout.addStretch() - - info_layout.addWidget(position_row) - - # 第四行：航向 + 速度 - nav_row = QWidget() - nav_layout = QHBoxLayout(nav_row) - nav_layout.setContentsMargins(0, 0, 0, 0) - - heading_title = QLabel("航向：") - heading_title.setStyleSheet("color: #888; min-width: 50px;") - - heading_label = QLabel("--") - heading_label.setObjectName(f"{drone_id}_heading") - - speed_title = QLabel("速度：") - speed_title.setStyleSheet("color: #888; margin-left: 10px;") - - groundspeed_label = QLabel("--") - groundspeed_label.setObjectName(f"{drone_id}_groundspeed") - - nav_layout.addWidget(heading_title) - nav_layout.addWidget(heading_label) - nav_layout.addWidget(speed_title) - nav_layout.addWidget(groundspeed_label) - nav_layout.addStretch() - - info_layout.addWidget(nav_row) - - # 右側控制按鈕區域 - control_widget = QWidget() - control_layout = QVBoxLayout(control_widget) - control_layout.setContentsMargins(0, 0, 0, 0) - control_layout.setSpacing(6) - - # 設置控制區域的固定寬度 - control_widget.setFixedWidth(80) - - mode_btn = QPushButton("切換模式") - mode_btn.setObjectName(f"{drone_id}_mode_btn") - mode_btn.clicked.connect(lambda: self.handle_mode_change(drone_id)) - mode_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - arm_btn = QPushButton("解鎖") - arm_btn.setObjectName(f"{drone_id}_arm_btn") - arm_btn.clicked.connect(lambda: self.handle_arm(drone_id)) - arm_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - takeoff_btn = QPushButton("起飛") - takeoff_btn.setObjectName(f"{drone_id}_takeoff_btn") - takeoff_btn.clicked.connect(lambda: self.handle_takeoff(drone_id)) - takeoff_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - setpoint_btn = QPushButton("Setpoint") - setpoint_btn.setObjectName(f"{drone_id}_setpoint_btn") - setpoint_btn.clicked.connect(lambda: self.handle_single_setpoint(drone_id)) - setpoint_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 8px 6px; - border-radius: 4px; - font-size: 11px; - } - QPushButton:hover { - background-color: #555; - } - """) - - # 按鈕由上而下排列 - control_layout.addWidget(mode_btn) - control_layout.addWidget(arm_btn) - control_layout.addWidget(takeoff_btn) - control_layout.addWidget(setpoint_btn) - control_layout.addStretch() # 將按鈕推向頂部 - - # 將左側資訊區域和右側控制區域加入主佈局 - main_layout.addWidget(info_widget) - main_layout.addWidget(control_widget) - - return panel - def create_udp_connection_panel(self, conn): - """創建 UDP 連接面板""" - panel = QWidget() - panel.setStyleSheet(""" - QWidget { - background-color: #2A2A2A; - border-radius: 6px; - padding: 8px; - border: 1px solid #444; - } - """) - - layout = QHBoxLayout(panel) - layout.setContentsMargins(8, 8, 8, 8) - - # 連接資訊 - info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") - info_label.setStyleSheet("color: #DDD; font-size: 12px;") - - # 狀態指示器 - status_label = QLabel("●") - if conn.get('enabled', False): - status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") - status_label.setToolTip("運行中") - else: - status_label.setStyleSheet("color: #888; font-size: 16px;") - status_label.setToolTip("已停止") - - # 控制按鈕 - toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") - toggle_btn.setFixedWidth(60) - toggle_btn.clicked.connect(lambda: self.toggle_udp_connection(conn, toggle_btn, status_label)) - toggle_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #555; } - """) - - remove_btn = QPushButton("移除") - remove_btn.setFixedWidth(60) - remove_btn.clicked.connect(lambda: self.remove_udp_connection(conn, panel)) - remove_btn.setStyleSheet(""" - QPushButton { - background-color: #d32f2f; - color: white; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #b71c1c; } - """) - - layout.addWidget(status_label) - layout.addWidget(info_label) - layout.addStretch() - layout.addWidget(toggle_btn) - layout.addWidget(remove_btn) - - # 儲存引用 - panel.connection = conn - panel.toggle_btn = toggle_btn - panel.status_label = status_label - - return panel - - def create_ws_connection_panel(self, conn): - """創建 WebSocket 連接面板""" - panel = QWidget() - panel.setStyleSheet(""" - QWidget { - background-color: #2A2A2A; - border-radius: 6px; - padding: 8px; - border: 1px solid #444; - } - """) - - layout = QHBoxLayout(panel) - layout.setContentsMargins(8, 8, 8, 8) - - # 連接資訊 - info_label = QLabel(f"{conn['name']} - {conn['url']}") - info_label.setStyleSheet("color: #DDD; font-size: 12px;") - - # 狀態指示器 - status_label = QLabel("●") - if conn.get('enabled', False): - status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") - status_label.setToolTip("運行中") - else: - status_label.setStyleSheet("color: #888; font-size: 16px;") - status_label.setToolTip("已停止") - - # 控制按鈕 - toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") - toggle_btn.setFixedWidth(60) - toggle_btn.clicked.connect(lambda: self.toggle_ws_connection(conn, toggle_btn, status_label)) - toggle_btn.setStyleSheet(""" - QPushButton { - background-color: #444; - color: #DDD; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #555; } - """) - - remove_btn = QPushButton("移除") - remove_btn.setFixedWidth(60) - remove_btn.clicked.connect(lambda: self.remove_ws_connection(conn, panel)) - remove_btn.setStyleSheet(""" - QPushButton { - background-color: #d32f2f; - color: white; - border: none; - padding: 4px 8px; - border-radius: 3px; - font-size: 11px; - } - QPushButton:hover { background-color: #b71c1c; } - """) - - layout.addWidget(status_label) - layout.addWidget(info_label) - layout.addStretch() - layout.addWidget(toggle_btn) - layout.addWidget(remove_btn) - - # 儲存引用 - panel.connection = conn - panel.toggle_btn = toggle_btn - panel.status_label = status_label - - return panel + def handle_map_click(self, lat, lon): + print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}") - def handle_add_ws_connection(self): - """處理添加 WebSocket 連接""" - input_url = self.ws_url_input.text().strip() - - if not input_url: - self.statusBar().showMessage("請輸入 WebSocket URL", 3000) - return + def handle_udp_connection_added(self, ip, port): + """处理 UDP 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } - # 自動添加 ws:// 前綴（如果用戶沒有輸入協議前綴） - if not input_url.startswith('ws://') and not input_url.startswith('wss://'): - url = f'ws://{input_url}' - else: - url = input_url + # 启动接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver - # 基本 URL 格式驗證 - try: - # 檢查是否包含 host:port 格式 - if '://' in url: - parts = url.split('://', 1) - if len(parts) == 2 and ':' not in parts[1]: - self.statusBar().showMessage("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) - return - except: - self.statusBar().showMessage("URL 格式錯誤", 3000) - return + self.udp_connections.append(new_conn) - # 檢查是否已存在相同連接 - for conn in self.ws_connections: - if conn['url'] == url: - self.statusBar().showMessage("連接已存在", 3000) - return + # 添加到 UI + self.comm_panel.add_udp_panel(new_conn) - # 創建新連接 + self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def handle_ws_connection_added(self, url): + """处理 WebSocket 连接添加信号""" + # 创建新连接 new_conn = { 'name': f'WS {len(self.ws_connections) + 1}', 'url': url, 'enabled': True } - # 啟動接收器 + # 启动接收器 receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) receiver.start() self.monitor.ws_receivers.append(receiver) @@ -1033,14 +315,23 @@ class ControlStationUI(QMainWindow): self.ws_connections.append(new_conn) # 添加到 UI - conn_panel = self.create_ws_connection_panel(new_conn) - self.ws_list_layout.addWidget(conn_panel) - - # 清空輸入框 - self.ws_url_input.clear() + self.comm_panel.add_ws_panel(new_conn) - self.statusBar().showMessage(f"已添加 WebSocket 連接: {url}", 3000) + self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000) print(f"WebSocket receiver added and started: {url}") + + def create_drone_panel(self, drone_id): + """創建無人機面板""" + panel = DronePanel(drone_id) + + # 連接信號 + panel.mode_change_requested.connect(self.handle_mode_change) + panel.arm_requested.connect(self.handle_arm) + panel.takeoff_requested.connect(self.handle_takeoff) + panel.setpoint_requested.connect(self.handle_single_setpoint) + panel.selection_changed.connect(self.handle_drone_selection) + + return panel def toggle_ws_connection(self, conn, btn, status_label): """切換 WebSocket 連接狀態""" @@ -1066,72 +357,25 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) def remove_ws_connection(self, conn, panel): - """移除 WebSocket 連接""" + """移除 WebSocket 连接""" # 停止接收器 if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.monitor.ws_receivers: self.monitor.ws_receivers.remove(conn['receiver']) - # 從列表移除 + # 从列表移除 if conn in self.ws_connections: self.ws_connections.remove(conn) - # 從 UI 移除 + # 从 comm_panel 列表移除 + self.comm_panel.remove_ws_connection_from_list(conn) + + # 从 UI 移除 panel.setParent(None) panel.deleteLater() - self.statusBar().showMessage(f"已移除 WebSocket 連接: {conn['url']}", 3000) - - def handle_add_udp_connection(self): - """處理添加 UDP 連接""" - ip = self.udp_ip_input.text().strip() - port_text = self.udp_port_input.text().strip() - - if not ip or not port_text: - self.statusBar().showMessage("請輸入 IP 和 Port", 3000) - return - - try: - port = int(port_text) - if port < 1 or port > 65535: - raise ValueError("Port 超出範圍") - except ValueError: - self.statusBar().showMessage("Port 必須是 1-65535 的數字", 3000) - return - - # 檢查是否已存在相同連接 - for conn in self.udp_connections: - if conn['ip'] == ip and conn['port'] == port: - self.statusBar().showMessage("連接已存在", 3000) - return - - # 創建新連接 - new_conn = { - 'name': f'UDP {len(self.udp_connections) + 1}', - 'ip': ip, - 'port': port, - 'enabled': True - } - - # 啟動接收器 - receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) - receiver.start() - self.udp_receivers.append(receiver) - new_conn['receiver'] = receiver - - self.udp_connections.append(new_conn) - - # 添加到 UI - conn_panel = self.create_udp_connection_panel(new_conn) - self.udp_list_layout.addWidget(conn_panel) - - # 清空輸入框 - self.udp_ip_input.clear() - self.udp_port_input.clear() - - self.statusBar().showMessage(f"已添加 UDP 連接: {ip}:{port}", 3000) - print(f"UDP MAVLink receiver added and started on {ip}:{port}") + self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000) def toggle_udp_connection(self, conn, btn, status_label): """切換 UDP 連接狀態""" @@ -1157,109 +401,32 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) def remove_udp_connection(self, conn, panel): - """移除 UDP 連接""" + """移除 UDP 连接""" # 停止接收器 if 'receiver' in conn and conn['receiver']: conn['receiver'].stop() if conn['receiver'] in self.udp_receivers: self.udp_receivers.remove(conn['receiver']) - # 從列表移除 + # 从列表移除 if conn in self.udp_connections: self.udp_connections.remove(conn) - # 從 UI 移除 + # 从 comm_panel 列表移除 + self.comm_panel.remove_udp_connection_from_list(conn) + + # 从 UI 移除 panel.setParent(None) panel.deleteLater() - self.statusBar().showMessage(f"已移除 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" - group_panel = QWidget() - group_panel.setObjectName(f"socket_group_{socket_id}") - group_panel.setStyleSheet(f""" - QWidget#socket_group_{socket_id} {{ - background-color: #1E1E1E; - border: 2px solid #555; - border-radius: 12px; - margin: 8px; - padding: 12px; - }} - QLabel {{ - color: #DDD; - font-size: 12px; - padding: 2px; - }} - """) - - layout = QVBoxLayout(group_panel) - layout.setContentsMargins(10, 10, 10, 10) - layout.setSpacing(6) - - # Socket 分組標題行 - 包含勾選框 - title_row = QWidget() - title_layout = QHBoxLayout(title_row) - title_layout.setContentsMargins(0, 0, 0, 0) - - # 分組勾選框 - group_checkbox = QCheckBox() - group_checkbox.setObjectName(f"socket_{socket_id}_checkbox") - group_checkbox.setStyleSheet("QCheckBox { color: #DDD; }") - group_checkbox.stateChanged.connect(lambda state: self.handle_group_selection(socket_id, state)) - - # Socket 分組標題 color = self.socket_colors.get(socket_id, self.socket_colors['default']) - title_label = QLabel(f"Socket {socket_id}") - title_label.setStyleSheet(f""" - font-weight: bold; - font-size: 16px; - color: {color}; - margin-bottom: 8px; - padding: 4px 8px; - background-color: #333; - border-radius: 6px; - """) - - title_layout.addWidget(group_checkbox) - title_layout.addWidget(title_label) - title_layout.addStretch() - - layout.addWidget(title_row) - - # 創建子容器用於放置該 socket 下的所有無人機面板 - drones_container = QWidget() - drones_layout = QVBoxLayout(drones_container) - drones_layout.setContentsMargins(0, 0, 0, 0) - drones_layout.setSpacing(4) - - layout.addWidget(drones_container) - - # 儲存容器的引用以便後續添加無人機 - group_panel.drones_container = drones_container - group_panel.drones_layout = drones_layout - - return group_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) + panel = SocketGroupPanel(socket_id, color) + panel.group_selection_changed.connect(self.handle_group_selection) + return panel def handle_mode_change(self, drone_id): """處理單個無人機的模式切換""" @@ -1436,11 +603,11 @@ class ControlStationUI(QMainWindow): self.update_overview_table(drone_id, 'pitch', pitch_text) self.update_overview_table(drone_id, 'yaw', yaw_text) - # 新的代碼 - 將地圖更新放入緩存，等待批量處理： + # 更新地圖上的無人機位置 if drone_id in self.drone_positions and drone_id in self.drone_headings: lat, lon = self.drone_positions[drone_id] heading = self.drone_headings[drone_id] - self.pending_map_updates[drone_id] = (lat, lon, heading) + self.drone_map.update_drone_position(drone_id, lat, lon, heading) # 新增處理分組勾選的方法 def handle_group_selection(self, socket_id, state): @@ -1453,7 +620,7 @@ class ControlStationUI(QMainWindow): is_checked = state == Qt.CheckState.Checked.value for drone_id in group_drones: - checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") + checkbox = self.drones[drone_id].get_checkbox() if checkbox: # 暫時斷開信號連接，避免遞迴觸發 checkbox.blockSignals(True) @@ -1489,7 +656,7 @@ class ControlStationUI(QMainWindow): # 檢查該分組內有多少無人機被勾選 checked_count = sum(1 for did in group_drones - if self.drones[did].findChild(QCheckBox, f"{did}_checkbox").isChecked()) + if self.drones[did].is_checked()) # 獲取分組勾選框 if socket_id in self.socket_groups: @@ -1515,7 +682,7 @@ class ControlStationUI(QMainWindow): """處理全選按鈕 - 支援分組結構""" # 檢查是否所有無人機都已被選中 all_selected = all( - self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox").isChecked() + self.drones[drone_id].is_checked() for drone_id in self.drones ) @@ -1524,9 +691,7 @@ class ControlStationUI(QMainWindow): # 更新所有勾選框狀態（無人機和分組） for drone_id in self.drones: - checkbox = self.drones[drone_id].findChild(QCheckBox, f"{drone_id}_checkbox") - if checkbox: - checkbox.setChecked(new_state) + self.drones[drone_id].set_checked(new_state) # 更新所有分組勾選框狀態 for socket_id in self.socket_groups: @@ -1557,20 +722,10 @@ class ControlStationUI(QMainWindow): future = self.monitor.set_mode(drone_id, mode) loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) - def handle_single_mode_change(self, drone_id): - mode = self.mode_combo.currentText() - loop = asyncio.get_event_loop() - future = self.monitor.set_mode(drone_id, mode) - loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) - def update_field(self, panel, drone_id, field, text, color=None): - """Update a specific field in the panel - 添加變更檢查""" - if label := panel.findChild(QLabel, f"{drone_id}_{field}"): - # 只有在文字真正改變時才更新 - if label.text() != text: - label.setText(text) - if color: - label.setStyleSheet(f"color: {color};") + """更新面板中的指定欄位""" + if isinstance(panel, DronePanel): + panel.update_field(field, text, color) def update_overview_table(self, drone_id=None, field=None, value=None): # Ensure the widget is available diff --git a/src/unitdev01/unitdev01/map_layout.py b/src/unitdev01/unitdev01/map_layout.py new file mode 100644 index 0000000..11f7171 --- /dev/null +++ b/src/unitdev01/unitdev01/map_layout.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python3 +from PyQt6.QtWebEngineWidgets import QWebEngineView +from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot +from PyQt6.QtWebChannel import QWebChannel + +class DroneMap: + """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" + + def __init__(self): + """初始化地圖""" + self.map_view = QWebEngineView() + self.map_loaded = False + self.pending_map_updates = {} + + # 創建橋接對象 + self.bridge = MapBridge() + + # 設置 QWebChannel + self.channel = QWebChannel() + self.channel.registerObject('bridge', self.bridge) + self.map_view.page().setWebChannel(self.channel) + + # 設置地圖 HTML + inline_html = ''' + + + + + + + + + + + +
+
+ +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + # 批量執行所有待更新的位置 + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + # 清空待更新緩存 + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) # lat, lon + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) \ No newline at end of file From e4b658d578fe24f62c0df0c0d98270039d734187 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Thu, 8 Jan 2026 17:14:12 +0800 Subject: [PATCH 091/146] Delete 'src/unitdev01/unitdev01/xbee.py' --- src/unitdev01/unitdev01/xbee.py | 619 -------------------------------- 1 file changed, 619 deletions(-) delete mode 100644 src/unitdev01/unitdev01/xbee.py diff --git a/src/unitdev01/unitdev01/xbee.py b/src/unitdev01/unitdev01/xbee.py deleted file mode 100644 index b550e1e..0000000 --- a/src/unitdev01/unitdev01/xbee.py +++ /dev/null @@ -1,619 +0,0 @@ -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 fa0a2d08315e527037599f89ce28814e68084f4c Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 16 Jan 2026 12:58:23 +0800 Subject: [PATCH 092/146] =?UTF-8?q?(Tested)=20=E6=96=B0=E5=A2=9E=E5=88=AA?= =?UTF-8?q?=E9=99=A4=E8=BC=89=E5=85=B7=E5=8A=9F=E8=83=BD=20=E4=BB=A3?= =?UTF-8?q?=E7=A2=BC=E5=84=AA=E5=8C=96=20=E8=A9=B3=E8=A6=8B=E6=94=B9?= =?UTF-8?q?=E7=89=88=E8=A8=98=E9=8C=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/fc_network_adapter.md | 28 ++++++---- .../fc_network_adapter/mainOrchestrator.py | 45 ++++++++++++---- .../fc_network_adapter/mavlinkObject.py | 51 ++++++++----------- .../fc_network_adapter/mavlinkVehicleView.py | 28 +++++++--- 4 files changed, 98 insertions(+), 54 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md index 98dba3a..ace76ac 100644 --- a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -84,16 +84,20 @@ 唯一實例 實際去解析 mavlink 封包的地方 接收 stream_bridge_ring 與 return_packet_ring 的資料 + 這邊是比較偏自動化 不會被操作的 - *self.thread* 自己的執行緒 --- - *_run_thread()* 核心方法 - *_handle_XXXXX()* 每一種單項 mavlink 封包的解析 +- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法 +- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理 ### **[Class]** async_io_manager 唯一實例 異步 event loop - 管理 mavlink_object 的地方 沒有核心方法 + 這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作) + - *self.thread* 自己的執行緒 - *self.managed_objects* 資料結構 socket_id: mavlink_object --- @@ -110,7 +114,7 @@ --- - *process_data()* [async method] 核心方法 - *remove_target_socket()* *add_target_socket()* -- *send_message()* +- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出 ## serialManager.py 看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件 @@ -120,7 +124,6 @@ 管理 mavlink_object 的地方 - *self.thread* 自己的執行緒 - *self.loop* 自己的事件迴圈 -- *self.serial_objects* 存放管理的物件 serial id num : serial_object --- - *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通 - *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序 @@ -157,10 +160,15 @@ ## 已實現功能 1. mavlink 分流解析 2. mavlink socket 建立 -3. mavlink socket 轉拋 -4. 連結 Serial 轉 UDP -5. 各單元模組化 -6. 終端機介面控制 -7. 基礎載具流量觀測 - - +3. mavlink socket 轉拋 proxy +4. 建立 Serial 轉 UDP 連結 並管理 +5. 建立 serial 連線 +6. 各單元模組化 +7. 終端機介面控制 +8. 基礎載具流量觀測 +9. 載具狀態收集與彙整 + +### 待開發功能 +5-1. 建立 serial 連線 並可以對接收器下達AT指令 +5-2. 模組化 serial 連線機制 以利後期擴容其他模組 +10-1. ros2 應用開發介面 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index f77d1ad..c7b573f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -28,7 +28,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -VERSION_NO = "v0.56" +VERSION_NO = "v0.58" class PanelState: def __init__(self): @@ -1088,12 +1088,17 @@ class ControlPanel: # 確保跳過顯示區域 line = line + 6 - dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") + dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...") dialog_win.refresh() # 檢查是否有按鍵（非阻塞） ch = dialog_win.getch() - if ch != -1: # 有按鍵則退出 + if ch in (ord('R'), ord('r')): + cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid)) + elif ch in (ord('C'), ord('c')): + cmd_q.put(("UNREGISTER_VEHICLE", sysid)) + break + elif ch != -1: # 有按鍵則退出 break # 短暫延遲 @@ -1197,7 +1202,7 @@ class Orchestrator: self.remove_target_from_object(source_id, target_id) elif action == "INSPECT_MAV_OBJECT": socket_id = cmd[1] - mav_obj = self.manager.managed_objects.get(socket_id, None) + mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None) if mav_obj: self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type self.panelState.socket_info_single["socket_state"] = mav_obj.state.name @@ -1223,9 +1228,17 @@ class Orchestrator: elif action == "INSPECT_VEHICLE": sysid, compid = cmd[1], cmd[2] self._prepare_vehicle_info(sysid, compid) - elif action == "UPDATE_VEHICLES_LIST": - self._update_vehicles_list() - + # elif action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug? + # logger.debug("Orchestrator: Updating vehicles list upon request") + # self._update_vehicles_list() + elif action == "RESET_VEHICLE_STATISTICS": + sysid, compid = cmd[1], cmd[2] + vehicle_sys = self.vehicle_registry.get(sysid) + vehicle_sys.reset_component_stats(compid) + elif action == "UNREGISTER_VEHICLE": + sysid = cmd[1] + self.vehicle_registry.unregister(sysid) + elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" self.create_udp_object() @@ -1240,12 +1253,12 @@ class Orchestrator: self.manager.shutdown() elif cmd == "SHUTDOWN_SERIAL_MANAGER": self.plumber.shutdown() + except queue.Empty: pass except Exception as e: logger.error(f"Error processing command: {e}") - time.sleep(0.1) except KeyboardInterrupt: @@ -1321,6 +1334,14 @@ class Orchestrator: def delete_udp_object(self, socket_id): """移除指定的 mavlink_object""" + + mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id] + connection_string = mavlink_obj.mavlink_socket.address + strings = connection_string.split(':') + ip = strings[0] + port = int(strings[1]) + self.occupied_ip_ports[ip].remove(port) + self.manager.remove_mavlink_object(socket_id) def add_target_to_object(self, source_id, target_id): @@ -1414,7 +1435,7 @@ class Orchestrator: # 更新 vehicle_info_single socket_type = "N/A" - socket_obj = self.manager.managed_objects.get(socket_id, None) + socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None) if socket_obj: socket_type = socket_obj.socket_type @@ -1534,5 +1555,11 @@ if __name__ == "__main__": 4. 修改 避免 serial 與 ip port 重複建立相同的通道 5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死 +2025.01.16 +1. 新增 "移除載具" 與 "重置載具統計" 功能 +2. 修正 udp port 在移除後仍被記錄為佔用的問題 +3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 +4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 7b321f1..82dccce 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -323,7 +323,7 @@ class mavlink_bridge: return False mav_obj = mavlink_object.mavlinkObjects[socket_id] - return mav_obj.send_message(message_bytes) + return mav_obj.message_put_queue(message_bytes) # 定義 mavlink_object 的狀態 class MavlinkObjectState(Enum): @@ -501,7 +501,7 @@ class mavlink_object: logger.error(f"Invalid return message types: {msg_types}") return False - def send_message(self, message_bytes): + def message_put_queue(self, message_bytes): """ 從主線程向此 mavlink_object 的 socket 發送數據 將數據添加到簡單的列表中，由 asyncio 任務處理 @@ -568,9 +568,7 @@ class async_io_manager: self.loop = None self.running = False # self.main_task = None - self.managed_objects = {} # socket_id: mavlink_object self.thread = None - self._stop_event = threading.Event() def __del__(self): self.loop = None @@ -586,7 +584,6 @@ class async_io_manager: return self.running = True - self._stop_event.clear() # 啟動獨立線程 命名為 AsyncIOManager self.thread = threading.Thread( @@ -618,12 +615,11 @@ class async_io_manager: return # 停止所有被管理的 mavlink_object 所屬的 task - for socket_id in list(self.managed_objects.keys()): + for socket_id in list(mavlink_object.mavlinkObjects.keys()): self.remove_mavlink_object(socket_id) # 停止自己的 task self.running = False - self._stop_event.set() # 解開事件循環的阻塞 self.loop.call_soon_threadsafe(self.loop.stop) @@ -662,15 +658,6 @@ class async_io_manager: self.loop = None self.running = False logger.info("async_io_manager event loop END!") - - # async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管 - # """主任務協程 讓 async_io_manager 在執行緒中持續運作""" - # logger.info("async_io_manager main task started") - - # while self.running and not self._stop_event.is_set(): - # await asyncio.sleep(0.1) - - # logger.info("async_io_manager main task ended") def add_mavlink_object(self, mavlink_obj: mavlink_object): """添加 mavlink_object""" @@ -681,9 +668,12 @@ class async_io_manager: socket_id = mavlink_obj.socket_id - if socket_id in self.managed_objects: - logger.warning(f"mavlink_object {socket_id} already managed") - return False + # 檢查該對象是否已經在運行中 + if socket_id in mavlink_object.mavlinkObjects: + existing_obj = mavlink_object.mavlinkObjects[socket_id] + if existing_obj.state == MavlinkObjectState.RUNNING: + logger.warning(f"mavlink_object {socket_id} already managed") + return False # 使用 run_coroutine_threadsafe 執行協程並獲取結果 future = asyncio.run_coroutine_threadsafe( @@ -708,7 +698,6 @@ class async_io_manager: try: task = asyncio.create_task(mavlink_obj.process_data()) - self.managed_objects[socket_id] = mavlink_obj mavlink_obj.task = task mavlink_obj.state = MavlinkObjectState.RUNNING mavlink_obj.outgoing_msgs.clear() @@ -743,11 +732,11 @@ class async_io_manager: async def _async_remove_mavlink_object(self, socket_id): """在事件循環線程中同步執行""" - if socket_id not in self.managed_objects: - logger.warning(f"mavlink_object {socket_id} not managed") - return + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False - mavlink_obj = self.managed_objects[socket_id] + mavlink_obj = mavlink_object.mavlinkObjects[socket_id] mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN if not mavlink_obj.task.done(): @@ -761,9 +750,8 @@ class async_io_manager: break await asyncio.sleep(0.1) - # 如果正常結束 則移除 + # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理） if mavlink_obj.task.done(): - del self.managed_objects[socket_id] mavlink_obj.state = MavlinkObjectState.CLOSED logger.info(f"Removed mavlink_object {socket_id} from manager.") return True @@ -773,8 +761,9 @@ class async_io_manager: return False def get_managed_objects(self): - """獲取所有被管理的對象列表""" - return list(self.managed_objects.keys()) + """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）""" + return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items() + if obj.state == MavlinkObjectState.RUNNING] # ====================== 分割線 ===================== @@ -807,9 +796,13 @@ if __name__ == '__main__': 3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 -6. mavlink_object 優化 send_message 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 7. 移除迴圈內的 try except 堆疊 增加效能 8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 +2026年 01月 15日 +1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects +2. async_io_manager 的 _stop_event 無效變數移除 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index f59eb17..aeae274 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -222,15 +222,15 @@ class VehicleComponent: def reset_packet_stats(self) -> None: """重置封包統計""" self.packet_stats = PacketStats() - + def set_parameter(self, param_name: str, param_value: Any) -> None: """設定參數 (手動餵入)""" self.parameters[param_name] = param_value - + def get_parameter(self, param_name: str, default: Any = None) -> Any: """取得參數""" return self.parameters.get(param_name, default) - + def __str__(self) -> str: return (f"Component(id={self.component_id}, type={self.type.value}, " f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, " @@ -323,7 +323,7 @@ class VehicleView: """ if component_id not in self.components: self.components[component_id] = VehicleComponent(component_id, comp_type) - logger.info(f"Added component {component_id} to system {self.sysid}") + # logger.info(f"Added component {component_id} to system {self.sysid}") return self.components[component_id] def get_component(self, component_id: int) -> Optional[VehicleComponent]: @@ -334,10 +334,17 @@ class VehicleView: """移除組件""" if component_id in self.components: del self.components[component_id] - logger.info(f"Removed component {component_id} from system {self.sysid}") + # logger.info(f"Removed component {component_id} from system {self.sysid}") return True return False - + + def reset_component_stats(self, component_id: int) -> None: + """重置指定組件的封包統計""" + component = self.get_component(component_id) + if component: + component.reset_packet_stats() + # logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}") + def set_rf_module(self, rf_type: RFModuleType) -> RFModule: """設定RF模組""" self.rf_module = RFModule(rf_type) @@ -435,3 +442,12 @@ class VehicleViewRegistry: # 全域註冊表實例 vehicle_registry = VehicleViewRegistry() + +''' +================= 改版記錄 ============================ + +2026.01.16 +1. 新增 重置指定組件的封包統計 功能 + +''' + From e0165c9aab818c272d7d6e449f8e355bc4b312da Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 21 Jan 2026 17:00:58 +0800 Subject: [PATCH 093/146] =?UTF-8?q?(add)=201.=20=E5=A2=9E=E5=8A=A0=20ros2?= =?UTF-8?q?=20topic=20publish?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/mavlinkROS2Nodes.py | 611 ++++++++++++++++++ .../tests/test_vehicleStatusPublisher.py | 498 ++++++++++++++ 2 files changed, 1109 insertions(+) create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py create mode 100644 src/fc_network_adapter/tests/test_vehicleStatusPublisher.py diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py new file mode 100644 index 0000000..496ce48 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -0,0 +1,611 @@ +""" +MAVLink ROS2 Nodes +包含兩個獨立的 ROS2 Node： +1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics +2. MavlinkCommandService - 提供 MAVLink 指令 service 介面 + +從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計 +""" + +import os +import time +import math +import threading +from typing import Dict, Optional + +# ROS2 imports +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor + +# ROS2 Message imports +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 自定義 imports +from . import mavlinkVehicleView as mvv +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ============================================================================ +# 頻率控制器 +# ============================================================================ + +class PublishRateController: + """發布頻率控制器 - 按時間間隔控制發布頻率""" + + def __init__(self): + # 各 topic 的發布間隔（秒） + self.topic_intervals = { + 'position': 0.5, # GPS位置 2Hz + 'attitude': 0.5, # 姿態 2Hz + 'velocity': 0.5, # 速度 2Hz + 'battery': 1.0, # 電池 1Hz + 'vfr_hud': 0.5, # VFR HUD 2Hz + 'mode': 1.0, # 飛行模式 1Hz + 'summary': 1.0, # 載具摘要 1Hz + } + # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} + self.last_publish_time: Dict[tuple, float] = {} + + def should_publish(self, sysid: int, topic: str) -> bool: + """ + 檢查是否應該發布此 topic + + Args: + sysid: 系統ID + topic: topic 名稱 + + Returns: + bool: True 表示應該發布 + """ + key = (sysid, topic) + now = time.time() + + # 首次發布 + if key not in self.last_publish_time: + self.last_publish_time[key] = now + return True + + # 檢查時間間隔 + interval = self.topic_intervals.get(topic, 1.0) + if now - self.last_publish_time[key] >= interval: + self.last_publish_time[key] = now + return True + + return False + + def reset(self): + """重置所有計時器""" + self.last_publish_time.clear() + + +# ============================================================================ +# VehicleStatusPublisher Node +# ============================================================================ + +class VehicleStatusPublisher(Node): + """ + 載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics + + 職責: + - 定期從 vehicle_registry 讀取載具狀態 + - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz） + - 發布標準 ROS2 消息類型 + - 檢測訂閱者，按需發布 + """ + + def __init__(self): + super().__init__('vehicle_status_publisher') + + # 頻率控制器 + self.rate_controller = PublishRateController() + + # fc_publishers 字典 {(sysid, topic_name): publisher} + self.fc_publishers: Dict[tuple, any] = {} + + # 定時器：以較高頻率檢查 vehicle_registry 並發布 + # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率 + self.timer_period = 0.1 # 100ms + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + # 狀態標誌 + self.running = True + + # logger.info("VehicleStatusPublisher initialized") + + def timer_callback(self): + """定時器回調 - 檢查所有載具並發布狀態""" + if not self.running: + return + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = mvv.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + self._publish_vehicle_status(vehicle) + + def _publish_vehicle_status(self, vehicle: mvv.VehicleView): + """ + 發布單個載具的所有狀態 + + Args: + vehicle: VehicleView 實例 + """ + sysid = vehicle.sysid + + # 假設只有一個 autopilot component (component_id=1) + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # 發布各種狀態（通過頻率控制器判斷是否發布） + self._publish_position(sysid, status) + self._publish_attitude(sysid, status) + self._publish_velocity(sysid, status) + self._publish_battery(sysid, status) + self._publish_vfr_hud(sysid, status) + self._publish_mode(sysid, status) + self._publish_summary(vehicle) + + def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): + """ + 獲取或創建 publisher + + Args: + sysid: 系統ID + topic: topic 名稱 + msg_type: ROS2 消息類型 + qos: QoS 設置 + + Returns: + publisher 對象 + """ + key = (sysid, topic) + if key not in self.fc_publishers: + topic_name = f'/vehicle_status/sys{sysid}/{topic}' + publisher = self.create_publisher(msg_type, topic_name, qos) + self.fc_publishers[key] = publisher + logger.info(f"Created publisher: {topic_name}") + return self.fc_publishers[key] + + def _publish_position(self, sysid: int, status: mvv.ComponentStatus): + """發布 GPS 位置""" + if not self.rate_controller.should_publish(sysid, 'position'): + return + + pos = status.position + if pos.latitude is None or pos.longitude is None: + return + + publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix) + + # 檢查是否有訂閱者 + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.NavSatFix() + msg.latitude = pos.latitude + msg.longitude = pos.longitude + msg.altitude = pos.altitude if pos.altitude is not None else 0.0 + + # GPS 狀態資訊 + gps = status.gps + if gps.fix_type is not None: + msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus + + publisher.publish(msg) + + def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus): + """發布姿態（IMU）""" + if not self.rate_controller.should_publish(sysid, 'attitude'): + return + + att = status.attitude + if att.roll is None: + return + + publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.Imu() + + # 歐拉角轉四元數 + qx, qy, qz, qw = self._euler_to_quaternion( + att.roll, att.pitch, att.yaw + ) + msg.orientation.x = qx + msg.orientation.y = qy + msg.orientation.z = qz + msg.orientation.w = qw + + # 角速度 + if att.rollspeed is not None: + msg.angular_velocity.x = att.rollspeed + msg.angular_velocity.y = att.pitchspeed + msg.angular_velocity.z = att.yawspeed + + publisher.publish(msg) + + def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus): + """發布速度""" + if not self.rate_controller.should_publish(sysid, 'velocity'): + return + + vfr = status.vfr + if vfr.groundspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped) + + if publisher.get_subscription_count() == 0: + return + + msg = geometry_msgs.msg.TwistStamped() + msg.header.stamp = self.get_clock().now().to_msg() + + # 使用 VFR HUD 的地速和航向計算速度分量 + if vfr.heading is not None: + heading_rad = math.radians(vfr.heading) + msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad) + msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad) + + # 爬升率作為 z 軸速度 + if vfr.climb is not None: + msg.twist.linear.z = vfr.climb + + publisher.publish(msg) + + def _publish_battery(self, sysid: int, status: mvv.ComponentStatus): + """發布電池狀態""" + if not self.rate_controller.should_publish(sysid, 'battery'): + return + + bat = status.battery + if bat.voltage is None: + return + + publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.BatteryState() + msg.voltage = bat.voltage + + if bat.current is not None: + msg.current = bat.current + + if bat.remaining is not None: + msg.percentage = bat.remaining / 100.0 + + if bat.temperature is not None: + msg.temperature = bat.temperature + + publisher.publish(msg) + + def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus): + """發布 VFR HUD""" + if not self.rate_controller.should_publish(sysid, 'vfr_hud'): + return + + vfr = status.vfr + if vfr.airspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud) + + if publisher.get_subscription_count() == 0: + return + + msg = mavros_msgs.msg.VfrHud() + msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0 + msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0 + msg.heading = vfr.heading if vfr.heading is not None else 0 + msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0 + msg.climb = vfr.climb if vfr.climb is not None else 0.0 + + # altitude 需要從 position 獲取 + if status.position.altitude is not None: + msg.altitude = status.position.altitude + + publisher.publish(msg) + + def _publish_mode(self, sysid: int, status: mvv.ComponentStatus): + """發布飛行模式""" + if not self.rate_controller.should_publish(sysid, 'mode'): + return + + mode = status.mode + if mode.mode_name is None: + return + + publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + msg = std_msgs.msg.String() + msg.data = mode.mode_name + publisher.publish(msg) + + def _publish_summary(self, vehicle: mvv.VehicleView): + """ + 發布載具摘要資訊（自定義格式，使用 String 暫時代替） + TODO: 未來可以定義專門的 VehicleSummary.msg + """ + sysid = vehicle.sysid + + if not self.rate_controller.should_publish(sysid, 'summary'): + return + + publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + # 獲取 autopilot component + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # 構建摘要資訊（JSON 格式字串） + import json + summary = { + 'sysid': sysid, + 'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0, + 'autopilot': component.mav_autopilot if component.mav_autopilot else 0, + 'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！ + 'armed': status.armed if status.armed is not None else False, + 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, + 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN", + 'latitude': status.position.latitude if status.position.latitude else 0.0, + 'longitude': status.position.longitude if status.position.longitude else 0.0, + 'altitude': status.position.altitude if status.position.altitude else 0.0, + 'battery_percent': status.battery.remaining if status.battery.remaining else 0, + 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0, + 'connection_type': vehicle.connected_via.value, + 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, + } + + msg = std_msgs.msg.String() + msg.data = json.dumps(summary) + publisher.publish(msg) + + @staticmethod + def _euler_to_quaternion(roll, pitch, yaw): + """ + 歐拉角轉四元數 + + Args: + roll: 橫滾角 (弧度) + pitch: 俯仰角 (弧度) + yaw: 偏航角 (弧度) + + Returns: + tuple: (qx, qy, qz, qw) + """ + 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 stop(self): + """停止發布""" + self.running = False + # logger.info("VehicleStatusPublisher stopped") + + +# ============================================================================ +# MavlinkCommandService Node +# ============================================================================ + +class MavlinkCommandService(Node): + """ + MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令 + + 職責: + - 作為 service server，等待 client 請求 + - 接收請求，組裝 MAVLink 封包 + - 調用 mavlinkObject 發送封包 + - 處理 ACK 等待和超時（未來實現） + + TODO: 稍後實現 + """ + + def __init__(self): + super().__init__('mavlink_command_service') + + logger.info("MavlinkCommandService initialized (not implemented yet)") + + def stop(self): + """停止服務""" + # logger.info("MavlinkCommandService stopped") + + +# ============================================================================ +# ROS2 節點管理器 +# ============================================================================ + +class fc_ros_manager: + """ + MAVLink ROS2 節點管理器 + + 管理兩個獨立的 ROS2 Node： + - VehicleStatusPublisher + - MavlinkCommandService + + 提供統一的啟動/停止介面給 mainOrchestrator + """ + + def __init__(self): + self.initialized = False + self.running = False + + # 两个 node 实例 + self.status_publisher: Optional[VehicleStatusPublisher] = None + self.command_service: Optional[MavlinkCommandService] = None + + # Executor & Thread + self.executor: Optional[MultiThreadedExecutor] = None + self.spin_thread: Optional[threading.Thread] = None + + def initialize(self): + """初始化 ROS2 环境和 nodes""" + if self.initialized: + logger.warning("fc_ros_manager already initialized") + return False + + try: + # 初始化 ROS2 + rclpy.init() + + # 創建節點 node + self.status_publisher = VehicleStatusPublisher() + self.command_service = MavlinkCommandService() + + # 創建執行者 MultiThreadedExecutor + self.executor = MultiThreadedExecutor() + self.executor.add_node(self.status_publisher) + self.executor.add_node(self.command_service) + + self.initialized = True + # logger.info("fc_ros_manager initialized") + return True + + except Exception as e: + logger.error(f"Failed to initialize fc_ros_manager: {e}") + return False + + def start(self): + """啟動 ROS2 nodes (在獨立執行緒中運行 executor) """ + if not self.initialized: + logger.error("fc_ros_manager initialize failed or not called") + return False + + if self.running: + logger.warning("fc_ros_manager already running") + return False + + try: + self.running = True + + self.spin_thread = threading.Thread( + target=self._spin_executor, + daemon=True, + name="ROS2ExecutorThread" + ) + self.spin_thread.start() + + logger.info("fc_ros_manager started <-") + return True + + except Exception as e: + logger.error(f"Failed to start fc_ros_manager: {e}") + self.running = False + return False + + def _spin_executor(self): + """在 thread 中運行的 executor""" + try: + # logger.info("ROS2 executor spinning...") + while self.running: + self.executor.spin_once(timeout_sec=0.1) + except Exception as e: + logger.error(f"fc_ros_manager error in spinning executor: {e}") + + def stop(self): + """停止 ROS2 nodes""" + if not self.running: + logger.warning("fc_ros_manager not running") + return False + + try: + logger.info("Stopping fc_ros_manager...") + + # 標記停止 + self.running = False + + # 停止各個 node + if self.status_publisher: + self.status_publisher.stop() + if self.command_service: + self.command_service.stop() + + # 等待 spin 執行緒結束 + if self.spin_thread and self.spin_thread.is_alive(): + self.spin_thread.join(timeout=2.0) + + logger.info("fc_ros_manager thread END!") + return True + + except Exception as e: + logger.error(f"Error stopping fc_ros_manager: {e}") + return False + + def shutdown(self): + """完全關閉並清理資源""" + if self.running: + self.stop() + + if self.initialized: + try: + # 銷毀 nodes + if self.status_publisher: + self.status_publisher.destroy_node() + if self.command_service: + self.command_service.destroy_node() + + # 關閉 ROS2 + if rclpy.ok(): + rclpy.shutdown() + + self.initialized = False + logger.info("fc_ros_manager Node END!") + + except Exception as e: + logger.error(f"Error during shutdown: {e}") + + def get_status(self) -> dict: + return { + 'initialized': self.initialized, + 'running': self.running, + 'status_publisher_active': self.status_publisher is not None and self.status_publisher.running, + 'command_service_active': self.command_service is not None, + } + + +# ============================================================================ +# 全域實例 +# ============================================================================ + +# 全域管理器實例（供 mainOrchestrator 使用） +ros2_manager = fc_ros_manager() + + +''' +================= 改版記錄 ============================ + +2026.01.20 +1. 重構自 mavlinkPublish.py +2. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態 +3. 添加頻率控制器 - 按需發布（2Hz 位置/姿態，1Hz 電池/摘要） +4. 預留 MavlinkCommandService 結構（稍後實現） +5. 提供 fc_ros_manager 統一管理介面 + +''' diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py new file mode 100644 index 0000000..5c902bd --- /dev/null +++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py @@ -0,0 +1,498 @@ +""" +VehicleStatusPublisher 測試程式 + +測試從 vehicle_registry 讀取資料並發布到 ROS2 topics +""" + +import time +import json +import threading + +# ROS2 imports +import rclpy +from rclpy.node import Node + +# 標準 ROS2 消息類型 +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 專案 imports +from ..fc_network_adapter.mavlinkROS2Nodes import ( + VehicleStatusPublisher, + fc_ros_manager, + ros2_manager +) +from ..fc_network_adapter.mavlinkVehicleView import ( + vehicle_registry, + ConnectionType, + ComponentType, +) + + +class TestSubscriber(Node): + """測試用的訂閱者節點 - 接收並記錄收到的消息""" + + def __init__(self, sysid: int = 1): + super().__init__(f'test_subscriber_sys{sysid}') + + self.sysid = sysid + self.received_messages = { + 'position': [], + 'attitude': [], + 'velocity': [], + 'battery': [], + 'vfr_hud': [], + 'mode': [], + 'summary': [], + } + + # 建立所有訂閱者 + self._create_subscriptions() + + print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics") + + def _create_subscriptions(self): + """建立所有 topic 的訂閱者""" + base_topic = f'/vehicle_status/sys{self.sysid}' + + # Position + self.create_subscription( + sensor_msgs.msg.NavSatFix, + f'{base_topic}/position', + lambda msg: self._on_message('position', msg), + 10 + ) + + # Attitude + self.create_subscription( + sensor_msgs.msg.Imu, + f'{base_topic}/attitude', + lambda msg: self._on_message('attitude', msg), + 10 + ) + + # Velocity + self.create_subscription( + geometry_msgs.msg.TwistStamped, + f'{base_topic}/velocity', + lambda msg: self._on_message('velocity', msg), + 10 + ) + + # Battery + self.create_subscription( + sensor_msgs.msg.BatteryState, + f'{base_topic}/battery', + lambda msg: self._on_message('battery', msg), + 10 + ) + + # VFR HUD + self.create_subscription( + mavros_msgs.msg.VfrHud, + f'{base_topic}/vfr_hud', + lambda msg: self._on_message('vfr_hud', msg), + 10 + ) + + # Mode + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/mode', + lambda msg: self._on_message('mode', msg), + 10 + ) + + # Summary + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/summary', + lambda msg: self._on_message('summary', msg), + 10 + ) + + def _on_message(self, topic_name: str, msg): + """通用消息接收回調""" + self.received_messages[topic_name].append(msg) + print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}") + + def _format_msg(self, topic_name: str, msg) -> str: + """格式化消息以便顯示""" + if topic_name == 'position': + return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m" + elif topic_name == 'attitude': + return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})" + elif topic_name == 'velocity': + return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})" + elif topic_name == 'battery': + return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%" + elif topic_name == 'vfr_hud': + return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}" + elif topic_name == 'mode': + return f"mode={msg.data}" + elif topic_name == 'summary': + try: + data = json.loads(msg.data) + return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}" + except: + return msg.data + return str(msg) + + def get_message_count(self, topic_name: str) -> int: + """獲取收到的消息數量""" + return len(self.received_messages[topic_name]) + + def clear_messages(self): + """清空已收到的消息""" + for key in self.received_messages: + self.received_messages[key].clear() + + +def setup_test_vehicle(sysid: int = 1, socket_id: int = 10): + """ + 建立測試用的載具數據 + + Args: + sysid: 系統 ID + socket_id: Socket ID + """ + print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===") + + # 註冊載具 + vehicle = vehicle_registry.register(sysid) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['socket_id'] = socket_id + + # 新增 autopilot 組件 (component_id=1) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + # 填充狀態資料 + status = autopilot.status + + # 位置 + status.position.latitude = 25.0330 + status.position.longitude = 121.5654 + status.position.altitude = 100.5 + status.position.relative_altitude = 50.0 + status.position.timestamp = time.time() + + # 姿態 + status.attitude.roll = 0.1 + status.attitude.pitch = -0.05 + status.attitude.yaw = 1.57 + status.attitude.rollspeed = 0.01 + status.attitude.pitchspeed = 0.02 + status.attitude.yawspeed = 0.03 + status.attitude.timestamp = time.time() + + # 飛行模式 + status.mode.base_mode = 89 + status.mode.custom_mode = 4 + status.mode.mode_name = "GUIDED" + status.mode.timestamp = time.time() + + # 電池 + status.battery.voltage = 12.6 + status.battery.current = 15.3 + status.battery.remaining = 75 + status.battery.temperature = 35.2 + status.battery.timestamp = time.time() + + # GPS + status.gps.fix_type = 3 # 3D fix + status.gps.satellites_visible = 12 + status.gps.eph = 100 + status.gps.epv = 150 + status.gps.timestamp = time.time() + + # VFR + status.vfr.airspeed = 5.5 + status.vfr.groundspeed = 6.0 + status.vfr.heading = 90 + status.vfr.throttle = 65 + status.vfr.climb = 1.2 + status.vfr.timestamp = time.time() + + # 系統狀態 + status.armed = True + status.system_status = 4 # MAV_STATE_ACTIVE + + # 更新封包統計 + autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time()) + + print(f"✓ 載具 {sysid} 已建立並填充測試數據") + return vehicle + + +def test_basic_publishing(): + """測試基本的發布功能""" + print("\n" + "="*60) + print("測試 1: 基本發布功能") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化 ROS2 管理器 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 開始發布，等待 5 秒 ---") + + # 運行 5 秒，持續 spin + start_time = time.time() + while time.time() - start_time < 5.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查收到的消息 + print("\n--- 消息統計 ---") + total_messages = 0 + for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + total_messages += count + print(f" {topic:15s}: {count:3d} 條消息") + + print(f"\n總計收到: {total_messages} 條消息") + + # 驗證 + if total_messages > 0: + print("\n✓ 測試通過：成功接收到消息") + else: + print("\n✗ 測試失敗：沒有接收到任何消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_frequency_control(): + """測試頻率控制功能""" + print("\n" + "="*60) + print("測試 2: 頻率控制") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化（如果還沒初始化） + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試頻率控制，運行 3 秒 ---") + print("預期：position/attitude 約 6 條 (2Hz)，battery/mode/summary 約 3 條 (1Hz)") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查頻率 + print("\n--- 頻率分析 ---") + + # 2Hz topics (預期約 6 條) + print("2Hz Topics (預期 ~6 條):") + for topic in ['position', 'attitude', 'velocity', 'vfr_hud']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + + # 1Hz topics (預期約 3 條) + print("\n1Hz Topics (預期 ~3 條):") + for topic in ['battery', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + + print("\n✓ 頻率控制測試完成") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_multi_vehicle(): + """測試多載具發布""" + print("\n" + "="*60) + print("測試 3: 多載具發布") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立 3 個測試載具 + v1 = setup_test_vehicle(sysid=1, socket_id=10) + v2 = setup_test_vehicle(sysid=2, socket_id=11) + v3 = setup_test_vehicle(sysid=3, socket_id=12) + + # 修改各載具的位置以便區分 + v2.components[1].status.position.latitude = 26.0 + v3.components[1].status.position.latitude = 27.0 + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立 3 個測試訂閱者 + test_sub1 = TestSubscriber(sysid=1) + test_sub2 = TestSubscriber(sysid=2) + test_sub3 = TestSubscriber(sysid=3) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試多載具，運行 3 秒 ---") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub1, timeout_sec=0.05) + rclpy.spin_once(test_sub2, timeout_sec=0.05) + rclpy.spin_once(test_sub3, timeout_sec=0.05) + time.sleep(0.1) + + # 檢查每個載具的消息 + print("\n--- 各載具消息統計 ---") + for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]: + total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"載具 {sysid}: {total:3d} 條消息") + + # 檢查 summary 中的 socket_id + if test_sub.get_message_count('summary') > 0: + last_summary = test_sub.received_messages['summary'][-1] + data = json.loads(last_summary.data) + print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}") + + print("\n✓ 多載具測試完成") + + # 停止 + ros2_manager.stop() + test_sub1.destroy_node() + test_sub2.destroy_node() + test_sub3.destroy_node() + + +def test_dynamic_vehicle(): + """測試動態新增/移除載具""" + print("\n" + "="*60) + print("測試 4: 動態載具管理") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 階段 1: 無載具，運行 1 秒 ---") + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_before} 條") + + # 新增載具 + print("\n--- 階段 2: 新增載具，運行 2 秒 ---") + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + start_time = time.time() + while time.time() - start_time < 2.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_after - count_before} 條") + + # 移除載具 + print("\n--- 階段 3: 移除載具，運行 1 秒 ---") + vehicle_registry.unregister(1) + + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_final - count_after} 條（應該為 0）") + + if count_final - count_after == 0: + print("\n✓ 動態載具管理測試通過") + else: + print("\n✗ 移除載具後仍收到消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def main(): + """主測試函數""" + print("\n" + "="*60) + print("VehicleStatusPublisher 測試程式") + print("="*60) + + try: + # 執行各項測試 + test_basic_publishing() + time.sleep(1) + + # test_frequency_control() + # time.sleep(1) + + # test_multi_vehicle() + # time.sleep(1) + + # test_dynamic_vehicle() + + print("\n" + "="*60) + print("所有測試完成！") + print("="*60) + + except KeyboardInterrupt: + print("\n\n測試被中斷") + except Exception as e: + print(f"\n\n測試出錯: {e}") + import traceback + traceback.print_exc() + finally: + # 清理 + if ros2_manager.initialized: + ros2_manager.shutdown() + vehicle_registry.clear() + print("\n清理完成") + + +if __name__ == '__main__': + main() From bbd120d25ae493eba132159e79d380dc1c6e1cfc Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 26 Jan 2026 13:16:21 +0800 Subject: [PATCH 094/146] =?UTF-8?q?(Tested)=20=E5=B0=87ROS2=20Topic=20?= =?UTF-8?q?=E7=B4=8D=E5=85=A5=20mainOrchestrator=20=E7=B3=BB=E7=B5=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +- .../fc_network_adapter/mainOrchestrator.py | 44 ++++++++++++++++--- .../fc_network_adapter/mavlinkROS2Nodes.py | 39 ++++++++-------- .../tests/test_vehicleStatusPublisher.py | 23 +++++++--- 4 files changed, 77 insertions(+), 32 deletions(-) diff --git a/README.md b/README.md index ce0703a..78b7d1a 100644 --- a/README.md +++ b/README.md @@ -68,6 +68,7 @@ N. logs 是執行時期的記錄檔 . ./install/local_setup.bash # 範例 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator -python -m fc_network_adapter.tests.test_ringBuffer python -m fc_network_adapter.tests.demo_integration + ``` + diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index c7b573f..2078490 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -22,6 +22,7 @@ from pymavlink import mavutil from . import mavlinkObject as mo from . import serialManager as sm from . import mavlinkVehicleView as mvv +from . import mavlinkROS2Nodes as mros from .utils import RingBuffer, setup_logger from .utils import acquireSerial, acquirePort @@ -37,6 +38,7 @@ class PanelState: self.mavlink_bridge_state = "Stopped" self.object_manager_state = "Stopped" self.serial_manager_state = "Stopped" + self.ros2_manager_state = "Stopped" self.socket_object_list = [] # 已有的 mavlink object self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 @@ -240,12 +242,13 @@ class ControlPanel: stdscr.border() # 更新模組狀態顯示 - stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) - stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") - stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") - stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") - stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") + stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") + stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") + stdscr.addstr(3, 36, f"ROS2 Manager State : {state.ros2_manager_state}") # 顯示當前選單項目 start_line = 6 @@ -1133,6 +1136,20 @@ class Orchestrator: # === 3) serial_manager 部分的準備 === self.plumber = sm.serial_manager() + # === 4) ros 部分的準備 === + self.fc_ros_manager = mros.ros2_manager + if not self.fc_ros_manager.initialized: + self.fc_ros_manager.initialize() + self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = { + 'position': 1.0, + 'attitude': 0.0, + 'velocity': 0.0, + 'battery': 1.0, + 'vfr_hud': 1.0, + 'mode': 0.0, + 'summary': 1.0, + } + def engageWholeSystem(self): """啟動整個系統""" # === 1) 面板部分的啟動 === @@ -1146,6 +1163,9 @@ class Orchestrator: # === 3) serial_manager 部分的啟動 === self.plumber.start() + # === 4) ros 部分的啟動 === + self.fc_ros_manager.start() + def mainLoop(self): logger.info("Main orchestrator started <-") try: @@ -1174,6 +1194,11 @@ class Orchestrator: linked_serial_dict = self.plumber.get_serial_link() self.panelState.linked_serial_dict = linked_serial_dict + if self.fc_ros_manager.running: + self.panelState.ros2_manager_state = 'Running' + else: + self.panelState.ros2_manager_state = 'Stopped' + # B. 更新載具列表（從 vehicle_registry 獲取） self._update_vehicles_list() @@ -1284,9 +1309,16 @@ class Orchestrator: self.plumber.shutdown() self.plumber.thread.join(timeout=2) + if self.fc_ros_manager.spin_thread.is_alive(): + if self.fc_ros_manager.running: + self.fc_ros_manager.stop() + self.fc_ros_manager.spin_thread.join(timeout=2) + # 關閉面板執行緒 if self.panel_thread.is_alive(): self.panel_thread.join(timeout=2) + + time.sleep(0.5) # 等待各模組穩定關閉 logger.info("Main orchestrator END!") @@ -1560,6 +1592,6 @@ if __name__ == "__main__": 2. 修正 udp port 在移除後仍被記錄為佔用的問題 3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 - +5. 系統納入 mavlink ROS2 Manager 模組 ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 496ce48..97ff5d0 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -41,13 +41,13 @@ class PublishRateController: def __init__(self): # 各 topic 的發布間隔（秒） self.topic_intervals = { - 'position': 0.5, # GPS位置 2Hz - 'attitude': 0.5, # 姿態 2Hz - 'velocity': 0.5, # 速度 2Hz - 'battery': 1.0, # 電池 1Hz - 'vfr_hud': 0.5, # VFR HUD 2Hz - 'mode': 1.0, # 飛行模式 1Hz - 'summary': 1.0, # 載具摘要 1Hz + 'position': 0.5, # GPS位置 + 'attitude': 0.5, # 姿態 + 'velocity': 0.5, # 速度 + 'battery': 1.0, # 電池 + 'vfr_hud': 0.5, # VFR HUD + 'mode': 1.0, # 飛行模式 + 'summary': 1.0, # 載具摘要 } # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} self.last_publish_time: Dict[tuple, float] = {} @@ -66,13 +66,17 @@ class PublishRateController: key = (sysid, topic) now = time.time() + # 當間隔設定為0或負數時 關閉該 topic 的發布 + interval = self.topic_intervals.get(topic, 0) + if interval <= 0: + return False + # 首次發布 if key not in self.last_publish_time: self.last_publish_time[key] = now return True # 檢查時間間隔 - interval = self.topic_intervals.get(topic, 1.0) if now - self.last_publish_time[key] >= interval: self.last_publish_time[key] = now return True @@ -98,6 +102,7 @@ class VehicleStatusPublisher(Node): - 發布標準 ROS2 消息類型 - 檢測訂閱者，按需發布 """ + topicString_prefix = f'/fc_network/vehicle' def __init__(self): super().__init__('vehicle_status_publisher') @@ -169,7 +174,7 @@ class VehicleStatusPublisher(Node): """ key = (sysid, topic) if key not in self.fc_publishers: - topic_name = f'/vehicle_status/sys{sysid}/{topic}' + topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}' publisher = self.create_publisher(msg_type, topic_name, qos) self.fc_publishers[key] = publisher logger.info(f"Created publisher: {topic_name}") @@ -462,8 +467,8 @@ class fc_ros_manager: self.command_service: Optional[MavlinkCommandService] = None # Executor & Thread - self.executor: Optional[MultiThreadedExecutor] = None self.spin_thread: Optional[threading.Thread] = None + self.executor: Optional[MultiThreadedExecutor] = None def initialize(self): """初始化 ROS2 环境和 nodes""" @@ -535,9 +540,7 @@ class fc_ros_manager: logger.warning("fc_ros_manager not running") return False - try: - logger.info("Stopping fc_ros_manager...") - + try: # 標記停止 self.running = False @@ -602,10 +605,10 @@ ros2_manager = fc_ros_manager() ================= 改版記錄 ============================ 2026.01.20 -1. 重構自 mavlinkPublish.py -2. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態 -3. 添加頻率控制器 - 按需發布（2Hz 位置/姿態，1Hz 電池/摘要） -4. 預留 MavlinkCommandService 結構（稍後實現） -5. 提供 fc_ros_manager 統一管理介面 +1. 重構自 mavlinkPublish.py (該檔案將被棄用) +2. 提供 fc_ros_manager 統一管理介面 +3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態 +4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布 +5. 預留 MavlinkCommandService 結構（稍後實現） ''' diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py index 5c902bd..6fd1914 100644 --- a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py +++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py @@ -55,7 +55,8 @@ class TestSubscriber(Node): def _create_subscriptions(self): """建立所有 topic 的訂閱者""" - base_topic = f'/vehicle_status/sys{self.sysid}' + + base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}' # Position self.create_subscription( @@ -299,12 +300,23 @@ def test_frequency_control(): # 建立測試訂閱者 test_sub = TestSubscriber(sysid=1) + + # 修改頻率設定 + publisher_node = ros2_manager.status_publisher + publisher_node.rate_controller.topic_intervals = { + 'position': 1.5, + 'attitude': 1.0, + 'velocity': 1.0, + 'battery': 1.0, + 'vfr_hud': 0.5, + 'mode': 0.0, + 'summary': 0.0, + } # 啟動 publisher ros2_manager.start() print("\n--- 測試頻率控制，運行 3 秒 ---") - print("預期：position/attitude 約 6 條 (2Hz)，battery/mode/summary 約 3 條 (1Hz)") # 運行 3 秒 start_time = time.time() @@ -314,15 +326,12 @@ def test_frequency_control(): # 檢查頻率 print("\n--- 頻率分析 ---") + print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布") - # 2Hz topics (預期約 6 條) print("2Hz Topics (預期 ~6 條):") for topic in ['position', 'attitude', 'velocity', 'vfr_hud']: count = test_sub.get_message_count(topic) print(f" {topic:15s}: {count:3d} 條") - - # 1Hz topics (預期約 3 條) - print("\n1Hz Topics (預期 ~3 條):") for topic in ['battery', 'mode', 'summary']: count = test_sub.get_message_count(topic) print(f" {topic:15s}: {count:3d} 條") @@ -466,7 +475,7 @@ def main(): try: # 執行各項測試 test_basic_publishing() - time.sleep(1) + # time.sleep(1) # test_frequency_control() # time.sleep(1) From 317483a5c01862e9aaf934cd345f97e66e8efaff Mon Sep 17 00:00:00 2001 From: wenchun Date: Tue, 27 Jan 2026 14:21:34 +0800 Subject: [PATCH 095/146] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20GUI=20=E8=B3=87?= =?UTF-8?q?=E6=96=99=E5=A4=BE=E8=88=87=E7=9B=B8=E9=97=9C=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/GUI/comm_panel.py | 398 +++++++++++++ src/GUI/communication.py | 662 ++++++++++++++++++++++ src/GUI/drone_panel.py | 378 +++++++++++++ src/GUI/gui.py | 1039 ++++++++++++++++++++++++++++++++++ src/GUI/map_layout.py | 423 ++++++++++++++ src/GUI/mission_planner.py | 323 +++++++++++ src/external/angles | 1 + src/external/geographic_info | 1 + 8 files changed, 3225 insertions(+) create mode 100644 src/GUI/comm_panel.py create mode 100644 src/GUI/communication.py create mode 100644 src/GUI/drone_panel.py create mode 100644 src/GUI/gui.py create mode 100644 src/GUI/map_layout.py create mode 100644 src/GUI/mission_planner.py create mode 160000 src/external/angles create mode 160000 src/external/geographic_info diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py new file mode 100644 index 0000000..6259c8d --- /dev/null +++ b/src/GUI/comm_panel.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QLineEdit) +from PyQt6.QtCore import pyqtSignal + +class CommPanel(QWidget): + """通讯设置面板类""" + + # 定义信号 + udp_connection_added = pyqtSignal(str, int) # ip, port + udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + ws_connection_added = pyqtSignal(str) # url + ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + status_message = pyqtSignal(str, int) # message, timeout + + def __init__(self, parent=None): + super().__init__(parent) + self.udp_connections = [] + self.ws_connections = [] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14540") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self._handle_add_udp) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self._handle_add_ws) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + layout.addWidget(add_ws_widget) + layout.addStretch() + + def _handle_add_udp(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.status_message.emit("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.status_message.emit("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.udp_connection_added.emit(ip, port) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_udp_panel(self, conn): + """添加 UDP 連接面板到列表""" + panel = self.create_udp_connection_panel(conn) + self.udp_list_layout.addWidget(panel) + self.udp_connections.append(conn) + return panel + + def add_ws_panel(self, conn): + """添加 WebSocket 連接面板到列表""" + panel = self.create_ws_connection_panel(conn) + self.ws_list_layout.addWidget(panel) + self.ws_connections.append(conn) + return panel + + def remove_udp_connection_from_list(self, conn): + """從列表中移除 UDP 連接""" + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + def remove_ws_connection_from_list(self, conn): + """從列表中移除 WebSocket 連接""" + if conn in self.ws_connections: + self.ws_connections.remove(conn) \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py new file mode 100644 index 0000000..45c960c --- /dev/null +++ b/src/GUI/communication.py @@ -0,0 +1,662 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class SerialMavlinkReceiver(threading.Thread): + """串口 MAVLink 接收器""" + def __init__(self, port, baudrate, signals, connection_name): + super().__init__(daemon=True) + self.port = port + self.baudrate = baudrate + self.signals = signals + self.connection_name = connection_name + self.running = False + self.mav = None + + def run(self): + """執行串口接收循環""" + self.running = True + try: + print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") + + # 創建 MAVLink 串口連接 + self.mav = mavutil.mavlink_connection( + self.port, + baud=self.baudrate, + source_system=255 + ) + + print(f"Waiting for heartbeat from {self.port}...") + self.mav.wait_heartbeat() + print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") + + while self.running: + try: + msg = self.mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except Exception as e: + if self.running: + print(f"Error receiving MAVLink message from serial: {e}") + + except Exception as e: + print(f"Serial receiver error: {e}") + finally: + if self.mav: + try: + self.mav.close() + except: + pass + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message from serial: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 串口接收器列表 + + # ================================================================================ + # 【新增】儲存 GPS 資料的字典 + # ================================================================================ + self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} + # ================================================================================ + self.serial_receivers = [] + + # 主题检测定时器 + 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+_\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(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + 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 + ), + '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 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_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) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + 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) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + '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.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + # ================================================================================ + # 【新增】儲存 GPS 資料到 drone_gps 字典 + # ================================================================================ + self.drone_gps[drone_id] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + # ================================================================================ + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } + + def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): + """啟動串口 MAVLink 連接""" + connection_name = f"Serial_{port.replace('/', '_')}" + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver.start() + self.serial_receivers.append(receiver) + print(f"Started serial connection on {port} at {baudrate} baud") + return receiver \ No newline at end of file diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py new file mode 100644 index 0000000..a5a7bf5 --- /dev/null +++ b/src/GUI/drone_panel.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtCore import pyqtSignal + +class DronePanel(QWidget): + """單個無人機面板類別""" + + # 定義信號 + mode_change_requested = pyqtSignal(str) # drone_id + arm_requested = pyqtSignal(str) # drone_id + takeoff_requested = pyqtSignal(str) # drone_id + setpoint_requested = pyqtSignal(str) # drone_id + selection_changed = pyqtSignal(str, int) # drone_id, state + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.display_id = 's' + drone_id.split('_')[1] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"panel_{self.drone_id}") + self.setFixedHeight(140) + self.setStyleSheet(""" + background-color: #2A2A2A; + border-radius: 8px; + """) + + # 主佈局 + main_layout = QHBoxLayout(self) + main_layout.setContentsMargins(8, 8, 8, 8) + main_layout.setSpacing(0) + + # 創建內容容器（包含 info 和 control） + content_widget = QWidget() + content_widget.setStyleSheet("background-color: #333; border-radius: 6px;") + content_layout = QHBoxLayout(content_widget) + content_layout.setContentsMargins(8, 8, 8, 8) + content_layout.setSpacing(8) + + # 左側資訊區域 + info_widget = self._create_info_section() + + # 右側控制按鈕區域 + control_widget = self._create_control_section() + + # 將 info 和 control 加入內容容器 + content_layout.addWidget(info_widget) + content_layout.addWidget(control_widget) + + # 將內容容器加入主佈局 + main_layout.addWidget(content_widget) + + def _create_info_section(self): + """創建資訊顯示區域""" + info_widget = QWidget() + info_layout = QVBoxLayout(info_widget) + info_layout.setContentsMargins(0, 0, 0, 0) + info_layout.setSpacing(4) + + # 頂部標題欄 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # 勾選框 + self.checkbox = QCheckBox() + self.checkbox.setObjectName(f"{self.drone_id}_checkbox") + self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.stateChanged.connect( + lambda state: self.selection_changed.emit(self.drone_id, state) + ) + + # ID 顯示 + id_label = QLabel(self.display_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + header_layout.addWidget(self.checkbox) + header_layout.addWidget(id_label) + header_layout.addStretch() + + info_layout.addWidget(header) + + # 第一行：狀態 (模式 + ARM狀態) + status_row = self._create_status_row() + info_layout.addWidget(status_row) + + # 第二行：電池 + battery_row = self._create_battery_row() + info_layout.addWidget(battery_row) + + # 第三行：位置 + 高度 + position_row = self._create_position_row() + info_layout.addWidget(position_row) + + # 第四行：航向 + 速度 + nav_row = self._create_nav_row() + info_layout.addWidget(nav_row) + + return info_widget + + def _create_status_row(self): + """創建狀態行""" + status_row = QWidget() + status_layout = QHBoxLayout(status_row) + status_layout.setContentsMargins(0, 0, 0, 0) + + status_title = QLabel("狀態：") + status_title.setStyleSheet("color: #888; min-width: 50px;") + + self.mode_label = QLabel("--") + self.mode_label.setObjectName(f"{self.drone_id}_mode") + self.mode_label.setStyleSheet("color: #DDD;") + + self.armed_label = QLabel("--") + self.armed_label.setObjectName(f"{self.drone_id}_armed") + self.armed_label.setStyleSheet("color: #DDD;") + + status_layout.addWidget(status_title) + status_layout.addWidget(self.mode_label) + status_layout.addWidget(self.armed_label) + status_layout.addStretch() + + return status_row + + def _create_battery_row(self): + """創建電池行""" + battery_row = QWidget() + battery_layout = QHBoxLayout(battery_row) + battery_layout.setContentsMargins(0, 0, 0, 0) + + battery_title = QLabel("電池：") + battery_title.setStyleSheet("color: #888; min-width: 50px;") + + self.battery_label = QLabel("--") + self.battery_label.setObjectName(f"{self.drone_id}_battery") + self.battery_label.setStyleSheet("color: #DDD;") + + battery_layout.addWidget(battery_title) + battery_layout.addWidget(self.battery_label) + battery_layout.addStretch() + + return battery_row + + def _create_position_row(self): + """創建位置行""" + position_row = QWidget() + position_layout = QHBoxLayout(position_row) + position_layout.setContentsMargins(0, 0, 0, 0) + + position_title = QLabel("位置：") + position_title.setStyleSheet("color: #888; min-width: 50px;") + + self.local_label = QLabel("--") + self.local_label.setObjectName(f"{self.drone_id}_local") + self.local_label.setStyleSheet("color: #DDD;") + + altitude_title = QLabel("高度：") + altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.altitude_label = QLabel("--") + self.altitude_label.setObjectName(f"{self.drone_id}_altitude") + self.altitude_label.setStyleSheet("color: #DDD;") + + position_layout.addWidget(position_title) + position_layout.addWidget(self.local_label) + position_layout.addWidget(altitude_title) + position_layout.addWidget(self.altitude_label) + position_layout.addStretch() + + return position_row + + def _create_nav_row(self): + """創建導航行""" + nav_row = QWidget() + nav_layout = QHBoxLayout(nav_row) + nav_layout.setContentsMargins(0, 0, 0, 0) + + heading_title = QLabel("航向：") + heading_title.setStyleSheet("color: #888; min-width: 50px;") + + self.heading_label = QLabel("--") + self.heading_label.setObjectName(f"{self.drone_id}_heading") + self.heading_label.setStyleSheet("color: #DDD;") + + speed_title = QLabel("速度：") + speed_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.groundspeed_label = QLabel("--") + self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") + self.groundspeed_label.setStyleSheet("color: #DDD;") + + nav_layout.addWidget(heading_title) + nav_layout.addWidget(self.heading_label) + nav_layout.addWidget(speed_title) + nav_layout.addWidget(self.groundspeed_label) + nav_layout.addStretch() + + return nav_row + + def _create_control_section(self): + """創建控制按鈕區域""" + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + control_layout.setSpacing(6) + + control_widget.setFixedWidth(80) + + btn_style = """ + QPushButton { + background-color: #444; + color: #DDD; + border: none; + border-radius: 4px; + font-size: 11px; + } + QPushButton:hover { + background-color: #555; + } + """ + # 模式切換按鈕 + mode_btn = QPushButton("切換模式") + mode_btn.setStyleSheet(btn_style) + mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) + + # 解鎖按鈕 + arm_btn = QPushButton("解鎖") + arm_btn.setStyleSheet(btn_style) + arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) + + # 起飛按鈕 + takeoff_btn = QPushButton("起飛") + takeoff_btn.setStyleSheet(btn_style) + takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) + + # Setpoint 按鈕 + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.setStyleSheet(btn_style) + setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) + + control_layout.addWidget(mode_btn) + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addWidget(setpoint_btn) + + return control_widget + + def update_field(self, field, text, color=None): + """更新指定欄位的值""" + label = self.findChild(QLabel, f"{self.drone_id}_{field}") + if label and label.text() != text: + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def get_checkbox(self): + """獲取勾選框""" + return self.checkbox + + def set_checked(self, checked): + """設置勾選狀態""" + self.checkbox.setChecked(checked) + + def is_checked(self): + """獲取勾選狀態""" + return self.checkbox.isChecked() + +class SocketGroupPanel(QWidget): + # 定義信號 + group_selection_changed = pyqtSignal(str, int) # socket_id, state + + def __init__(self, socket_id, color='#AAAAAA', parent=None): + super().__init__(parent) + self.socket_id = socket_id + self.color = color + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"socket_group_{self.socket_id}") + self.setStyleSheet(""" + background-color: #1E1E1E; + border-radius: 12px; + """) + + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(6) + + # Socket 分組標題行 - 包含勾選框 + title_row = QWidget() + title_layout = QHBoxLayout(title_row) + title_layout.setContentsMargins(0, 0, 0, 0) + + # 分組勾選框 + self.group_checkbox = QCheckBox() + self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox") + self.group_checkbox.setStyleSheet(f""" + QCheckBox {{ color: #DDD; }} + QCheckBox::indicator {{ + width: 14px; + height: 14px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + }} + QCheckBox::indicator:checked {{ + background-color: {self.color}; + border: 2px solid #888888; + }} + QCheckBox::indicator:indeterminate {{ + background-color: #666; + border: 2px solid #888888; + }} + """) + self.group_checkbox.stateChanged.connect( + lambda state: self.group_selection_changed.emit(self.socket_id, state) + ) + + # Socket 分組標題 + title_label = QLabel(f"Socket {self.socket_id}") + title_label.setStyleSheet(f""" + font-weight: bold; + font-size: 16px; + color: {self.color}; + margin-bottom: 8px; + padding: 4px 8px; + border-radius: 6px; + """) + + title_layout.addWidget(self.group_checkbox) + title_layout.addWidget(title_label) + title_layout.addStretch() + + layout.addWidget(title_row) + + # 創建子容器用於放置該 socket 下的所有無人機面板 + self.drones_container = QWidget() + self.drones_layout = QVBoxLayout(self.drones_container) + self.drones_layout.setContentsMargins(0, 0, 0, 0) + self.drones_layout.setSpacing(4) + + layout.addWidget(self.drones_container) + + def add_drone_panel(self, panel): + """添加無人機面板到分組""" + self.drones_layout.addWidget(panel) + + def clear_drones(self): + """清空所有無人機面板""" + while self.drones_layout.count(): + item = self.drones_layout.takeAt(0) + if item.widget(): + item.widget().setParent(None) + + def get_checkbox(self): + """獲取分組勾選框""" + return self.group_checkbox + + def set_checked(self, checked): + """設置分組勾選狀態""" + self.group_checkbox.setChecked(checked) + + def set_check_state(self, state): + """設置分組勾選狀態（支持半選）""" + self.group_checkbox.setCheckState(state) \ No newline at end of file diff --git a/src/GUI/gui.py b/src/GUI/gui.py new file mode 100644 index 0000000..94c6889 --- /dev/null +++ b/src/GUI/gui.py @@ -0,0 +1,1039 @@ +#!/usr/bin/env python3 +import rclpy +from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, + QWidget, QLabel, QSplitter, QScrollArea, + QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, + QHeaderView, QPushButton, QCheckBox, QLineEdit) +from PyQt6.QtCore import Qt, QTimer +import sys +import asyncio + +# 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver +from map_layout import DroneMap +from drone_panel import DronePanel, SocketGroupPanel +from comm_panel import CommPanel + +# ================================================================================ +# 【新增】導入任務規劃器 +# ================================================================================ +from mission_planner import FormationPlanner +# ================================================================================ + +class ControlStationUI(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle('GCS') + self.resize(1400, 900) + + # 初始化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) + + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(10) + + # 初始化UI + self.drones = {} + self.socket_groups = {} + self.info_types = ["模式", "ARM", "電壓", "高度", "Local", "Velocity", "地速", "航向", + "Roll", "Pitch", "Yaw", "丟包", "延遲"] + self.info_type_map = { + "mode": 0, + "armed": 1, + "battery": 2, + "altitude": 3, + "local": 4, + "velocity": 5, + "groundspeed": 6, + "heading": 7, + "roll": 8, + "pitch": 9, + "yaw": 10, + "loss_rate": 11, + "ping": 12 + } + + self.socket_colors = { + '0': '#00BFFF', # 天藍色 (DeepSkyBlue) + '1': '#FFD700', # 金色 (Gold) + '2': '#FF6969', # 淺紅色 (Light Red) + '3': '#FF69B4', # 熱粉紅 (HotPink) + '4': '#00FA9A', # 中春綠 (MediumSpringGreen) + '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 + '6': '#FFA500', # 橙色 (Orange) + '7': '#20B2AA', # 淺海綠 (LightSeaGreen) + '8': '#7CFC00', # 草綠色 (LawnGreen) + '9': '#FF8C00', # 深橙色 (DarkOrange) + 'default': '#AAAAAA' # 灰色 + } + + self.drone_positions = {} + self.drone_headings = {} + # 初始化地圖 + self.drone_map = DroneMap() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) + + # ================================================================================ + # 【新增】初始化任務規劃器 + # ================================================================================ + self.mission_planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + self.planned_waypoints = None # 儲存規劃結果 + # ================================================================================ + + self.init_ui() + + def init_ui(self): + # 只呼叫一次 + main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左側 TabWidget + self.left_tab = QTabWidget() + + # — 分頁 1：Drone Panel + self.drone_panel_container = QWidget() + self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) + self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + self.drone_panel_layout.setSpacing(0) + self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) + + # Wrap drone panel in scroll area + scroll = QScrollArea() + scroll.setWidget(self.drone_panel_container) + scroll.setWidgetResizable(True) + self.left_tab.addTab(scroll, "無人載具") + + # — 分頁 2：Overview Table + self.overview_table = QTableWidget() + self.overview_table.setColumnCount(1) + self.overview_table.setRowCount(len(self.info_types)) + self.overview_table.setHorizontalHeaderLabels(["資訊"]) + header = self.overview_table.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.overview_table.verticalHeader().setVisible(False) + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(i, 0, item) + + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_panel = CommPanel() + self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) + self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) + self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) + self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) + + self.left_tab.addTab(self.comm_panel, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + + # 上方按鈕區域 + upper_buttons = QHBoxLayout() + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + arm_all_btn = QPushButton("解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + takeoff_all_btn = QPushButton("起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: + btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + upper_buttons.addWidget(btn) + upper_buttons.addStretch() + + # 模式切換區域 + mode_layout = QHBoxLayout() + mode_label = QLabel("模式：") + mode_label.setStyleSheet("color: #DDD; min-width: 40px;") + + from PyQt6.QtWidgets import QComboBox + self.mode_combo = QComboBox() + self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) + self.mode_combo.setCurrentIndex(1) + self.mode_combo.setStyleSheet(""" + QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} + """) + + batch_mode_btn = QPushButton("切換") + batch_mode_btn.clicked.connect(self.handle_batch_mode_change) + batch_mode_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + mode_layout.addWidget(mode_label) + mode_layout.addWidget(self.mode_combo) + mode_layout.addWidget(batch_mode_btn) + mode_layout.addStretch() + + # 座標輸入區域 + coord_widget = QWidget() + coord_layout = QHBoxLayout(coord_widget) + + self.x_input = QLineEdit() + self.y_input = QLineEdit() + self.z_input = QLineEdit() + + for input_field in [self.x_input, self.y_input, self.z_input]: + input_field.setFixedWidth(60) + input_field.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) + + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.x_input) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.y_input) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.z_input) + + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.clicked.connect(self.handle_setpoint_selected) + setpoint_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + + lower_control = QHBoxLayout() + lower_control.addWidget(coord_widget) + lower_control.addWidget(setpoint_btn) + lower_control.addStretch() + + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) + + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 + right_layout.addWidget(self.drone_map.get_widget()) + self.drone_map.get_gps_signal().connect(self.handle_map_click) + + + # Add widgets to splitter + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) + main_splitter.setSizes([400, 1000]) + + self.setCentralWidget(main_splitter) + + + def handle_udp_connection_added(self, ip, port): + """处理 UDP 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 启动接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_udp_panel(new_conn) + + self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def handle_ws_connection_added(self, url): + """处理 WebSocket 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 启动接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_ws_panel(new_conn) + + self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def create_drone_panel(self, drone_id): + """創建無人機面板""" + panel = DronePanel(drone_id) + + # 連接信號 + panel.mode_change_requested.connect(self.handle_mode_change) + panel.arm_requested.connect(self.handle_arm) + panel.takeoff_requested.connect(self.handle_takeoff) + panel.setpoint_requested.connect(self.handle_single_setpoint) + panel.selection_changed.connect(self.handle_drone_selection) + + return panel + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_ws_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000) + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_udp_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) + + def create_socket_group_panel(self, socket_id): + """創建 socket 分組面板""" + color = self.socket_colors.get(socket_id, self.socket_colors['default']) + panel = SocketGroupPanel(socket_id, color) + panel.group_selection_changed.connect(self.handle_group_selection) + return panel + + def handle_mode_change(self, drone_id): + """處理單個無人機的模式切換""" + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}")) + + def handle_arm(self, drone_id): + loop = asyncio.get_event_loop() + arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state + future = self.monitor.arm_drone(drone_id, arm_state) + loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) + + def handle_takeoff(self, drone_id): + loop = asyncio.get_event_loop() + future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) + + def handle_setpoint_selected(self): + """處理發送 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + for drone_id in self.monitor.selected_drones: + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + def handle_single_setpoint(self, drone_id): + """處理單個無人機的 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + async def handle_service_response(self, future, action): + try: + result = await future + if result: + self.statusBar().showMessage(f"{action} 成功", 3000) + else: + self.statusBar().showMessage(f"{action} 失敗", 3000) + except Exception as e: + self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) + + def update_ui(self, msg_type, drone_id, data): + # 檢查是否為新無人機，若是則加入無人機面板 + if drone_id not in self.drones: + self.add_drone(drone_id) + return + + # 確認無人機面板存在 + if not (panel := self.drones.get(drone_id)): + return + + # 更新特定欄位並在總覽頁面更新 + if msg_type == 'state': + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', None) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + if armed is True: + arm_text = "ARMED" + arm_color = '#55FF55' + elif armed is False: + arm_text = "DISARMED" + arm_color = '#FF5555' + else: + arm_text = "--" + arm_color = '#AAAAAA' # 未知狀態 + + # 更新無人機面板欄位 + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + + # 更新總覽頁面欄位 + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + + elif msg_type == 'battery': + voltage = data.get('voltage', 16) + + # 判斷電池節數 + cells = round(voltage / 3.95) + + # 計算電量百分比 + percentage = (voltage / cells - 3.7) / 0.5 * 100 + + # 根據百分比設置顏色 + if percentage < 20: + voltage_color = '#FF6464' # 紅色 (低電量) + elif percentage < 50: + voltage_color = '#FFA500' # 橘色 (中低電量) + else: + voltage_color = '#FFFFFF' # 白色 (正常電量) + + percentage = data.get('percentage', percentage) + + # 顯示總電壓、電池節數、單節電壓和百分比 + text = f"{percentage:.0f}%" + vol = f"{voltage:.2f}V" + + self.update_field(panel, drone_id, 'battery', text, voltage_color) + self.update_overview_table(drone_id, 'battery', vol) + + elif msg_type == 'gps': + lat, lon = data.get('lat', 0), data.get('lon', 0) + self.drone_positions[drone_id] = (lat, lon) + text = f"{lat:.6f}°, {lon:.6f}°" + self.update_field(panel, drone_id, 'gps', text) + self.update_overview_table(drone_id, 'gps', text) + + # ================================================================================ + # 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料） + # ================================================================================ + alt = data.get('alt', 0) # UDP/WebSocket 可能沒有 alt + if not hasattr(self.monitor, 'drone_gps'): + self.monitor.drone_gps = {} + self.monitor.drone_gps[drone_id] = { + 'lat': lat, + 'lon': lon, + 'alt': alt + } + # ================================================================================ + + elif msg_type == 'altitude': + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" + self.update_field(panel, drone_id, 'altitude', text) + self.update_overview_table(drone_id, 'altitude', text) + + elif msg_type == 'local_pose': + text = f"{data['x']:.1f}, {data['y']:.1f}" + self.update_field(panel, drone_id, 'local', text) + self.update_overview_table(drone_id, 'local', text) + + elif msg_type == 'hud': + heading = data.get('heading') + self.drone_headings[drone_id] = heading + groundspeed = data.get('groundspeed') + heading_text = f"{heading:.1f}°" + if isinstance(groundspeed, (int, float)): + groundspeed_text = f"{groundspeed:.1f} m/s" + else: + groundspeed_text = "--" + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'heading', heading_text) + self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + + elif msg_type == 'loss_rate': + loss_rate = data.get('loss_rate', 0) + text = f"{loss_rate:.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + ping = data.get('ping', 0) + text = f"{ping:.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', text) + + elif msg_type == 'velocity': + text = f"{data['vx']:.1f}, {data['vy']:.1f}" + self.update_overview_table(drone_id, 'velocity', text) + + elif msg_type == 'attitude': + roll = data.get('roll', 0) + pitch = data.get('pitch', 0) + yaw = data.get('yaw', 0) + roll_text = f"{roll:.1f}°" + pitch_text = f"{pitch:.1f}°" + yaw_text = f"{yaw:.1f}°" + self.update_overview_table(drone_id, 'roll', roll_text) + self.update_overview_table(drone_id, 'pitch', pitch_text) + self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 更新地圖上的無人機位置 + if drone_id in self.drone_positions and drone_id in self.drone_headings: + lat, lon = self.drone_positions[drone_id] + heading = self.drone_headings[drone_id] + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + # 新增處理分組勾選的方法 + def handle_group_selection(self, socket_id, state): + """處理 socket 分組勾選狀態變化""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 根據分組勾選狀態更新所有該分組的無人機勾選狀態 + is_checked = state == Qt.CheckState.Checked.value + + for drone_id in group_drones: + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + # 暫時斷開信號連接，避免遞迴觸發 + checkbox.blockSignals(True) + checkbox.setChecked(is_checked) + checkbox.blockSignals(False) + + # 手動更新選中集合 + if is_checked: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + def handle_drone_selection(self, drone_id, state): + """處理個別無人機勾選狀態變化""" + if state == Qt.CheckState.Checked.value: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + # 更新對應 socket 分組的勾選狀態 + socket_id = self.get_socket_id(drone_id) + self.update_group_checkbox_state(socket_id) + + # 新增更新分組勾選框狀態的方法 + def update_group_checkbox_state(self, socket_id): + """更新指定 socket 分組的勾選框狀態""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + if not group_drones: + return + + # 檢查該分組內有多少無人機被勾選 + checked_count = sum(1 for did in group_drones + if self.drones[did].is_checked()) + + # 獲取分組勾選框 + if socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + # 暫時斷開信號連接 + group_checkbox.blockSignals(True) + + if checked_count == 0: + # 沒有無人機被勾選 + group_checkbox.setCheckState(Qt.CheckState.Unchecked) + elif checked_count == len(group_drones): + # 所有無人機都被勾選 + group_checkbox.setCheckState(Qt.CheckState.Checked) + else: + # 部分無人機被勾選，顯示半勾選狀態 + group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked) + + # 重新連接信號 + group_checkbox.blockSignals(False) + + def handle_select_all(self): + """處理全選按鈕 - 支援分組結構""" + # 檢查是否所有無人機都已被選中 + all_selected = all( + self.drones[drone_id].is_checked() + for drone_id in self.drones + ) + + # 如果全部已選中，則取消全選；否則全選 + new_state = not all_selected + + # 更新所有勾選框狀態（無人機和分組） + for drone_id in self.drones: + self.drones[drone_id].set_checked(new_state) + + # 更新所有分組勾選框狀態 + for socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + group_checkbox.blockSignals(True) + group_checkbox.setChecked(new_state) + group_checkbox.blockSignals(False) + + def handle_arm_selected(self): + """處理批次解鎖""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) + + def handle_takeoff_selected(self): + """處理批次起飛""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.takeoff_drone(drone_id, 10.0) + loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) + + def handle_batch_mode_change(self): + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) + + def update_field(self, panel, drone_id, field, text, color=None): + """更新面板中的指定欄位""" + if isinstance(panel, DronePanel): + panel.update_field(field, text, color) + + def update_overview_table(self, drone_id=None, field=None, value=None): + # Ensure the widget is available + if not hasattr(self, 'overview_table') or self.overview_table is None: + return + + # Update a specific cell in the overview table + if drone_id and field and value: + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) # Get row from field mapping + + if row == -1: + return # Invalid field, exit + + item = self.overview_table.item(row, col) + if not item: + item = QTableWidgetItem() + self.overview_table.setItem(row, col, item) + + item.setText(value) # Update the cell's text + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) # Center the text + + # If no specific update, refresh entire table + if drone_id is None: + cols = 1 + len(self.drones) + self.overview_table.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.overview_table.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.overview_table.setItem(row, col, val_item) + + def get_socket_id(self, drone_id): + """從 drone_id 提取 socket_id (例如 's9_1' -> '9')""" + import re + match = re.match(r's(\d+)_\d+', drone_id) + return match.group(1) if match else 'unknown' + + def add_drone(self, drone_id): + if drone_id in self.drones: + return + + # 獲取 socket_id + socket_id = self.get_socket_id(drone_id) + + # 創建無人機面板 + panel = self.create_drone_panel(drone_id) + self.drones[drone_id] = panel + + # 如果該 socket 分組不存在，創建它 + if socket_id not in self.socket_groups: + group_panel = self.create_socket_group_panel(socket_id) + self.socket_groups[socket_id] = group_panel + + # 將無人機面板添加到對應的 socket 分組中 + group_panel = self.socket_groups[socket_id] + group_panel.drones_layout.addWidget(panel) + + # 重新排序並顯示所有 socket 分組 + self.reorganize_socket_groups() + + # 更新分組勾選框狀態 + self.update_group_checkbox_state(socket_id) + + # 更新總覽表 + self.update_overview_table() + + def reorganize_socket_groups(self): + """重新組織和排序 socket 分組""" + # 先清空主佈局 + while self.drone_panel_layout.count(): + w = self.drone_panel_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對每個 socket 分組內的無人機進行排序 + for socket_id, group_panel in self.socket_groups.items(): + # 獲取該分組內的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 清空分組佈局 + while group_panel.drones_layout.count(): + w = group_panel.drones_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對該分組內的無人機按數字排序 + def sort_key(x): + parts = x[1:].split('_') + return (int(parts[0]), int(parts[1])) + + # 重新添加排序後的無人機面板 + for did in sorted(group_drones, key=sort_key): + group_panel.drones_layout.addWidget(self.drones[did]) + + # 按 socket_id 數字順序添加分組到主佈局 + for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)): + self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) + + def spin_ros(self): + try: + self.executor.spin_once(timeout_sec=0.01) + for (drone_id, msg_type), data in self.monitor.latest_data.items(): + self.monitor.signals.update_signal.emit(msg_type, drone_id, data) + self.monitor.latest_data.clear() + except Exception as e: + print(f"ROS spin error: {e}") + + def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + + self.monitor.destroy_node() + self.executor.shutdown() + rclpy.shutdown() + event.accept() + + # ================================================================================ + # 【新增】以下方法是任務規劃功能 + # ================================================================================ + + def handle_map_click(self, lat, lon): + """ + 處理地圖點擊事件，自動規劃隊形任務 + + Args: + lat: 緯度 + lon: 經度 + """ + print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}") + + selected_drones = self.get_selected_drones() + + if len(selected_drones) == 0: + self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) + return + + # 設定目標點 + base_alt = 10.0 # 基準高度 + target_gps = (lat, lon, base_alt) + + self.statusBar().showMessage(f"⏳ 正在規劃 {len(selected_drones)} 台無人機的任務...", 2000) + + try: + # 取得當前無人機位置（GPS 座標） + drone_gps_positions = [] + + for drone_id in selected_drones: + # 檢查是否有 GPS 資料 + if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps: + # 使用實際的 GPS 資料 + gps_data = self.monitor.drone_gps[drone_id] + lat_drone = gps_data['lat'] + lon_drone = gps_data['lon'] + alt_drone = gps_data.get('alt', base_alt) + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用實際 GPS: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + elif drone_id in self.drone_positions: + # 備用方案：從 Local 座標轉換 + pos = self.drone_positions[drone_id] + # 使用簡化轉換 + lat_drone = 24.0 + pos[1] / 111000 + lon_drone = 120.0 + pos[0] / (111000 * 0.9) + alt_drone = pos[2] if len(pos) > 2 else base_alt + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用 Local 轉換: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + else: + self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) + return + + # 規劃任務 + # ================================================================================ + # 【修改】接收中心點座標 + # ================================================================================ + stage1_gps, stage2_gps, center_origin = self.mission_planner.plan_formation_mission( + drone_gps_positions, + target_gps + ) + # ================================================================================ + + # 儲存規劃結果 + self.planned_waypoints = { + 'stage1': stage1_gps, + 'stage2': stage2_gps, + 'drone_ids': selected_drones + } + + # 顯示規劃結果 + self.show_planned_waypoints() + + # ================================================================================ + # 【新增】在地圖上顯示任務規劃（中心點 + 虛線） + # ================================================================================ + center_lat, center_lon, center_alt = center_origin + self.drone_map.draw_mission_plan( + center_lat, center_lon, # 中心點座標 + lat, lon # 目標點座標 + ) + # ================================================================================ + + self.statusBar().showMessage( + f"✓ 任務規劃完成！{len(selected_drones)} 台無人機，2 階段飛行", 5000 + ) + + except Exception as e: + self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) + print(f"Mission planning error: {e}") + import traceback + traceback.print_exc() + + def show_planned_waypoints(self): + """ + 顯示規劃的航點（在終端輸出） + """ + if not self.planned_waypoints: + return + + print("\n" + "=" * 60) + print("任務規劃結果") + print("=" * 60) + + drone_ids = self.planned_waypoints['drone_ids'] + stage1 = self.planned_waypoints['stage1'] + stage2 = self.planned_waypoints['stage2'] + + print(f"\n共 {len(drone_ids)} 台無人機") + print(f"參數：間距={self.mission_planner.spacing}m, " + f"基準高度={self.mission_planner.base_altitude}m, " + f"高低差={self.mission_planner.altitude_diff}m") + + for i, drone_id in enumerate(drone_ids): + print(f"\n【{drone_id}】") + lat1, lon1, alt1 = stage1[i] + lat2, lon2, alt2 = stage2[i] + + print(f" 階段 1（集合點）：") + print(f" 緯度: {lat1:.6f}°") + print(f" 經度: {lon1:.6f}°") + print(f" 高度: {alt1:.1f} m") + + print(f" 階段 2（目標點）：") + print(f" 緯度: {lat2:.6f}°") + print(f" 經度: {lon2:.6f}°") + print(f" 高度: {alt2:.1f} m") + + print("\n" + "=" * 60) + + def get_selected_drones(self): + """ + 取得被選中的無人機 ID 列表 + + Returns: + list: 被選中的無人機 ID 列表 + """ + selected = [] + for drone_id, panel in self.drones.items(): + if hasattr(panel, 'checkbox') and panel.checkbox.isChecked(): + selected.append(drone_id) + return selected + + # ================================================================================ + # 【新增結束】 + # ================================================================================ + +if __name__ == '__main__': + app = QApplication(sys.argv) + station = ControlStationUI() + station.show() + app.exec() \ No newline at end of file diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py new file mode 100644 index 0000000..cfb3ba0 --- /dev/null +++ b/src/GUI/map_layout.py @@ -0,0 +1,423 @@ +#!/usr/bin/env python3 +from PyQt6.QtWebEngineWidgets import QWebEngineView +from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot +from PyQt6.QtWebChannel import QWebChannel + +class DroneMap: + """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" + + def __init__(self): + """初始化地圖""" + self.map_view = QWebEngineView() + self.map_loaded = False + self.pending_map_updates = {} + + # 創建橋接對象 + self.bridge = MapBridge() + + # 設置 QWebChannel + self.channel = QWebChannel() + self.channel.registerObject('bridge', self.bridge) + self.map_view.page().setWebChannel(self.channel) + + # 設置地圖 HTML + inline_html = ''' + + + + + + + + + + + +
+
+ +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + # 批量執行所有待更新的位置 + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + # 清空待更新緩存 + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + # ================================================================================ + # 【新增】任務規劃視覺化方法 + # ================================================================================ + def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon): + """ + 在地圖上繪製任務規劃 + + Args: + center_lat: 中心點緯度 + center_lon: 中心點經度 + target_lat: 目標點緯度 + target_lon: 目標點經度 + """ + if self.map_loaded: + js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});" + self.map_view.page().runJavaScript(js_code) + print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") + + def clear_mission_plan(self): + """清除地圖上的任務規劃標記""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearMissionPlan();") + print("🗑️ 地圖已清除任務規劃") + # ================================================================================ + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) # lat, lon + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) \ No newline at end of file diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py new file mode 100644 index 0000000..138795d --- /dev/null +++ b/src/GUI/mission_planner.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python3 +""" +飛行任務規劃模組 +負責將 GPS 點擊座標轉換為隊形飛行任務 +""" +import math +from typing import List, Tuple, Optional + + +class CoordinateConverter: + """GPS 座標與 Local 座標的轉換器""" + + def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0): + """ + 初始化座標轉換器 + + Args: + origin_lat: 參考原點緯度 + origin_lon: 參考原點經度 + origin_alt: 參考原點高度（公尺，相對於海平面） + """ + self.origin_lat = origin_lat + self.origin_lon = origin_lon + self.origin_alt = origin_alt + self.R = 6371000.0 # 地球半徑（公尺） + + def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]: + """ + GPS 座標轉換為 Local 座標（ENU 系統：East-North-Up） + + Args: + lat: 緯度 + lon: 經度 + alt: 高度（公尺，相對於海平面） + + Returns: + (x, y, z): Local 座標（公尺） + x: East（東方） + y: North（北方） + z: Up（向上） + """ + lat_rad = math.radians(lat) + lon_rad = math.radians(lon) + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + dlat = lat_rad - origin_lat_rad + dlon = lon_rad - origin_lon_rad + + # 使用 Equirectangular projection（適用於小範圍 < 10 km） + x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2) # East + y = self.R * dlat # North + z = alt - self.origin_alt # Up + + return x, y, z + + def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]: + """ + Local 座標轉換為 GPS 座標 + + Args: + x: East（東方，公尺） + y: North（北方，公尺） + z: Up（向上，公尺） + + Returns: + (lat, lon, alt): GPS 座標 + """ + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + lat_rad = origin_lat_rad + (y / self.R) + lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2))) + + lat = math.degrees(lat_rad) + lon = math.degrees(lon_rad) + alt = self.origin_alt + z + + return lat, lon, alt + + +class FormationPlanner: + """隊形規劃器""" + + def __init__(self, spacing: float = 5.0, + base_altitude: float = 10.0, + altitude_diff: float = 2.0): + """ + 初始化隊形規劃器 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺，相對於參考原點） + altitude_diff: M 字形的高低差（公尺） + """ + self.spacing = spacing + self.base_altitude = base_altitude + self.altitude_diff = altitude_diff + self.current_origin = None + self.converter = None + + def plan_formation_mission(self, + drone_gps_positions: List[Tuple[float, float, float]], + target_gps: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 規劃兩階段隊形任務 + + Args: + drone_gps_positions: 當前無人機 GPS 位置列表 [(lat, lon, alt), ...] + target_gps: 目標點 GPS 座標 (lat, lon, alt) + + Returns: + stage1_gps: 階段 1（集合點）的 GPS 航點列表 + stage2_gps: 階段 2（目標點）的 GPS 航點列表 + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + """ + if len(drone_gps_positions) == 0: + raise ValueError("無人機位置列表不能為空") + + # ===== 步驟 1: 計算中央點並設為參考原點 ===== + center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) + center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) + center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) + + self.current_origin = (center_lat, center_lon, center_alt) + self.converter = CoordinateConverter(center_lat, center_lon, center_alt) + + print(f"📍 參考原點: ({center_lat:.6f}, {center_lon:.6f}, {center_alt:.1f}m)") + + # ===== 步驟 2: 轉換到 Local 座標系 ===== + drone_local_positions = [] + for lat, lon, alt in drone_gps_positions: + x, y, z = self.converter.gps_to_local(lat, lon, alt) + drone_local_positions.append((x, y, z)) + + target_local = self.converter.gps_to_local( + target_gps[0], target_gps[1], target_gps[2] + ) + + # ===== 步驟 3: 在 Local 座標系中計算隊形 ===== + stage1_local, stage2_local = self._calculate_formation_local( + drone_local_positions, + target_local + ) + + # ===== 步驟 4: 轉回 GPS 座標 ===== + stage1_gps = [] + for x, y, z in stage1_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage1_gps.append((lat, lon, alt)) + + stage2_gps = [] + for x, y, z in stage2_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage2_gps.append((lat, lon, alt)) + + print(f"✅ 任務規劃完成: {len(stage1_gps)} 台無人機，2 階段飛行") + + # ================================================================================ + # 【修改】回傳中心點座標供地圖視覺化使用 + # ================================================================================ + return stage1_gps, stage2_gps, self.current_origin + # ================================================================================ + + def _calculate_formation_local(self, + drone_positions: List[Tuple[float, float, float]], + target_point: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 在 Local 座標系中計算隊形 + + Args: + drone_positions: Local 座標的無人機位置 [(x, y, z), ...] + target_point: Local 座標的目標點 (x, y, z) + + Returns: + stage1_positions: 階段 1（中央點分佈）的 Local 座標 + stage2_positions: 階段 2（目標點分佈）的 Local 座標 + """ + N = len(drone_positions) + + # ===== 步驟 1: 計算中央點（在 Local 座標系中應該接近原點）===== + C_x = sum(pos[0] for pos in drone_positions) / N + C_y = sum(pos[1] for pos in drone_positions) / N + C_z = sum(pos[2] for pos in drone_positions) / N + + print(f" 中央點 Local: ({C_x:.2f}, {C_y:.2f}, {C_z:.2f})") + + # ===== 步驟 2: 計算方向向量（中央點 → 目標點）===== + T_x, T_y, T_z = target_point + V_x = T_x - C_x + V_y = T_y - C_y + + print(f" 方向向量: ({V_x:.2f}, {V_y:.2f})") + + # ===== 步驟 3: 計算垂直向量（逆時針旋轉 90°）===== + P_x = -V_y + P_y = V_x + + # 單位化垂直向量 + length = math.sqrt(P_x**2 + P_y**2) + if length < 0.01: # 避免除以零 + # 如果目標點與中央點重合，使用默認方向（向東） + P_x_unit, P_y_unit = 1.0, 0.0 + else: + P_x_unit = P_x / length + P_y_unit = P_y / length + + print(f" 垂直單位向量: ({P_x_unit:.3f}, {P_y_unit:.3f})") + + # ===== 步驟 4: 計算無人機在垂直方向的投影並排序 ===== + projections = [] + for i, pos in enumerate(drone_positions): + relative_x = pos[0] - C_x + relative_y = pos[1] - C_y + projection = relative_x * P_x_unit + relative_y * P_y_unit + projections.append((projection, i)) + + # 按投影值排序（保持左右順序） + projections.sort() + sorted_indices = [idx for _, idx in projections] + + print(f" 排序後的無人機索引: {sorted_indices}") + + # ===== 步驟 5: 分佈無人機位置 ===== + stage1_positions = [None] * N # 預分配列表 + stage2_positions = [None] * N + + for rank, original_idx in enumerate(sorted_indices): + # 計算相對中心的水平偏移 + offset = (rank - (N - 1) / 2) * self.spacing + + # 計算 M 字形高度（交替高低） + if rank % 2 == 0: + altitude = self.base_altitude + self.altitude_diff # 高 + else: + altitude = self.base_altitude - self.altitude_diff # 低 + + # 階段 1：在中央點附近分佈 + stage1_x = C_x + P_x_unit * offset + stage1_y = C_y + P_y_unit * offset + stage1_z = altitude + + # 階段 2：在目標點附近分佈（保持相同的相對位置） + stage2_x = T_x + P_x_unit * offset + stage2_y = T_y + P_y_unit * offset + stage2_z = altitude + + # 按照原始索引存儲（保持無人機 ID 順序） + stage1_positions[original_idx] = (stage1_x, stage1_y, stage1_z) + stage2_positions[original_idx] = (stage2_x, stage2_y, stage2_z) + + return stage1_positions, stage2_positions + + def update_parameters(self, spacing: Optional[float] = None, + base_altitude: Optional[float] = None, + altitude_diff: Optional[float] = None): + """ + 更新隊形參數 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺） + altitude_diff: M 字形的高低差（公尺） + """ + if spacing is not None: + self.spacing = spacing + print(f" 間距更新為: {spacing} m") + + if base_altitude is not None: + self.base_altitude = base_altitude + print(f" 基準高度更新為: {base_altitude} m") + + if altitude_diff is not None: + self.altitude_diff = altitude_diff + print(f" 高低差更新為: {altitude_diff} m") + + +# ===== 測試程式碼 ===== +if __name__ == "__main__": + print("=" * 60) + print("隊形任務規劃器測試") + print("=" * 60) + + # 模擬 5 台無人機的 GPS 位置 + drone_gps = [ + (24.123450, 120.567800, 100.0), # 無人機 0 + (24.123470, 120.567820, 102.0), # 無人機 1 + (24.123440, 120.567810, 98.0), # 無人機 2 + (24.123460, 120.567830, 101.0), # 無人機 3 + (24.123445, 120.567795, 99.0), # 無人機 4 + ] + + # 目標點 + target_gps = (24.123600, 120.568000, 105.0) + + # 建立規劃器 + planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + + # 規劃任務 + print("\n開始規劃任務...") + stage1, stage2 = planner.plan_formation_mission(drone_gps, target_gps) + + # 顯示結果 + print("\n" + "=" * 60) + print("階段 1：集合點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage1): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n" + "=" * 60) + print("階段 2：目標點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage2): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n✅ 測試完成！") \ No newline at end of file diff --git a/src/external/angles b/src/external/angles new file mode 160000 index 0000000..a96224f --- /dev/null +++ b/src/external/angles @@ -0,0 +1 @@ +Subproject commit a96224f9ab3ac51fe8fd981c1e1554528dc4345a diff --git a/src/external/geographic_info b/src/external/geographic_info new file mode 160000 index 0000000..24806ad --- /dev/null +++ b/src/external/geographic_info @@ -0,0 +1 @@ +Subproject commit 24806adc767414eb3a34a58aefeb648ee415b09a From 3d48b1d9fe4f7b855f917b2890cbbb0b947928e8 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 29 Jan 2026 14:23:36 +0800 Subject: [PATCH 096/146] =?UTF-8?q?(fix)=201.=20=E6=8A=8A=20bridge=20?= =?UTF-8?q?=E7=9A=84=E7=A8=AE=E9=A1=9E=E8=A3=9C=E4=B8=8A=E5=8F=AF=E4=BB=A5?= =?UTF-8?q?=E8=B7=9F=20ros2=20=E6=90=AD=E9=85=8D=202.=20=E4=B8=80=E4=BA=9B?= =?UTF-8?q?=20ros2=20service=20=E7=9A=84=E5=9F=BA=E5=BA=95=20=E4=BB=8D?= =?UTF-8?q?=E5=9C=A8=E9=96=8B=E7=99=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/fc_network_adapter.md | 20 +- .../fc_network_adapter/mainOrchestrator.py | 2 +- .../fc_network_adapter/mavlinkObject.py | 2 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 306 +++++++++++++++++- .../fc_network_adapter/mavlinkVehicleView.py | 4 +- src/unitdev02/unitdev02/devnote.txt | 35 ++ src/unitdev02/unitdev02/sslChech.sh | 27 ++ 7 files changed, 381 insertions(+), 15 deletions(-) create mode 100644 src/unitdev02/unitdev02/devnote.txt create mode 100644 src/unitdev02/unitdev02/sslChech.sh diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md index ace76ac..efd48cd 100644 --- a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -3,8 +3,23 @@ 只會挑出重要的變數與方法描述 以利後續開發使用 +# 開發此專案的注意事項 +- 預設 autopilot 的 component id = 1 +- 不允許 system id 重複 +- 增加一個固定數值監控然後要到 ros2 topic + - mavlinkROS2Node.py 檔案內 + - PublishRateController.topic_intervals 建立 + - VehicleStatusPublisher._publish_vehicle_status 登記 + - VehicleStatusPublisher._publish_XXX 實作 + - mavlinkObject.py 檔案內 + - mavlink_bridge.message_handlers 登記 + - mavlink_bridge._handle_XXX 實作 + - mavlink_object.bridge_msg_types 登記 (這個可以用介面調) + - mavlinkVehicleView.py 檔案內 + - 注意對應的資料存放區 +--- # 檔案結構 特別注意: @@ -40,7 +55,7 @@ --- 關於載具管理與檢視 - *_update_vehicles_list()* -- *_prepare_vehicle_info* +- *_prepare_vehicle_info()* --- 關於 serial_manager 控制實現 - *create_serial_port_object()* @@ -167,8 +182,9 @@ 7. 終端機介面控制 8. 基礎載具流量觀測 9. 載具狀態收集與彙整 +10. a. ros2 topic 應用開發介面 ### 待開發功能 5-1. 建立 serial 連線 並可以對接收器下達AT指令 5-2. 模組化 serial 連線機制 以利後期擴容其他模組 -10-1. ros2 應用開發介面 \ No newline at end of file +10. a. ros2 應用開發介面 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 2078490..ae89d72 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -VERSION_NO = "v0.58" +VERSION_NO = "v0.59" class PanelState: def __init__(self): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 82dccce..c8cfaed 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -367,7 +367,7 @@ class mavlink_object: self.outgoing_msgs = deque() # 記錄訊息過濾類型 (可選) - self.bridge_msg_types = set([0, 30]) # 0 HEARTBEAT, 30 ATTITUDE, ... + self.bridge_msg_types = set([0, 30, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS self.return_msg_types = set() # 轉發到別的 mavlink object 作為目標端口 的列表 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 97ff5d0..a95fb26 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -39,6 +39,11 @@ class PublishRateController: """發布頻率控制器 - 按時間間隔控制發布頻率""" def __init__(self): + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 1/4】 + # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔 + # 例如：'ekf_status': 1.0, # EKF 狀態 + # ═══════════════════════════════════════════════════════════════ # 各 topic 的發布間隔（秒） self.topic_intervals = { 'position': 0.5, # GPS位置 @@ -48,6 +53,7 @@ class PublishRateController: 'vfr_hud': 0.5, # VFR HUD 'mode': 1.0, # 飛行模式 'summary': 1.0, # 載具摘要 + # 在這裡新增更多 topics... } # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} self.last_publish_time: Dict[tuple, float] = {} @@ -127,7 +133,7 @@ class VehicleStatusPublisher(Node): """定時器回調 - 檢查所有載具並發布狀態""" if not self.running: return - + # 從 vehicle_registry 獲取所有載具 all_vehicles = mvv.vehicle_registry.get_all() @@ -150,6 +156,11 @@ class VehicleStatusPublisher(Node): status = component.status + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 2/4】 + # 若要新增 topic，請在此處調用對應的發布方法 + # 例如：self._publish_ekf_status(sysid, status) + # ═══════════════════════════════════════════════════════════════ # 發布各種狀態（通過頻率控制器判斷是否發布） self._publish_position(sysid, status) self._publish_attitude(sysid, status) @@ -158,6 +169,7 @@ class VehicleStatusPublisher(Node): self._publish_vfr_hud(sysid, status) self._publish_mode(sysid, status) self._publish_summary(vehicle) + # 在這裡新增更多 publish 方法調用... def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): """ @@ -372,12 +384,12 @@ class VehicleStatusPublisher(Node): 'autopilot': component.mav_autopilot if component.mav_autopilot else 0, 'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！ 'armed': status.armed if status.armed is not None else False, - 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, + # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN", - 'latitude': status.position.latitude if status.position.latitude else 0.0, - 'longitude': status.position.longitude if status.position.longitude else 0.0, - 'altitude': status.position.altitude if status.position.altitude else 0.0, - 'battery_percent': status.battery.remaining if status.battery.remaining else 0, + # 'latitude': status.position.latitude if status.position.latitude else 0.0, + # 'longitude': status.position.longitude if status.position.longitude else 0.0, + # 'altitude': status.position.altitude if status.position.altitude else 0.0, + # 'battery_percent': status.battery.remaining if status.battery.remaining else 0, 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0, 'connection_type': vehicle.connected_via.value, 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, @@ -387,6 +399,24 @@ class VehicleStatusPublisher(Node): msg.data = json.dumps(summary) publisher.publish(msg) + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 3/4】 + # 若要新增 topic，請在此處實作對應的發布方法 + # 方法命名規則：def _publish_(self, sysid: int, status: mvv.ComponentStatus): + # 例如： + # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus): + # """發布 EKF 狀態""" + # if not self.rate_controller.should_publish(sysid, 'ekf_status'): + # return + # + # ekf = status.ekf + # if ekf.flags is None: + # return + # + # publisher = self._get_or_create_publisher(sysid, 'ekf_status', ... + # # ... 實作發布邏輯 + # ═══════════════════════════════════════════════════════════════ + @staticmethod def _euler_to_quaternion(roll, pitch, yaw): """ @@ -430,17 +460,275 @@ class MavlinkCommandService(Node): - 調用 mavlinkObject 發送封包 - 處理 ACK 等待和超時（未來實現） - TODO: 稍後實現 + 設計理念：回歸 MAVLink 純粹結構 + - 只負責將 ROS2 請求轉換為 MAVLink 封包 + - 不預設功能（如 ARM/DISARM），保持模組化 + - 高層應用可透過此 service 實現各種功能 """ def __init__(self): super().__init__('mavlink_command_service') - logger.info("MavlinkCommandService initialized (not implemented yet)") + # ═══════════════════════════════════════════════════════════════════ + # ROS2 Service 架構說明： + # + # 1. Service 定義：由 .srv 檔案定義（Request + Response） + # - Request: client 發送的請求內容 + # - Response: server 回傳的結果 + # + # 2. Service Server 創建： + # self.create_service(srv_type, service_name, callback_function) + # - srv_type: service 的訊息類型（需要自定義或使用標準） + # - service_name: service 的名稱（client 用此名稱呼叫） + # - callback_function: 處理請求的回調函數 + # + # 3. Callback 函數： + # def callback(self, request, response): + # # request: 包含 client 發送的數據 + # # response: 需要填充並返回給 client + # return response + # + # 4. Service Client 使用方式（在其他程式中）： + # client = node.create_client(srv_type, service_name) + # request = srv_type.Request() + # future = client.call_async(request) # 異步調用 + # # 或 response = client.call(request) # 同步調用 + # ═══════════════════════════════════════════════════════════════════ + + # 由於 ROS2 自定義 service 需要 .srv 檔案編譯 + # 這裡先使用標準 String service 作為簡化實現 + # TODO: 未來可創建專門的 .srv 檔案 + from std_srvs.srv import Trigger + from example_interfaces.srv import SetBool + + # ═══════════════════════════════════════════════════════════════════ + # Service 1: 發送 MAVLink Message（通用介面） + # 使用 Trigger 作為臨時實現，未來應使用自定義 service + # ═══════════════════════════════════════════════════════════════════ + # TODO: 創建 SendMavlinkMessage.srv + # Request: + # uint8 target_sysid + # uint8 target_compid + # uint16 message_id + # string fields_json # JSON 格式的字段數據 + # bool wait_response + # uint16 response_msgid + # float32 timeout + # Response: + # bool success + # string response_json + # string error_message + + # 暫時使用簡化版本（僅示範架構） + self.srv_send_message = self.create_service( + Trigger, + '/mavlink/send_message', + self.handle_send_message + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 2: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════ + self.srv_command_long = self.create_service( + Trigger, + '/mavlink/send_command_long', + self.handle_command_long + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 3: 參數請求 + # ═══════════════════════════════════════════════════════════════════ + self.srv_param_request = self.create_service( + Trigger, + '/mavlink/param_request', + self.handle_param_request + ) + + # 狀態標記 + self.running = True + + # mavlinkObject 的引用（將由外部設置） + self.mavlink_analyzer = None + + logger.info("MavlinkCommandService initialized") + + def set_mavlink_analyzer(self, mavlink_analyzer): + """ + 設置 mavlink_analyzer 引用 + + Args: + mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例 + """ + self.mavlink_analyzer = mavlink_analyzer + logger.info("MavlinkCommandService: mavlink_analyzer set") + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 MAVLink Message + # ═══════════════════════════════════════════════════════════════════════ + def handle_send_message(self, request, response): + """ + 處理發送 MAVLink 訊息的請求 + + ROS2 Service Callback 說明： + - 此函數會在 client 調用 service 時被執行 + - request: 包含 client 傳入的參數 + - response: 需要填充結果並返回給 client + - 必須 return response + + Args: + request: Trigger.Request (暫時使用，未來改為自定義) + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received send_message request") + + # 檢查 mavlink_analyzer 是否已設置 + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + logger.error(response.message) + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數（target_sysid, message_id, fields 等） + # 2. 使用 pymavlink 組裝 MAVLink 封包 + # 3. 調用 mavlink_analyzer.send_message() 發送 + # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應 + + # 暫時返回成功（示範用） + response.success = True + response.message = "Message sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════════ + def handle_command_long(self, request, response): + """ + 處理發送 COMMAND_LONG 的請求 + + COMMAND_LONG (MAVLink message ID=76): + - 用於發送簡單命令給載具 + - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等 + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received command_long request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析 COMMAND_LONG 參數 + # - target_sysid, target_compid + # - command (MAV_CMD_xxx) + # - param1~param7 + # 2. 組裝 COMMAND_LONG 封包 + # 3. 發送並等待 COMMAND_ACK (message ID=77) + # 4. 解析 ACK 結果（ACCEPTED/FAILED 等） + + response.success = True + response.message = "Command sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 參數請求 + # ═══════════════════════════════════════════════════════════════════════ + def handle_param_request(self, request, response): + """ + 處理參數讀取請求 + + MAVLink 參數協議： + - PARAM_REQUEST_READ (ID=20): 請求讀取參數 + - PARAM_VALUE (ID=22): 參數值回應 + - PARAM_SET (ID=23): 設置參數值 + + ═══════════════════════════════════════════════════════════════════ + 【使用 mavlinkObject 回應機制的步驟】 + + 1. 設置回應訊息類型： + self.mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + + 2. 發送請求封包： + message_bytes = ... # 組裝 PARAM_REQUEST_READ + self.mavlink_analyzer.send_message( + message_bytes, + target_sysid=1 + ) + + 3. 監聽回應（在獨立線程或定時器中）： + from ..fc_network_adapter import mavlinkObject as mo + + # 等待回應（帶超時） + timeout = 3.0 + start_time = time.time() + while time.time() - start_time < timeout: + items = mo.return_packet_ring.get_all() + for socket_id, timestamp, msg in items: + if msg.get_type() == 'PARAM_VALUE': + # 找到回應！ + param_id = msg.param_id + param_value = msg.param_value + # 處理回應... + return + time.sleep(0.01) # 短暫等待 + + # 超時處理 + + 4. 清理（可選）： + self.mavlink_analyzer.set_return_message_types([]) # 清空 + mo.return_packet_ring.clear() # 清空緩衝區 + + 注意事項： + - return_packet_ring 是全域的，所有 mavlink_object 共用 + - 需要通過 socket_id 或 sysid 來識別回應來源 + - 實際使用時建議實現專門的回應管理器 + ═══════════════════════════════════════════════════════════════════ + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received param_request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數名稱或索引 + # 2. 設置 mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + # 3. 發送 PARAM_REQUEST_READ + # 4. 監聽 return_packet_ring，等待 PARAM_VALUE + # 5. 解析回應並填充到 response + + response.success = True + response.message = "Param request sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # 【新增 Service 位置 4/4】 + # 若要新增 service，請在此處添加新的 handler 方法 + # 並在 __init__ 中創建對應的 service server + # ═══════════════════════════════════════════════════════════════════════ def stop(self): """停止服務""" - # logger.info("MavlinkCommandService stopped") + self.running = False + logger.info("MavlinkCommandService stopped") # ============================================================================ diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index aeae274..b924c42 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -323,7 +323,7 @@ class VehicleView: """ if component_id not in self.components: self.components[component_id] = VehicleComponent(component_id, comp_type) - # logger.info(f"Added component {component_id} to system {self.sysid}") + # logger.debug(f"Added component {component_id} to system {self.sysid}") return self.components[component_id] def get_component(self, component_id: int) -> Optional[VehicleComponent]: @@ -334,7 +334,7 @@ class VehicleView: """移除組件""" if component_id in self.components: del self.components[component_id] - # logger.info(f"Removed component {component_id} from system {self.sysid}") + # logger.debug(f"Removed component {component_id} from system {self.sysid}") return True return False diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt new file mode 100644 index 0000000..e555f59 --- /dev/null +++ b/src/unitdev02/unitdev02/devnote.txt @@ -0,0 +1,35 @@ +備選需要的功能 + - serail 對於 telemetry 的支援 + - serial_manager.serial_object.transport 這些變數可能不需要 + +不用動 + - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async + - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) + +這一步 + 研究 ros2 service + +下一步 + +下下一步 + + +後面 + rssi 資訊提取s + + + +自己的常用指令 +python -m fc_network_adapter.tests.test_vehicleStatusPublisher +python -m fc_network_adapter.tests.test_ringBuffer + +ros2 topic list +ros2 topic echo + + +/home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7 + +mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560 + + + diff --git a/src/unitdev02/unitdev02/sslChech.sh b/src/unitdev02/unitdev02/sslChech.sh new file mode 100644 index 0000000..9f38904 --- /dev/null +++ b/src/unitdev02/unitdev02/sslChech.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# 網站清單 +DOMAINS=("google.com" "smarter.nchu.edu.tw") + +echo "網站 SSL 憑證剩餘天數：" +echo "---------------------------" + +for domain in "${DOMAINS[@]}"; do + end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null | + openssl x509 -noout -enddate | cut -d= -f2) + + end_timestamp=$(date -d "$end_date" +%s) + now_timestamp=$(date +%s) + + remaining_days=$(( (end_timestamp - now_timestamp) / 86400 )) + + if [ $remaining_days -lt 0 ]; then + status="已過期 ❌" + elif [ $remaining_days -lt 15 ]; then + status="即將到期 ⚠️" + else + status="正常 ✅" + fi + + printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status" +done From 9f1235197a6da568dd19797cf69040484024c45c Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 01:04:37 +0800 Subject: [PATCH 097/146] Add/Update GUI module from wenchun --- src/GUI/comm_panel.py | 398 ++++++++++++++ src/GUI/communication.py | 662 +++++++++++++++++++++++ src/GUI/drone_panel.py | 378 ++++++++++++++ src/GUI/gui.py | 1009 ++++++++++++++++++++++++++++++++++++ src/GUI/map_layout.py | 436 ++++++++++++++++ src/GUI/mission_planner.py | 323 ++++++++++++ src/GUI/overview_table.py | 89 ++++ 7 files changed, 3295 insertions(+) create mode 100644 src/GUI/comm_panel.py create mode 100644 src/GUI/communication.py create mode 100644 src/GUI/drone_panel.py create mode 100644 src/GUI/gui.py create mode 100644 src/GUI/map_layout.py create mode 100644 src/GUI/mission_planner.py create mode 100644 src/GUI/overview_table.py diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py new file mode 100644 index 0000000..82b539e --- /dev/null +++ b/src/GUI/comm_panel.py @@ -0,0 +1,398 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QLineEdit) +from PyQt6.QtCore import pyqtSignal + +class CommPanel(QWidget): + """通讯设置面板类""" + + # 定义信号 + udp_connection_added = pyqtSignal(str, int) # ip, port + udp_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + udp_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + ws_connection_added = pyqtSignal(str) # url + ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + status_message = pyqtSignal(str, int) # message, timeout + + def __init__(self, parent=None): + super().__init__(parent) + self.udp_connections = [] + self.ws_connections = [] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(10) + + # ========== UDP MAVLink 區域 ========== + udp_title = QLabel("UDP") + udp_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(udp_title) + + # UDP 連接列表容器 + self.udp_list_widget = QWidget() + self.udp_list_layout = QVBoxLayout(self.udp_list_widget) + self.udp_list_layout.setContentsMargins(0, 0, 0, 0) + self.udp_list_layout.setSpacing(5) + layout.addWidget(self.udp_list_widget) + + # UDP 添加新連接區域 + add_udp_widget = QWidget() + add_udp_layout = QHBoxLayout(add_udp_widget) + add_udp_layout.setContentsMargins(0, 0, 0, 0) + + self.udp_ip_input = QLineEdit() + self.udp_ip_input.setText("127.0.0.1") + self.udp_ip_input.setPlaceholderText("IP") + self.udp_ip_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + self.udp_port_input = QLineEdit() + self.udp_port_input.setText("14550") + self.udp_port_input.setPlaceholderText("Port") + self.udp_port_input.setFixedWidth(80) + self.udp_port_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_udp_btn = QPushButton("添加") + add_udp_btn.clicked.connect(self._handle_add_udp) + add_udp_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_udp_layout.addWidget(QLabel("IP:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_ip_input) + add_udp_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_udp_layout.addWidget(self.udp_port_input) + add_udp_layout.addWidget(add_udp_btn) + + layout.addWidget(add_udp_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + + # ========== WebSocket 區域 ========== + ws_title = QLabel("WebSocket") + ws_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(ws_title) + + # WebSocket 連接列表容器 + self.ws_list_widget = QWidget() + self.ws_list_layout = QVBoxLayout(self.ws_list_widget) + self.ws_list_layout.setContentsMargins(0, 0, 0, 0) + self.ws_list_layout.setSpacing(5) + layout.addWidget(self.ws_list_widget) + + # WebSocket 添加新連接區域 + add_ws_widget = QWidget() + add_ws_layout = QHBoxLayout(add_ws_widget) + add_ws_layout.setContentsMargins(0, 0, 0, 0) + + self.ws_url_input = QLineEdit() + self.ws_url_input.setPlaceholderText("host") + self.ws_url_input.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + """) + + add_ws_btn = QPushButton("添加") + add_ws_btn.clicked.connect(self._handle_add_ws) + add_ws_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_ws_layout.addWidget(QLabel("URL:", styleSheet="color: #DDD;")) + add_ws_layout.addWidget(self.ws_url_input) + add_ws_layout.addWidget(add_ws_btn) + + layout.addWidget(add_ws_widget) + layout.addStretch() + + def _handle_add_udp(self): + """處理添加 UDP 連接""" + ip = self.udp_ip_input.text().strip() + port_text = self.udp_port_input.text().strip() + + if not ip or not port_text: + self.status_message.emit("請輸入 IP 和 Port", 3000) + return + + try: + port = int(port_text) + if port < 1 or port > 65535: + raise ValueError("Port 超出範圍") + except ValueError: + self.status_message.emit("Port 必須是 1-65535 的數字", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.udp_connections: + if conn['ip'] == ip and conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.udp_connection_added.emit(ip, port) + + # 清空輸入框 + self.udp_ip_input.clear() + self.udp_port_input.clear() + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + + def create_udp_connection_panel(self, conn): + """創建 UDP 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['ip']}:{conn['port']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.udp_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.udp_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def create_ws_connection_panel(self, conn): + """創建 WebSocket 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['url']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.ws_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.ws_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_udp_panel(self, conn): + """添加 UDP 連接面板到列表""" + panel = self.create_udp_connection_panel(conn) + self.udp_list_layout.addWidget(panel) + self.udp_connections.append(conn) + return panel + + def add_ws_panel(self, conn): + """添加 WebSocket 連接面板到列表""" + panel = self.create_ws_connection_panel(conn) + self.ws_list_layout.addWidget(panel) + self.ws_connections.append(conn) + return panel + + def remove_udp_connection_from_list(self, conn): + """從列表中移除 UDP 連接""" + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + def remove_ws_connection_from_list(self, conn): + """從列表中移除 WebSocket 連接""" + if conn in self.ws_connections: + self.ws_connections.remove(conn) \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py new file mode 100644 index 0000000..45c960c --- /dev/null +++ b/src/GUI/communication.py @@ -0,0 +1,662 @@ +from rclpy.node import Node +from PyQt6.QtCore import QObject, pyqtSignal +import math +import re +import threading +from threading import Lock +import asyncio +import websockets +import json +import socket +from pymavlink import mavutil +from geometry_msgs.msg import Point, Vector3 +from sensor_msgs.msg import BatteryState, NavSatFix, Imu +from std_msgs.msg import Float64 +from mavros_msgs.msg import State, VfrHud +from mavros_msgs.srv import CommandBool, CommandTOL + +class DroneSignals(QObject): + update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) + +class UDPMavlinkReceiver(threading.Thread): + """UDP MAVLink 接收器""" + def __init__(self, ip, port, signals, connection_name): + super().__init__(daemon=True) + self.ip = ip + self.port = port + self.signals = signals + self.connection_name = connection_name + self.running = False + self.sock = None + + def run(self): + """執行 UDP 接收循環""" + self.running = True + try: + print(f"UDP MAVLink receiver started on {self.ip}:{self.port}") + + # 創建 MAVLink 連接 + mav = mavutil.mavlink_connection(f'udpin:{self.ip}:{self.port}') + while self.running: + try: + msg = mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except socket.timeout: + continue + except Exception as e: + print(f"Error receiving MAVLink message: {e}") + + except Exception as e: + print(f"UDP receiver error: {e}") + finally: + if self.sock: + self.sock.close() + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class SerialMavlinkReceiver(threading.Thread): + """串口 MAVLink 接收器""" + def __init__(self, port, baudrate, signals, connection_name): + super().__init__(daemon=True) + self.port = port + self.baudrate = baudrate + self.signals = signals + self.connection_name = connection_name + self.running = False + self.mav = None + + def run(self): + """執行串口接收循環""" + self.running = True + try: + print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") + + # 創建 MAVLink 串口連接 + self.mav = mavutil.mavlink_connection( + self.port, + baud=self.baudrate, + source_system=255 + ) + + print(f"Waiting for heartbeat from {self.port}...") + self.mav.wait_heartbeat() + print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}") + + while self.running: + try: + msg = self.mav.recv_match(blocking=True, timeout=1.0) + if msg is None: + continue + + self.process_mavlink_message(msg) + + except Exception as e: + if self.running: + print(f"Error receiving MAVLink message from serial: {e}") + + except Exception as e: + print(f"Serial receiver error: {e}") + finally: + if self.mav: + try: + self.mav.close() + except: + pass + + def process_mavlink_message(self, msg): + """處理 MAVLink 訊息""" + try: + msg_type = msg.get_type() + system_id = msg.get_srcSystem() + drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + + if msg_type == "HEARTBEAT": + mode = mavutil.mode_string_v10(msg) + armed = bool(msg.base_mode & 128) + self.signals.update_signal.emit('state', drone_id, { + 'mode': mode, + 'armed': armed + }) + + elif msg_type == "BATTERY_STATUS": + voltage = msg.voltages[0] / 1000 + self.signals.update_signal.emit('battery', drone_id, { + 'voltage': voltage + }) + + elif msg_type == "GLOBAL_POSITION_INT": + latitude = msg.lat / 1e7 + longitude = msg.lon / 1e7 + self.signals.update_signal.emit('gps', drone_id, { + 'lat': latitude, + 'lon': longitude + }) + + elif msg_type == "GPS_RAW_INT": + fix_type = msg.fix_type + + elif msg_type == "LOCAL_POSITION_NED": + x = msg.y + y = msg.x + z = -msg.z + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': x, + 'y': y, + 'z': z + }) + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': z + }) + self.signals.update_signal.emit('velocity', drone_id, { + 'vx': msg.vx, + 'vy': msg.vy, + 'vz': msg.vz + }) + + elif msg_type == "ATTITUDE": + pitch = math.degrees(msg.pitch) + self.signals.update_signal.emit('attitude', drone_id, { + 'pitch': pitch, + 'roll': 0, + 'yaw': 0, + 'rates': (0, 0, 0) + }) + + elif msg_type == "VFR_HUD": + groundspeed = msg.groundspeed + heading = msg.heading + self.signals.update_signal.emit('hud', drone_id, { + 'heading': heading, + 'groundspeed': groundspeed + }) + + except Exception as e: + print(f"Error processing MAVLink message from serial: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class WebSocketMavlinkReceiver(threading.Thread): + """WebSocket MAVLink 接收器""" + def __init__(self, url, signals, connection_name): + super().__init__(daemon=True) + self.url = url + self.signals = signals + self.connection_name = connection_name + self.running = False + self.max_retries = 5 + self.base_delay = 1.0 + + def run(self): + """執行 WebSocket 接收循環""" + self.running = True + asyncio.set_event_loop(asyncio.new_event_loop()) + asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + + async def ws_client_loop(self): + """WebSocket 連接的主循環""" + retry_count = 0 + + print(f"Starting WebSocket client for {self.connection_name} at {self.url}") + + while self.running and retry_count < self.max_retries: + try: + async with websockets.connect(self.url) as websocket: + print(f"WebSocket {self.connection_name} connected to {self.url}") + retry_count = 0 # 重置重試計數 + + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + + except websockets.exceptions.ConnectionClosedError: + print(f"WebSocket {self.connection_name} connection closed") + if self.running: + retry_count += 1 + if retry_count < self.max_retries: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"Reconnecting in {delay}s...") + await asyncio.sleep(delay) + else: + break + except Exception as e: + retry_count += 1 + if retry_count < self.max_retries and self.running: + delay = self.base_delay * (2 ** min(retry_count, 4)) + print(f"WebSocket {self.connection_name} connection error: {e}, retrying in {delay}s (attempt {retry_count}/{self.max_retries})") + await asyncio.sleep(delay) + else: + break + + print(f"WebSocket client {self.connection_name} stopped") + + def process_websocket_message(self, data): + """處理 WebSocket 訊息""" + try: + drone_id = data.get('system_id') + drone_id = f"s9_{drone_id}" if drone_id else None + if not drone_id: + return + + # 模式 + if 'mode' in data: + self.signals.update_signal.emit('state', drone_id, { + 'mode': data['mode'], + }) + + # 電池 + if 'battery' in data: + self.signals.update_signal.emit('battery', drone_id, { + 'percentage': data['battery'] + }) + + # 位置 + if 'position' in data: + pos = data['position'] + self.signals.update_signal.emit('gps', drone_id, { + 'lat': pos.get('lat', 0), + 'lon': pos.get('lon', 0) + }) + + # Local position - 設定 x, y 為 0.0 + self.signals.update_signal.emit('local_pose', drone_id, { + 'x': 0.0, + 'y': 0.0, + 'z': 0.0 + }) + + # Altitude - 設定為 0.0 + self.signals.update_signal.emit('altitude', drone_id, { + 'altitude': 0.0 + }) + + # 航向 + if 'heading' in data: + self.signals.update_signal.emit('hud', drone_id, { + 'heading': data['heading'], + 'groundspeed': 0.0 + }) + + except Exception as e: + print(f"WebSocket message processing error: {e}") + + def stop(self): + """停止接收器""" + self.running = False + +class DroneMonitor(Node): + def __init__(self): + super().__init__('drone_monitor') + self.signals = DroneSignals() + self.drone_topics = {} + self.lock = Lock() + + self.arm_clients = {} + self.takeoff_clients = {} + self.setpoint_pubs = {} + self.selected_drones = set() + self.latest_data = {} + + # 定義需要過濾的模式 + self.filtered_modes = ['Mode(0x000000c0)'] + + # WebSocket 接收器列表 + self.ws_receivers = [] + + # 串口接收器列表 + + # ================================================================================ + # 【新增】儲存 GPS 資料的字典 + # ================================================================================ + self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} + # ================================================================================ + self.serial_receivers = [] + + # 主题检测定时器 + 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+_\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(drone_id) + + def setup_drone(self, drone_id): + base_topic = f'/MavLinkBus/{drone_id}' + + # Add service clients + self.arm_clients[drone_id] = self.create_client( + CommandBool, + f'{base_topic}/cmd/arming' + ) + self.takeoff_clients[drone_id] = self.create_client( + CommandTOL, + f'{base_topic}/cmd/takeoff' + ) + + # Add setpoint publisher + self.setpoint_pubs[drone_id] = self.create_publisher( + Point, + f'{base_topic}/setpoint_position/local', + 10 + ) + + 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 + ), + '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 + ), + 'loss_rate': self.create_subscription( + Float64, + f'{base_topic}/packet_loss_rate', + lambda msg, did=drone_id: self.loss_rate_callback(did, msg), + 10 + ), + 'state': self.create_subscription( + State, + f'{base_topic}/state', + lambda msg, did=drone_id: self.state_callback(did, msg), + 10 + ), + 'ping': self.create_subscription( + Float64, + f'{base_topic}/ping', + lambda msg, did=drone_id: self.ping_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) + + async def arm_drone(self, drone_id, arm): + if drone_id not in self.arm_clients: + return False + + client = self.arm_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandBool.Request() + request.value = arm + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Arm service call failed: {e}') + return False + + async def takeoff_drone(self, drone_id, altitude=10.0): + if drone_id not in self.takeoff_clients: + return False + + client = self.takeoff_clients[drone_id] + if not client.wait_for_service(timeout_sec=1.0): + return False + + request = CommandTOL.Request() + request.altitude = altitude + request.min_pitch = 0.0 + request.yaw = 0.0 + + future = client.call_async(request) + try: + response = await future + return response.success + except Exception as e: + self.get_logger().error(f'Takeoff service call failed: {e}') + return False + + def send_setpoint(self, drone_id, x, y, z): + """Send setpoint position command""" + if drone_id not in self.setpoint_pubs: + return False + + msg = Point() + msg.x = float(x) + msg.y = float(y) + msg.z = float(z) + + self.setpoint_pubs[drone_id].publish(msg) + return True + + 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) + + # callbacks + def attitude_callback(self, drone_id, msg): + if hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + self.latest_data[(drone_id, 'attitude')] = { + '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.latest_data[(drone_id, 'battery')] = { + 'voltage': msg.voltage + } + + def state_callback(self, drone_id, msg): + mode = msg.mode + if mode in self.filtered_modes: + return + self.latest_data[(drone_id, 'state')] = { + 'mode': msg.mode, + 'armed': msg.armed + } + + def gps_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'gps')] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + + # ================================================================================ + # 【新增】儲存 GPS 資料到 drone_gps 字典 + # ================================================================================ + self.drone_gps[drone_id] = { + 'lat': msg.latitude, + 'lon': msg.longitude, + 'alt': msg.altitude + } + # ================================================================================ + + def local_vel_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'velocity')] = { + 'vx': msg.x, + 'vy': msg.y, + 'vz': msg.z + } + + def altitude_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'altitude')] = { + 'altitude': msg.data + } + + def local_pose_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'local_pose')] = { + 'x': msg.x, + 'y': msg.y, + 'z': msg.z + } + + def hud_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'hud')] = { + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.altitude, + 'climb': msg.climb + } + + def loss_rate_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'loss_rate')] = { + 'loss_rate': msg.data + } + + def ping_callback(self, drone_id, msg): + self.latest_data[(drone_id, 'ping')] = { + 'ping': msg.data + } + + def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): + """啟動串口 MAVLink 連接""" + connection_name = f"Serial_{port.replace('/', '_')}" + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver.start() + self.serial_receivers.append(receiver) + print(f"Started serial connection on {port} at {baudrate} baud") + return receiver \ No newline at end of file diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py new file mode 100644 index 0000000..a5a7bf5 --- /dev/null +++ b/src/GUI/drone_panel.py @@ -0,0 +1,378 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, + QPushButton, QSizePolicy, QCheckBox) +from PyQt6.QtCore import pyqtSignal + +class DronePanel(QWidget): + """單個無人機面板類別""" + + # 定義信號 + mode_change_requested = pyqtSignal(str) # drone_id + arm_requested = pyqtSignal(str) # drone_id + takeoff_requested = pyqtSignal(str) # drone_id + setpoint_requested = pyqtSignal(str) # drone_id + selection_changed = pyqtSignal(str, int) # drone_id, state + + def __init__(self, drone_id, parent=None): + super().__init__(parent) + self.drone_id = drone_id + self.display_id = 's' + drone_id.split('_')[1] + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"panel_{self.drone_id}") + self.setFixedHeight(140) + self.setStyleSheet(""" + background-color: #2A2A2A; + border-radius: 8px; + """) + + # 主佈局 + main_layout = QHBoxLayout(self) + main_layout.setContentsMargins(8, 8, 8, 8) + main_layout.setSpacing(0) + + # 創建內容容器（包含 info 和 control） + content_widget = QWidget() + content_widget.setStyleSheet("background-color: #333; border-radius: 6px;") + content_layout = QHBoxLayout(content_widget) + content_layout.setContentsMargins(8, 8, 8, 8) + content_layout.setSpacing(8) + + # 左側資訊區域 + info_widget = self._create_info_section() + + # 右側控制按鈕區域 + control_widget = self._create_control_section() + + # 將 info 和 control 加入內容容器 + content_layout.addWidget(info_widget) + content_layout.addWidget(control_widget) + + # 將內容容器加入主佈局 + main_layout.addWidget(content_widget) + + def _create_info_section(self): + """創建資訊顯示區域""" + info_widget = QWidget() + info_layout = QVBoxLayout(info_widget) + info_layout.setContentsMargins(0, 0, 0, 0) + info_layout.setSpacing(4) + + # 頂部標題欄 + header = QWidget() + header_layout = QHBoxLayout(header) + header_layout.setContentsMargins(0, 0, 0, 0) + + # 勾選框 + self.checkbox = QCheckBox() + self.checkbox.setObjectName(f"{self.drone_id}_checkbox") + self.checkbox.setStyleSheet("QCheckBox { color: #DDD; }") + self.checkbox.stateChanged.connect( + lambda state: self.selection_changed.emit(self.drone_id, state) + ) + + # ID 顯示 + id_label = QLabel(self.display_id) + id_label.setStyleSheet(""" + font-weight: bold; + font-size: 14px; + color: #7FFFD4; + min-width: 80px; + """) + + header_layout.addWidget(self.checkbox) + header_layout.addWidget(id_label) + header_layout.addStretch() + + info_layout.addWidget(header) + + # 第一行：狀態 (模式 + ARM狀態) + status_row = self._create_status_row() + info_layout.addWidget(status_row) + + # 第二行：電池 + battery_row = self._create_battery_row() + info_layout.addWidget(battery_row) + + # 第三行：位置 + 高度 + position_row = self._create_position_row() + info_layout.addWidget(position_row) + + # 第四行：航向 + 速度 + nav_row = self._create_nav_row() + info_layout.addWidget(nav_row) + + return info_widget + + def _create_status_row(self): + """創建狀態行""" + status_row = QWidget() + status_layout = QHBoxLayout(status_row) + status_layout.setContentsMargins(0, 0, 0, 0) + + status_title = QLabel("狀態：") + status_title.setStyleSheet("color: #888; min-width: 50px;") + + self.mode_label = QLabel("--") + self.mode_label.setObjectName(f"{self.drone_id}_mode") + self.mode_label.setStyleSheet("color: #DDD;") + + self.armed_label = QLabel("--") + self.armed_label.setObjectName(f"{self.drone_id}_armed") + self.armed_label.setStyleSheet("color: #DDD;") + + status_layout.addWidget(status_title) + status_layout.addWidget(self.mode_label) + status_layout.addWidget(self.armed_label) + status_layout.addStretch() + + return status_row + + def _create_battery_row(self): + """創建電池行""" + battery_row = QWidget() + battery_layout = QHBoxLayout(battery_row) + battery_layout.setContentsMargins(0, 0, 0, 0) + + battery_title = QLabel("電池：") + battery_title.setStyleSheet("color: #888; min-width: 50px;") + + self.battery_label = QLabel("--") + self.battery_label.setObjectName(f"{self.drone_id}_battery") + self.battery_label.setStyleSheet("color: #DDD;") + + battery_layout.addWidget(battery_title) + battery_layout.addWidget(self.battery_label) + battery_layout.addStretch() + + return battery_row + + def _create_position_row(self): + """創建位置行""" + position_row = QWidget() + position_layout = QHBoxLayout(position_row) + position_layout.setContentsMargins(0, 0, 0, 0) + + position_title = QLabel("位置：") + position_title.setStyleSheet("color: #888; min-width: 50px;") + + self.local_label = QLabel("--") + self.local_label.setObjectName(f"{self.drone_id}_local") + self.local_label.setStyleSheet("color: #DDD;") + + altitude_title = QLabel("高度：") + altitude_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.altitude_label = QLabel("--") + self.altitude_label.setObjectName(f"{self.drone_id}_altitude") + self.altitude_label.setStyleSheet("color: #DDD;") + + position_layout.addWidget(position_title) + position_layout.addWidget(self.local_label) + position_layout.addWidget(altitude_title) + position_layout.addWidget(self.altitude_label) + position_layout.addStretch() + + return position_row + + def _create_nav_row(self): + """創建導航行""" + nav_row = QWidget() + nav_layout = QHBoxLayout(nav_row) + nav_layout.setContentsMargins(0, 0, 0, 0) + + heading_title = QLabel("航向：") + heading_title.setStyleSheet("color: #888; min-width: 50px;") + + self.heading_label = QLabel("--") + self.heading_label.setObjectName(f"{self.drone_id}_heading") + self.heading_label.setStyleSheet("color: #DDD;") + + speed_title = QLabel("速度：") + speed_title.setStyleSheet("color: #888; margin-left: 10px;") + + self.groundspeed_label = QLabel("--") + self.groundspeed_label.setObjectName(f"{self.drone_id}_groundspeed") + self.groundspeed_label.setStyleSheet("color: #DDD;") + + nav_layout.addWidget(heading_title) + nav_layout.addWidget(self.heading_label) + nav_layout.addWidget(speed_title) + nav_layout.addWidget(self.groundspeed_label) + nav_layout.addStretch() + + return nav_row + + def _create_control_section(self): + """創建控制按鈕區域""" + control_widget = QWidget() + control_layout = QVBoxLayout(control_widget) + control_layout.setContentsMargins(0, 0, 0, 0) + control_layout.setSpacing(6) + + control_widget.setFixedWidth(80) + + btn_style = """ + QPushButton { + background-color: #444; + color: #DDD; + border: none; + border-radius: 4px; + font-size: 11px; + } + QPushButton:hover { + background-color: #555; + } + """ + # 模式切換按鈕 + mode_btn = QPushButton("切換模式") + mode_btn.setStyleSheet(btn_style) + mode_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + mode_btn.clicked.connect(lambda: self.mode_change_requested.emit(self.drone_id)) + + # 解鎖按鈕 + arm_btn = QPushButton("解鎖") + arm_btn.setStyleSheet(btn_style) + arm_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + arm_btn.clicked.connect(lambda: self.arm_requested.emit(self.drone_id)) + + # 起飛按鈕 + takeoff_btn = QPushButton("起飛") + takeoff_btn.setStyleSheet(btn_style) + takeoff_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + takeoff_btn.clicked.connect(lambda: self.takeoff_requested.emit(self.drone_id)) + + # Setpoint 按鈕 + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.setStyleSheet(btn_style) + setpoint_btn.setSizePolicy(QSizePolicy.Policy.Preferred, QSizePolicy.Policy.Expanding) + setpoint_btn.clicked.connect(lambda: self.setpoint_requested.emit(self.drone_id)) + + control_layout.addWidget(mode_btn) + control_layout.addWidget(arm_btn) + control_layout.addWidget(takeoff_btn) + control_layout.addWidget(setpoint_btn) + + return control_widget + + def update_field(self, field, text, color=None): + """更新指定欄位的值""" + label = self.findChild(QLabel, f"{self.drone_id}_{field}") + if label and label.text() != text: + label.setText(text) + if color: + label.setStyleSheet(f"color: {color};") + + def get_checkbox(self): + """獲取勾選框""" + return self.checkbox + + def set_checked(self, checked): + """設置勾選狀態""" + self.checkbox.setChecked(checked) + + def is_checked(self): + """獲取勾選狀態""" + return self.checkbox.isChecked() + +class SocketGroupPanel(QWidget): + # 定義信號 + group_selection_changed = pyqtSignal(str, int) # socket_id, state + + def __init__(self, socket_id, color='#AAAAAA', parent=None): + super().__init__(parent) + self.socket_id = socket_id + self.color = color + self._init_ui() + + def _init_ui(self): + """初始化UI""" + self.setObjectName(f"socket_group_{self.socket_id}") + self.setStyleSheet(""" + background-color: #1E1E1E; + border-radius: 12px; + """) + + layout = QVBoxLayout(self) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(6) + + # Socket 分組標題行 - 包含勾選框 + title_row = QWidget() + title_layout = QHBoxLayout(title_row) + title_layout.setContentsMargins(0, 0, 0, 0) + + # 分組勾選框 + self.group_checkbox = QCheckBox() + self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox") + self.group_checkbox.setStyleSheet(f""" + QCheckBox {{ color: #DDD; }} + QCheckBox::indicator {{ + width: 14px; + height: 14px; + border: 2px solid #888888; + border-radius: 3px; + background: transparent; + }} + QCheckBox::indicator:checked {{ + background-color: {self.color}; + border: 2px solid #888888; + }} + QCheckBox::indicator:indeterminate {{ + background-color: #666; + border: 2px solid #888888; + }} + """) + self.group_checkbox.stateChanged.connect( + lambda state: self.group_selection_changed.emit(self.socket_id, state) + ) + + # Socket 分組標題 + title_label = QLabel(f"Socket {self.socket_id}") + title_label.setStyleSheet(f""" + font-weight: bold; + font-size: 16px; + color: {self.color}; + margin-bottom: 8px; + padding: 4px 8px; + border-radius: 6px; + """) + + title_layout.addWidget(self.group_checkbox) + title_layout.addWidget(title_label) + title_layout.addStretch() + + layout.addWidget(title_row) + + # 創建子容器用於放置該 socket 下的所有無人機面板 + self.drones_container = QWidget() + self.drones_layout = QVBoxLayout(self.drones_container) + self.drones_layout.setContentsMargins(0, 0, 0, 0) + self.drones_layout.setSpacing(4) + + layout.addWidget(self.drones_container) + + def add_drone_panel(self, panel): + """添加無人機面板到分組""" + self.drones_layout.addWidget(panel) + + def clear_drones(self): + """清空所有無人機面板""" + while self.drones_layout.count(): + item = self.drones_layout.takeAt(0) + if item.widget(): + item.widget().setParent(None) + + def get_checkbox(self): + """獲取分組勾選框""" + return self.group_checkbox + + def set_checked(self, checked): + """設置分組勾選狀態""" + self.group_checkbox.setChecked(checked) + + def set_check_state(self, state): + """設置分組勾選狀態（支持半選）""" + self.group_checkbox.setCheckState(state) \ No newline at end of file diff --git a/src/GUI/gui.py b/src/GUI/gui.py new file mode 100644 index 0000000..cefc9c9 --- /dev/null +++ b/src/GUI/gui.py @@ -0,0 +1,1009 @@ +#!/usr/bin/env python3 +import rclpy +from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, + QWidget, QLabel, QSplitter, QScrollArea, + QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, + QHeaderView, QPushButton, QCheckBox, QLineEdit) +from PyQt6.QtCore import Qt, QTimer +import sys +import asyncio + +# 導入分離的類別 +from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver +from map_layout import DroneMap +from drone_panel import DronePanel, SocketGroupPanel +from comm_panel import CommPanel +from overview_table import OverviewTable + +# ================================================================================ +# 【新增】導入任務規劃器 +# ================================================================================ +from mission_planner import FormationPlanner +# ================================================================================ + +class ControlStationUI(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle('GCS') + self.resize(1400, 900) + + # 初始化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) + + # 定时处理ROS事件 + self.timer = QTimer() + self.timer.timeout.connect(self.spin_ros) + self.timer.start(10) + + # 初始化UI + self.drones = {} + self.socket_groups = {} + self.info_types = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", + "Roll", "Pitch", "Yaw", "丟包", "延遲"] + self.info_type_map = { + "mode": 0, + "armed": 1, + "battery": 2, + "longitude": 3, + "latitude": 4, + "altitude": 5, + "local": 6, + "velocity": 7, + "groundspeed": 8, + "heading": 9, + "roll": 10, + "pitch": 11, + "yaw": 12, + "loss_rate": 13, + "ping": 14 + } + + self.socket_colors = { + '0': '#00BFFF', # 天藍色 (DeepSkyBlue) + '1': '#FFD700', # 金色 (Gold) + '2': '#FF6969', # 淺紅色 (Light Red) + '3': '#FF69B4', # 熱粉紅 (HotPink) + '4': '#00FA9A', # 中春綠 (MediumSpringGreen) + '5': '#9370DB', # 中紫色 (MediumPurple) - 串口 + '6': '#FFA500', # 橙色 (Orange) + '7': '#20B2AA', # 淺海綠 (LightSeaGreen) + '8': '#7CFC00', # 草綠色 (LawnGreen) + '9': '#FF8C00', # 深橙色 (DarkOrange) + 'default': '#AAAAAA' # 灰色 + } + + self.drone_positions = {} + self.drone_headings = {} + # 初始化地圖 + self.drone_map = DroneMap() + # 初始化連接列表 + self.udp_receivers = [] + self.udp_connections = [] + self.ws_connections = [] + self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) + + # ================================================================================ + # 【新增】初始化任務規劃器 + # ================================================================================ + self.mission_planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + self.planned_waypoints = None # 儲存規劃結果 + # ================================================================================ + + self.init_ui() + + def init_ui(self): + # 只呼叫一次 + main_splitter = QSplitter(Qt.Orientation.Horizontal) + + # 左側 TabWidget + self.left_tab = QTabWidget() + + # — 分頁 1：Drone Panel + self.drone_panel_container = QWidget() + self.drone_panel_layout = QVBoxLayout(self.drone_panel_container) + self.drone_panel_layout.setAlignment(Qt.AlignmentFlag.AlignTop) + self.drone_panel_layout.setSpacing(0) + self.drone_panel_layout.setContentsMargins(10, 10, 10, 10) + + # Wrap drone panel in scroll area + scroll = QScrollArea() + scroll.setWidget(self.drone_panel_container) + scroll.setWidgetResizable(True) + self.left_tab.addTab(scroll, "無人載具") + + # — 分頁 2：Overview Table + self.overview_table = OverviewTable(self.info_types, self.info_type_map) + self.left_tab.addTab(self.overview_table, "總覽") + + # — 分頁 3：通訊設定 + self.comm_panel = CommPanel() + self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) + self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) + self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) + self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) + + self.left_tab.addTab(self.comm_panel, "通訊") + + # 右侧容器 + right_container = QWidget() + right_layout = QVBoxLayout(right_container) + right_layout.setContentsMargins(10, 10, 10, 10) + right_layout.setSpacing(10) + + # ========== 批次控制區域（直接使用 layout） ========== + batch_control_layout = QHBoxLayout() + + # 添加批次操作標題 + batch_title = QLabel("批次操作") + batch_title.setStyleSheet(""" + color: #DDD; + font-size: 16px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + batch_control_layout.addWidget(batch_title) + + # 上方按鈕區域 + upper_buttons = QHBoxLayout() + select_all_btn = QPushButton("全選") + select_all_btn.clicked.connect(self.handle_select_all) + arm_all_btn = QPushButton("解鎖") + arm_all_btn.clicked.connect(self.handle_arm_selected) + takeoff_all_btn = QPushButton("起飛") + takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) + for btn in [select_all_btn, arm_all_btn, takeoff_all_btn]: + btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + upper_buttons.addWidget(btn) + upper_buttons.addStretch() + + # 模式切換區域 + mode_layout = QHBoxLayout() + mode_label = QLabel("模式：") + mode_label.setStyleSheet("color: #DDD; min-width: 40px;") + + from PyQt6.QtWidgets import QComboBox + self.mode_combo = QComboBox() + self.mode_combo.addItems(["AUTO", "GUIDED", "LOITER", "LAND"]) + self.mode_combo.setCurrentIndex(1) + self.mode_combo.setStyleSheet(""" + QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} + """) + + batch_mode_btn = QPushButton("切換") + batch_mode_btn.clicked.connect(self.handle_batch_mode_change) + batch_mode_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + mode_layout.addWidget(mode_label) + mode_layout.addWidget(self.mode_combo) + mode_layout.addWidget(batch_mode_btn) + mode_layout.addStretch() + + # 座標輸入區域 + coord_widget = QWidget() + coord_layout = QHBoxLayout(coord_widget) + + self.x_input = QLineEdit() + self.y_input = QLineEdit() + self.z_input = QLineEdit() + + for input_field in [self.x_input, self.y_input, self.z_input]: + input_field.setFixedWidth(60) + input_field.setStyleSheet(""" + QLineEdit { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 3px; + } + """) + + coord_layout.addWidget(QLabel("X:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.x_input) + coord_layout.addWidget(QLabel("Y:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.y_input) + coord_layout.addWidget(QLabel("Z:", styleSheet="color: #DDD;")) + coord_layout.addWidget(self.z_input) + + setpoint_btn = QPushButton("Setpoint") + setpoint_btn.clicked.connect(self.handle_setpoint_selected) + setpoint_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 8px 12px; + border-radius: 4px; + min-width: 80px; + } + QPushButton:hover { background-color: #555; } + """) + + lower_control = QHBoxLayout() + lower_control.addWidget(coord_widget) + lower_control.addWidget(setpoint_btn) + lower_control.addStretch() + + # 組裝批次控制 layout + batch_control_layout.addLayout(upper_buttons) + batch_control_layout.addLayout(mode_layout) + batch_control_layout.addLayout(lower_control) + + # 將批次控制 layout 添加到右側容器 + right_layout.addLayout(batch_control_layout) + + # 添加地圖 + right_layout.addWidget(self.drone_map.get_widget()) + self.drone_map.get_gps_signal().connect(self.handle_map_click) + + + # Add widgets to splitter + main_splitter.addWidget(self.left_tab) + main_splitter.addWidget(right_container) + main_splitter.setSizes([400, 1000]) + + self.setCentralWidget(main_splitter) + + + def handle_udp_connection_added(self, ip, port): + """处理 UDP 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'UDP {len(self.udp_connections) + 1}', + 'ip': ip, + 'port': port, + 'enabled': True + } + + # 启动接收器 + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.udp_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_udp_panel(new_conn) + + self.statusBar().showMessage(f"已添加 UDP 连接: {ip}:{port}", 3000) + print(f"UDP MAVLink receiver added and started on {ip}:{port}") + + def handle_ws_connection_added(self, url): + """处理 WebSocket 连接添加信号""" + # 创建新连接 + new_conn = { + 'name': f'WS {len(self.ws_connections) + 1}', + 'url': url, + 'enabled': True + } + + # 启动接收器 + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + new_conn['receiver'] = receiver + + self.ws_connections.append(new_conn) + + # 添加到 UI + self.comm_panel.add_ws_panel(new_conn) + + self.statusBar().showMessage(f"已添加 WebSocket 连接: {url}", 3000) + print(f"WebSocket receiver added and started: {url}") + + def create_drone_panel(self, drone_id): + """創建無人機面板""" + panel = DronePanel(drone_id) + + # 連接信號 + panel.mode_change_requested.connect(self.handle_mode_change) + panel.arm_requested.connect(self.handle_arm) + panel.takeoff_requested.connect(self.handle_takeoff) + panel.setpoint_requested.connect(self.handle_single_setpoint) + panel.selection_changed.connect(self.handle_drone_selection) + + return panel + + def toggle_ws_connection(self, conn, btn, status_label): + """切換 WebSocket 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000) + else: + # 啟動連接 + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name']) + receiver.start() + self.monitor.ws_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 WebSocket 連接: {conn['url']}", 3000) + + def remove_ws_connection(self, conn, panel): + """移除 WebSocket 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.monitor.ws_receivers: + self.monitor.ws_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.ws_connections: + self.ws_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_ws_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 WebSocket 连接: {conn['url']}", 3000) + + def toggle_udp_connection(self, conn, btn, status_label): + """切換 UDP 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + else: + # 啟動連接 + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name']) + receiver.start() + self.udp_receivers.append(receiver) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 UDP 連接: {conn['ip']}:{conn['port']}", 3000) + + def remove_udp_connection(self, conn, panel): + """移除 UDP 连接""" + # 停止接收器 + if 'receiver' in conn and conn['receiver']: + conn['receiver'].stop() + if conn['receiver'] in self.udp_receivers: + self.udp_receivers.remove(conn['receiver']) + + # 从列表移除 + if conn in self.udp_connections: + self.udp_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_udp_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) + + def create_socket_group_panel(self, socket_id): + """創建 socket 分組面板""" + color = self.socket_colors.get(socket_id, self.socket_colors['default']) + panel = SocketGroupPanel(socket_id, color) + panel.group_selection_changed.connect(self.handle_group_selection) + return panel + + def handle_mode_change(self, drone_id): + """處理單個無人機的模式切換""" + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}")) + + def handle_arm(self, drone_id): + loop = asyncio.get_event_loop() + arm_state = not self.monitor.get_arm_state(drone_id) # Toggle arm state + future = self.monitor.arm_drone(drone_id, arm_state) + loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}")) + + def handle_takeoff(self, drone_id): + loop = asyncio.get_event_loop() + future = self.monitor.takeoff_drone(drone_id, 10.0) # 10 meters default + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) + + def handle_setpoint_selected(self): + """處理發送 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + for drone_id in self.monitor.selected_drones: + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + def handle_single_setpoint(self, drone_id): + """處理單個無人機的 setpoint 命令""" + try: + x = float(self.x_input.text() or '0') + y = float(self.y_input.text() or '0') + z = float(self.z_input.text() or '0') + + if self.monitor.send_setpoint(drone_id, x, y, z): + self.statusBar().showMessage(f"發送位置命令到 {drone_id}: ({x}, {y}, {z})", 3000) + else: + self.statusBar().showMessage(f"發送位置命令失敗: {drone_id}", 3000) + except ValueError: + self.statusBar().showMessage("座標格式錯誤", 3000) + + async def handle_service_response(self, future, action): + try: + result = await future + if result: + self.statusBar().showMessage(f"{action} 成功", 3000) + else: + self.statusBar().showMessage(f"{action} 失敗", 3000) + except Exception as e: + self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) + + def update_ui(self, msg_type, drone_id, data): + # 檢查是否為新無人機，若是則加入無人機面板 + if drone_id not in self.drones: + self.add_drone(drone_id) + return + + # 確認無人機面板存在 + if not (panel := self.drones.get(drone_id)): + return + + # 更新特定欄位並在總覽頁面更新 + if msg_type == 'state': + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', None) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + if armed is True: + arm_text = "ARMED" + arm_color = '#55FF55' + elif armed is False: + arm_text = "DISARMED" + arm_color = '#FF5555' + else: + arm_text = "--" + arm_color = '#AAAAAA' # 未知狀態 + + # 更新無人機面板欄位 + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + + # 更新總覽頁面欄位 + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + + elif msg_type == 'battery': + voltage = data.get('voltage', 16) + + # 判斷電池節數 + cells = round(voltage / 3.95) + + # 計算電量百分比 + percentage = (voltage / cells - 3.7) / 0.5 * 100 + + # 根據百分比設置顏色 + if percentage < 20: + voltage_color = '#FF6464' # 紅色 (低電量) + elif percentage < 50: + voltage_color = '#FFA500' # 橘色 (中低電量) + else: + voltage_color = '#FFFFFF' # 白色 (正常電量) + + percentage = data.get('percentage', percentage) + + # 顯示總電壓、電池節數、單節電壓和百分比 + text = f"{percentage:.0f}%" + vol = f"{voltage:.2f}V" + + self.update_field(panel, drone_id, 'battery', text, voltage_color) + self.update_overview_table(drone_id, 'battery', vol) + + elif msg_type == 'gps': + lat, lon = data.get('lat', 0), data.get('lon', 0) + self.drone_positions[drone_id] = (lat, lon) + lon_text = f"{lon:.6f}°" + lat_text = f"{lat:.6f}°" + self.update_field(panel, drone_id, 'longitude', lon_text) + self.update_field(panel, drone_id, 'latitude', lat_text) + self.update_overview_table(drone_id, 'longitude', lon_text) + self.update_overview_table(drone_id, 'latitude', lat_text) + + # ================================================================================ + # 【新增】同時儲存到 monitor.drone_gps（處理 UDP/WebSocket 的 GPS 資料） + # ================================================================================ + alt = data.get('alt', 0) # UDP/WebSocket 可能沒有 alt + if not hasattr(self.monitor, 'drone_gps'): + self.monitor.drone_gps = {} + self.monitor.drone_gps[drone_id] = { + 'lat': lat, + 'lon': lon, + 'alt': alt + } + # ================================================================================ + + # 更新地圖上的無人機位置 + heading = self.drone_headings.get(drone_id, 0) # 如果沒有航向，使用 0 + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + elif msg_type == 'altitude': + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" + self.update_field(panel, drone_id, 'altitude', text) + self.update_overview_table(drone_id, 'altitude', text) + + elif msg_type == 'local_pose': + text = f"{data['x']:.1f}, {data['y']:.1f}" + self.update_field(panel, drone_id, 'local', text) + self.update_overview_table(drone_id, 'local', text) + + elif msg_type == 'hud': + heading = data.get('heading') + self.drone_headings[drone_id] = heading + groundspeed = data.get('groundspeed') + heading_text = f"{heading:.1f}°" + if isinstance(groundspeed, (int, float)): + groundspeed_text = f"{groundspeed:.1f} m/s" + else: + groundspeed_text = "--" + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'heading', heading_text) + self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + + # 如果有位置資訊，也更新地圖上的航向 + if drone_id in self.drone_positions: + lat, lon = self.drone_positions[drone_id] + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + elif msg_type == 'loss_rate': + loss_rate = data.get('loss_rate', 0) + text = f"{loss_rate:.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + ping = data.get('ping', 0) + text = f"{ping:.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', text) + + elif msg_type == 'velocity': + text = f"{data['vx']:.1f}, {data['vy']:.1f}" + self.update_overview_table(drone_id, 'velocity', text) + + elif msg_type == 'attitude': + roll = data.get('roll', 0) + pitch = data.get('pitch', 0) + yaw = data.get('yaw', 0) + roll_text = f"{roll:.1f}°" + pitch_text = f"{pitch:.1f}°" + yaw_text = f"{yaw:.1f}°" + self.update_overview_table(drone_id, 'roll', roll_text) + self.update_overview_table(drone_id, 'pitch', pitch_text) + self.update_overview_table(drone_id, 'yaw', yaw_text) + + # 新增處理分組勾選的方法 + def handle_group_selection(self, socket_id, state): + """處理 socket 分組勾選狀態變化""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 根據分組勾選狀態更新所有該分組的無人機勾選狀態 + is_checked = state == Qt.CheckState.Checked.value + + for drone_id in group_drones: + checkbox = self.drones[drone_id].get_checkbox() + if checkbox: + # 暫時斷開信號連接，避免遞迴觸發 + checkbox.blockSignals(True) + checkbox.setChecked(is_checked) + checkbox.blockSignals(False) + + # 手動更新選中集合 + if is_checked: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + def handle_drone_selection(self, drone_id, state): + """處理個別無人機勾選狀態變化""" + if state == Qt.CheckState.Checked.value: + self.monitor.selected_drones.add(drone_id) + else: + self.monitor.selected_drones.discard(drone_id) + + # 更新對應 socket 分組的勾選狀態 + socket_id = self.get_socket_id(drone_id) + self.update_group_checkbox_state(socket_id) + + # 新增更新分組勾選框狀態的方法 + def update_group_checkbox_state(self, socket_id): + """更新指定 socket 分組的勾選框狀態""" + # 獲取該分組下的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + if not group_drones: + return + + # 檢查該分組內有多少無人機被勾選 + checked_count = sum(1 for did in group_drones + if self.drones[did].is_checked()) + + # 獲取分組勾選框 + if socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + # 暫時斷開信號連接 + group_checkbox.blockSignals(True) + + if checked_count == 0: + # 沒有無人機被勾選 + group_checkbox.setCheckState(Qt.CheckState.Unchecked) + elif checked_count == len(group_drones): + # 所有無人機都被勾選 + group_checkbox.setCheckState(Qt.CheckState.Checked) + else: + # 部分無人機被勾選，顯示半勾選狀態 + group_checkbox.setCheckState(Qt.CheckState.PartiallyChecked) + + # 重新連接信號 + group_checkbox.blockSignals(False) + + def handle_select_all(self): + """處理全選按鈕 - 支援分組結構""" + # 檢查是否所有無人機都已被選中 + all_selected = all( + self.drones[drone_id].is_checked() + for drone_id in self.drones + ) + + # 如果全部已選中，則取消全選；否則全選 + new_state = not all_selected + + # 更新所有勾選框狀態（無人機和分組） + for drone_id in self.drones: + self.drones[drone_id].set_checked(new_state) + + # 更新所有分組勾選框狀態 + for socket_id in self.socket_groups: + group_checkbox = self.socket_groups[socket_id].findChild(QCheckBox, f"socket_{socket_id}_checkbox") + if group_checkbox: + group_checkbox.blockSignals(True) + group_checkbox.setChecked(new_state) + group_checkbox.blockSignals(False) + + def handle_arm_selected(self): + """處理批次解鎖""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}")) + + def handle_takeoff_selected(self): + """處理批次起飛""" + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.takeoff_drone(drone_id, 10.0) + loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) + + def handle_batch_mode_change(self): + mode = self.mode_combo.currentText() + loop = asyncio.get_event_loop() + for drone_id in self.monitor.selected_drones: + future = self.monitor.set_mode(drone_id, mode) + loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) + + def update_field(self, panel, drone_id, field, text, color=None): + """更新面板中的指定欄位""" + if isinstance(panel, DronePanel): + panel.update_field(field, text, color) + + def update_overview_table(self, drone_id=None, field=None, value=None): + """更新總覽表格""" + if not hasattr(self, 'overview_table') or self.overview_table is None: + return + + # 更新無人機引用 + self.overview_table.set_drones(self.drones) + # 委託給 OverviewTable 處理 + self.overview_table.update_table(drone_id, field, value) + + def get_socket_id(self, drone_id): + """從 drone_id 提取 socket_id (例如 's9_1' -> '9')""" + import re + match = re.match(r's(\d+)_\d+', drone_id) + return match.group(1) if match else 'unknown' + + def add_drone(self, drone_id): + if drone_id in self.drones: + return + + # 獲取 socket_id + socket_id = self.get_socket_id(drone_id) + + # 創建無人機面板 + panel = self.create_drone_panel(drone_id) + self.drones[drone_id] = panel + + # 如果該 socket 分組不存在，創建它 + if socket_id not in self.socket_groups: + group_panel = self.create_socket_group_panel(socket_id) + self.socket_groups[socket_id] = group_panel + + # 將無人機面板添加到對應的 socket 分組中 + group_panel = self.socket_groups[socket_id] + group_panel.drones_layout.addWidget(panel) + + # 重新排序並顯示所有 socket 分組 + self.reorganize_socket_groups() + + # 更新分組勾選框狀態 + self.update_group_checkbox_state(socket_id) + + # 更新總覽表 + self.update_overview_table() + + def reorganize_socket_groups(self): + """重新組織和排序 socket 分組""" + # 先清空主佈局 + while self.drone_panel_layout.count(): + w = self.drone_panel_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對每個 socket 分組內的無人機進行排序 + for socket_id, group_panel in self.socket_groups.items(): + # 獲取該分組內的所有無人機 + group_drones = [did for did in self.drones.keys() + if self.get_socket_id(did) == socket_id] + + # 清空分組佈局 + while group_panel.drones_layout.count(): + w = group_panel.drones_layout.takeAt(0).widget() + if w: + w.setParent(None) + + # 對該分組內的無人機按數字排序 + def sort_key(x): + parts = x[1:].split('_') + return (int(parts[0]), int(parts[1])) + + # 重新添加排序後的無人機面板 + for did in sorted(group_drones, key=sort_key): + group_panel.drones_layout.addWidget(self.drones[did]) + + # 按 socket_id 數字順序添加分組到主佈局 + for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)): + self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) + + def spin_ros(self): + try: + self.executor.spin_once(timeout_sec=0.01) + for (drone_id, msg_type), data in self.monitor.latest_data.items(): + self.monitor.signals.update_signal.emit(msg_type, drone_id, data) + self.monitor.latest_data.clear() + except Exception as e: + print(f"ROS spin error: {e}") + + def closeEvent(self, event): + # 停止 UDP 接收器 + for receiver in self.udp_receivers: + receiver.stop() + + # 停止 WebSocket 接收器 + for receiver in self.monitor.ws_receivers: + receiver.stop() + + self.monitor.destroy_node() + self.executor.shutdown() + rclpy.shutdown() + event.accept() + + # ================================================================================ + # 【新增】以下方法是任務規劃功能 + # ================================================================================ + + def handle_map_click(self, lat, lon): + """ + 處理地圖點擊事件，自動規劃隊形任務 + + Args: + lat: 緯度 + lon: 經度 + """ + print(f"地圖點擊位置: {lat:.6f}, {lon:.6f}") + + selected_drones = self.get_selected_drones() + + if len(selected_drones) == 0: + self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) + return + + # 設定目標點 + base_alt = 10.0 # 基準高度 + target_gps = (lat, lon, base_alt) + + self.statusBar().showMessage(f"⏳ 正在規劃 {len(selected_drones)} 台無人機的任務...", 2000) + + try: + # 取得當前無人機位置（GPS 座標） + drone_gps_positions = [] + + for drone_id in selected_drones: + # 檢查是否有 GPS 資料 + if hasattr(self.monitor, 'drone_gps') and drone_id in self.monitor.drone_gps: + # 使用實際的 GPS 資料 + gps_data = self.monitor.drone_gps[drone_id] + lat_drone = gps_data['lat'] + lon_drone = gps_data['lon'] + alt_drone = gps_data.get('alt', base_alt) + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用實際 GPS: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + elif drone_id in self.drone_positions: + # 備用方案：從 Local 座標轉換 + pos = self.drone_positions[drone_id] + # 使用簡化轉換 + lat_drone = 24.0 + pos[1] / 111000 + lon_drone = 120.0 + pos[0] / (111000 * 0.9) + alt_drone = pos[2] if len(pos) > 2 else base_alt + drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) + print(f"使用 Local 轉換: {drone_id} -> ({lat_drone:.6f}, {lon_drone:.6f}, {alt_drone:.1f})") + + else: + self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) + return + + # 規劃任務 + # ================================================================================ + # 【修改】接收中心點座標 + # ================================================================================ + stage1_gps, stage2_gps, center_origin = self.mission_planner.plan_formation_mission( + drone_gps_positions, + target_gps + ) + # ================================================================================ + + # 儲存規劃結果 + self.planned_waypoints = { + 'stage1': stage1_gps, + 'stage2': stage2_gps, + 'drone_ids': selected_drones + } + + # 顯示規劃結果 + self.show_planned_waypoints() + + # ================================================================================ + # 【新增】在地圖上顯示任務規劃（中心點 + 虛線） + # ================================================================================ + center_lat, center_lon, center_alt = center_origin + self.drone_map.draw_mission_plan( + center_lat, center_lon, # 中心點座標 + lat, lon # 目標點座標 + ) + # ================================================================================ + + self.statusBar().showMessage( + f"✓ 任務規劃完成！{len(selected_drones)} 台無人機，2 階段飛行", 5000 + ) + + except Exception as e: + self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) + print(f"Mission planning error: {e}") + import traceback + traceback.print_exc() + + def show_planned_waypoints(self): + """ + 顯示規劃的航點（在終端輸出） + """ + if not self.planned_waypoints: + return + + print("\n" + "=" * 60) + print("任務規劃結果") + print("=" * 60) + + drone_ids = self.planned_waypoints['drone_ids'] + stage1 = self.planned_waypoints['stage1'] + stage2 = self.planned_waypoints['stage2'] + + print(f"\n共 {len(drone_ids)} 台無人機") + print(f"參數：間距={self.mission_planner.spacing}m, " + f"基準高度={self.mission_planner.base_altitude}m, " + f"高低差={self.mission_planner.altitude_diff}m") + + for i, drone_id in enumerate(drone_ids): + print(f"\n【{drone_id}】") + lat1, lon1, alt1 = stage1[i] + lat2, lon2, alt2 = stage2[i] + + print(f" 階段 1（集合點）：") + print(f" 緯度: {lat1:.6f}°") + print(f" 經度: {lon1:.6f}°") + print(f" 高度: {alt1:.1f} m") + + print(f" 階段 2（目標點）：") + print(f" 緯度: {lat2:.6f}°") + print(f" 經度: {lon2:.6f}°") + print(f" 高度: {alt2:.1f} m") + + print("\n" + "=" * 60) + + def get_selected_drones(self): + """ + 取得被選中的無人機 ID 列表 + + Returns: + list: 被選中的無人機 ID 列表 + """ + selected = [] + for drone_id, panel in self.drones.items(): + if hasattr(panel, 'checkbox') and panel.checkbox.isChecked(): + selected.append(drone_id) + return selected + + # ================================================================================ + # 【新增結束】 + # ================================================================================ + +if __name__ == '__main__': + app = QApplication(sys.argv) + station = ControlStationUI() + station.show() + app.exec() \ No newline at end of file diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py new file mode 100644 index 0000000..24bd2d2 --- /dev/null +++ b/src/GUI/map_layout.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python3 +from PyQt6.QtWebEngineWidgets import QWebEngineView +from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot +from PyQt6.QtWebChannel import QWebChannel + +class DroneMap: + """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" + + def __init__(self): + """初始化地圖""" + self.map_view = QWebEngineView() + self.map_loaded = False + self.pending_map_updates = {} + + # 創建橋接對象 + self.bridge = MapBridge() + + # 設置 QWebChannel + self.channel = QWebChannel() + self.channel.registerObject('bridge', self.bridge) + self.map_view.page().setWebChannel(self.channel) + + # 設置地圖 HTML + inline_html = ''' + + + + + + + + + + + +
+
+ +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + # 批量執行所有待更新的位置 + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + # 清空待更新緩存 + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + # ================================================================================ + # 【新增】任務規劃視覺化方法 + # ================================================================================ + def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon): + """ + 在地圖上繪製任務規劃 + + Args: + center_lat: 中心點緯度 + center_lon: 中心點經度 + target_lat: 目標點緯度 + target_lon: 目標點經度 + """ + if self.map_loaded: + js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});" + self.map_view.page().runJavaScript(js_code) + print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") + + def clear_mission_plan(self): + """清除地圖上的任務規劃標記""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearMissionPlan();") + print("🗑️ 地圖已清除任務規劃") + # ================================================================================ + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) # lat, lon + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) \ No newline at end of file diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py new file mode 100644 index 0000000..138795d --- /dev/null +++ b/src/GUI/mission_planner.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python3 +""" +飛行任務規劃模組 +負責將 GPS 點擊座標轉換為隊形飛行任務 +""" +import math +from typing import List, Tuple, Optional + + +class CoordinateConverter: + """GPS 座標與 Local 座標的轉換器""" + + def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0): + """ + 初始化座標轉換器 + + Args: + origin_lat: 參考原點緯度 + origin_lon: 參考原點經度 + origin_alt: 參考原點高度（公尺，相對於海平面） + """ + self.origin_lat = origin_lat + self.origin_lon = origin_lon + self.origin_alt = origin_alt + self.R = 6371000.0 # 地球半徑（公尺） + + def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]: + """ + GPS 座標轉換為 Local 座標（ENU 系統：East-North-Up） + + Args: + lat: 緯度 + lon: 經度 + alt: 高度（公尺，相對於海平面） + + Returns: + (x, y, z): Local 座標（公尺） + x: East（東方） + y: North（北方） + z: Up（向上） + """ + lat_rad = math.radians(lat) + lon_rad = math.radians(lon) + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + dlat = lat_rad - origin_lat_rad + dlon = lon_rad - origin_lon_rad + + # 使用 Equirectangular projection（適用於小範圍 < 10 km） + x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2) # East + y = self.R * dlat # North + z = alt - self.origin_alt # Up + + return x, y, z + + def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]: + """ + Local 座標轉換為 GPS 座標 + + Args: + x: East（東方，公尺） + y: North（北方，公尺） + z: Up（向上，公尺） + + Returns: + (lat, lon, alt): GPS 座標 + """ + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + lat_rad = origin_lat_rad + (y / self.R) + lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2))) + + lat = math.degrees(lat_rad) + lon = math.degrees(lon_rad) + alt = self.origin_alt + z + + return lat, lon, alt + + +class FormationPlanner: + """隊形規劃器""" + + def __init__(self, spacing: float = 5.0, + base_altitude: float = 10.0, + altitude_diff: float = 2.0): + """ + 初始化隊形規劃器 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺，相對於參考原點） + altitude_diff: M 字形的高低差（公尺） + """ + self.spacing = spacing + self.base_altitude = base_altitude + self.altitude_diff = altitude_diff + self.current_origin = None + self.converter = None + + def plan_formation_mission(self, + drone_gps_positions: List[Tuple[float, float, float]], + target_gps: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 規劃兩階段隊形任務 + + Args: + drone_gps_positions: 當前無人機 GPS 位置列表 [(lat, lon, alt), ...] + target_gps: 目標點 GPS 座標 (lat, lon, alt) + + Returns: + stage1_gps: 階段 1（集合點）的 GPS 航點列表 + stage2_gps: 階段 2（目標點）的 GPS 航點列表 + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + origin: 中心點（參考原點）的 GPS 座標 (lat, lon, alt) + """ + if len(drone_gps_positions) == 0: + raise ValueError("無人機位置列表不能為空") + + # ===== 步驟 1: 計算中央點並設為參考原點 ===== + center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) + center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) + center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) + + self.current_origin = (center_lat, center_lon, center_alt) + self.converter = CoordinateConverter(center_lat, center_lon, center_alt) + + print(f"📍 參考原點: ({center_lat:.6f}, {center_lon:.6f}, {center_alt:.1f}m)") + + # ===== 步驟 2: 轉換到 Local 座標系 ===== + drone_local_positions = [] + for lat, lon, alt in drone_gps_positions: + x, y, z = self.converter.gps_to_local(lat, lon, alt) + drone_local_positions.append((x, y, z)) + + target_local = self.converter.gps_to_local( + target_gps[0], target_gps[1], target_gps[2] + ) + + # ===== 步驟 3: 在 Local 座標系中計算隊形 ===== + stage1_local, stage2_local = self._calculate_formation_local( + drone_local_positions, + target_local + ) + + # ===== 步驟 4: 轉回 GPS 座標 ===== + stage1_gps = [] + for x, y, z in stage1_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage1_gps.append((lat, lon, alt)) + + stage2_gps = [] + for x, y, z in stage2_local: + lat, lon, alt = self.converter.local_to_gps(x, y, z) + stage2_gps.append((lat, lon, alt)) + + print(f"✅ 任務規劃完成: {len(stage1_gps)} 台無人機，2 階段飛行") + + # ================================================================================ + # 【修改】回傳中心點座標供地圖視覺化使用 + # ================================================================================ + return stage1_gps, stage2_gps, self.current_origin + # ================================================================================ + + def _calculate_formation_local(self, + drone_positions: List[Tuple[float, float, float]], + target_point: Tuple[float, float, float]) -> Tuple[List[Tuple[float, float, float]], + List[Tuple[float, float, float]]]: + """ + 在 Local 座標系中計算隊形 + + Args: + drone_positions: Local 座標的無人機位置 [(x, y, z), ...] + target_point: Local 座標的目標點 (x, y, z) + + Returns: + stage1_positions: 階段 1（中央點分佈）的 Local 座標 + stage2_positions: 階段 2（目標點分佈）的 Local 座標 + """ + N = len(drone_positions) + + # ===== 步驟 1: 計算中央點（在 Local 座標系中應該接近原點）===== + C_x = sum(pos[0] for pos in drone_positions) / N + C_y = sum(pos[1] for pos in drone_positions) / N + C_z = sum(pos[2] for pos in drone_positions) / N + + print(f" 中央點 Local: ({C_x:.2f}, {C_y:.2f}, {C_z:.2f})") + + # ===== 步驟 2: 計算方向向量（中央點 → 目標點）===== + T_x, T_y, T_z = target_point + V_x = T_x - C_x + V_y = T_y - C_y + + print(f" 方向向量: ({V_x:.2f}, {V_y:.2f})") + + # ===== 步驟 3: 計算垂直向量（逆時針旋轉 90°）===== + P_x = -V_y + P_y = V_x + + # 單位化垂直向量 + length = math.sqrt(P_x**2 + P_y**2) + if length < 0.01: # 避免除以零 + # 如果目標點與中央點重合，使用默認方向（向東） + P_x_unit, P_y_unit = 1.0, 0.0 + else: + P_x_unit = P_x / length + P_y_unit = P_y / length + + print(f" 垂直單位向量: ({P_x_unit:.3f}, {P_y_unit:.3f})") + + # ===== 步驟 4: 計算無人機在垂直方向的投影並排序 ===== + projections = [] + for i, pos in enumerate(drone_positions): + relative_x = pos[0] - C_x + relative_y = pos[1] - C_y + projection = relative_x * P_x_unit + relative_y * P_y_unit + projections.append((projection, i)) + + # 按投影值排序（保持左右順序） + projections.sort() + sorted_indices = [idx for _, idx in projections] + + print(f" 排序後的無人機索引: {sorted_indices}") + + # ===== 步驟 5: 分佈無人機位置 ===== + stage1_positions = [None] * N # 預分配列表 + stage2_positions = [None] * N + + for rank, original_idx in enumerate(sorted_indices): + # 計算相對中心的水平偏移 + offset = (rank - (N - 1) / 2) * self.spacing + + # 計算 M 字形高度（交替高低） + if rank % 2 == 0: + altitude = self.base_altitude + self.altitude_diff # 高 + else: + altitude = self.base_altitude - self.altitude_diff # 低 + + # 階段 1：在中央點附近分佈 + stage1_x = C_x + P_x_unit * offset + stage1_y = C_y + P_y_unit * offset + stage1_z = altitude + + # 階段 2：在目標點附近分佈（保持相同的相對位置） + stage2_x = T_x + P_x_unit * offset + stage2_y = T_y + P_y_unit * offset + stage2_z = altitude + + # 按照原始索引存儲（保持無人機 ID 順序） + stage1_positions[original_idx] = (stage1_x, stage1_y, stage1_z) + stage2_positions[original_idx] = (stage2_x, stage2_y, stage2_z) + + return stage1_positions, stage2_positions + + def update_parameters(self, spacing: Optional[float] = None, + base_altitude: Optional[float] = None, + altitude_diff: Optional[float] = None): + """ + 更新隊形參數 + + Args: + spacing: 無人機間距（公尺） + base_altitude: 基準飛行高度（公尺） + altitude_diff: M 字形的高低差（公尺） + """ + if spacing is not None: + self.spacing = spacing + print(f" 間距更新為: {spacing} m") + + if base_altitude is not None: + self.base_altitude = base_altitude + print(f" 基準高度更新為: {base_altitude} m") + + if altitude_diff is not None: + self.altitude_diff = altitude_diff + print(f" 高低差更新為: {altitude_diff} m") + + +# ===== 測試程式碼 ===== +if __name__ == "__main__": + print("=" * 60) + print("隊形任務規劃器測試") + print("=" * 60) + + # 模擬 5 台無人機的 GPS 位置 + drone_gps = [ + (24.123450, 120.567800, 100.0), # 無人機 0 + (24.123470, 120.567820, 102.0), # 無人機 1 + (24.123440, 120.567810, 98.0), # 無人機 2 + (24.123460, 120.567830, 101.0), # 無人機 3 + (24.123445, 120.567795, 99.0), # 無人機 4 + ] + + # 目標點 + target_gps = (24.123600, 120.568000, 105.0) + + # 建立規劃器 + planner = FormationPlanner( + spacing=5.0, # 5 公尺間距 + base_altitude=10.0, # 基準高度 10 公尺 + altitude_diff=2.0 # 高低差 2 公尺 + ) + + # 規劃任務 + print("\n開始規劃任務...") + stage1, stage2 = planner.plan_formation_mission(drone_gps, target_gps) + + # 顯示結果 + print("\n" + "=" * 60) + print("階段 1：集合點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage1): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n" + "=" * 60) + print("階段 2：目標點位置（GPS）") + print("=" * 60) + for i, (lat, lon, alt) in enumerate(stage2): + print(f"無人機 {i}: ({lat:.6f}, {lon:.6f}, {alt:.1f}m)") + + print("\n✅ 測試完成！") \ No newline at end of file diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py new file mode 100644 index 0000000..11953f0 --- /dev/null +++ b/src/GUI/overview_table.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import QTableWidget, QTableWidgetItem, QHeaderView, QLabel +from PyQt6.QtCore import Qt + +class OverviewTable(QTableWidget): + """總覽表格，顯示所有無人機的狀態資訊""" + + def __init__(self, info_types, info_type_map, parent=None): + super().__init__(parent) + + self.info_types = info_types + self.info_type_map = info_type_map + self.drones = {} # 存儲無人機面板的引用 + + # 初始化表格 + self.setColumnCount(1) + self.setRowCount(len(self.info_types)) + self.setHorizontalHeaderLabels(["資訊"]) + header = self.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.verticalHeader().setVisible(False) + + # 設置第一列的資訊類型 + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(i, 0, item) + + def set_drones(self, drones): + """設置無人機面板字典的引用""" + self.drones = drones + + def update_table(self, drone_id=None, field=None, value=None): + """更新總覽表格 + + Args: + drone_id: 無人機 ID + field: 欄位名稱 (如 'mode', 'altitude' 等) + value: 要更新的值 + """ + # 更新特定儲存格 + if drone_id and field and value: + if drone_id not in self.drones: + return + + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) + + if row == -1: + return # 無效的欄位 + + item = self.item(row, col) + if not item: + item = QTableWidgetItem() + self.setItem(row, col, item) + + item.setText(value) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + + # 如果沒有指定更新，刷新整個表格 + if drone_id is None: + self.refresh_all() + + def refresh_all(self): + """刷新整個表格""" + cols = 1 + len(self.drones) + self.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(row, col, val_item) + + def add_drone_column(self, drone_id): + """當新增無人機時，添加一列""" + if drone_id in self.drones: + self.refresh_all() + + def remove_drone_column(self, drone_id): + """當移除無人機時，刷新表格""" + if drone_id in self.drones: + self.refresh_all() From e54e42aad27cc755b0200f6bd93bfe332711b509 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 01:13:59 +0800 Subject: [PATCH 098/146] Bring fc_network_adapter from master --- .../fc_network_adapter/devRun.py | 508 ------ .../fc_network_adapter/fc_network_adapter.md | 190 ++ .../fc_network_adapter/mainOrchestrator.py | 1597 +++++++++++++++++ .../fc_network_adapter/mavlinkObject.py | 1004 +++++++---- .../fc_network_adapter/mavlinkPublish.py | 129 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 902 ++++++++++ .../fc_network_adapter/mavlinkVehicleView.py | 453 +++++ .../fc_network_adapter/serialManager.py | 611 +++++++ .../fc_network_adapter/utils/__init__.py | 7 + .../fc_network_adapter/utils/acquirePort.py | 129 ++ .../fc_network_adapter/utils/acquireSerial.py | 111 ++ .../fc_network_adapter/utils/ringBuffer.py | 231 +++ .../fc_network_adapter/utils/theLogger.py | 43 + src/fc_network_adapter/setup.py | 1 + src/fc_network_adapter/tests/__init__.py | 0 .../tests/demo_integration.py | 277 +++ .../tests/demo_mavlinkVehicleView.py | 331 ++++ .../tests/demo_ringBuffer.py | 152 ++ .../tests/test_mavlinkObject.py | 468 +++++ .../tests/test_ringBuffer.py | 296 +++ .../tests/test_vehicleStatusPublisher.py | 507 ++++++ 21 files changed, 7070 insertions(+), 877 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/devRun.py create mode 100644 src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md create mode 100644 src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py create mode 100644 src/fc_network_adapter/fc_network_adapter/serialManager.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/__init__.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py create mode 100644 src/fc_network_adapter/fc_network_adapter/utils/theLogger.py create mode 100644 src/fc_network_adapter/tests/__init__.py create mode 100644 src/fc_network_adapter/tests/demo_integration.py create mode 100644 src/fc_network_adapter/tests/demo_mavlinkVehicleView.py create mode 100644 src/fc_network_adapter/tests/demo_ringBuffer.py create mode 100644 src/fc_network_adapter/tests/test_mavlinkObject.py create mode 100644 src/fc_network_adapter/tests/test_ringBuffer.py create mode 100644 src/fc_network_adapter/tests/test_vehicleStatusPublisher.py diff --git a/src/fc_network_adapter/fc_network_adapter/devRun.py b/src/fc_network_adapter/fc_network_adapter/devRun.py deleted file mode 100644 index 78abb8f..0000000 --- a/src/fc_network_adapter/fc_network_adapter/devRun.py +++ /dev/null @@ -1,508 +0,0 @@ -# 基礎功能的 import -import queue -import time - -# ROS2 的 import -import rclpy - -# mavlink 的 import -from pymavlink import mavutil - -# 自定義的 import -import mavlinkObject as mo -import mavlinkDevice as md - -# ====================== 分割線 ===================== - -test_item = 51 -running_time =10000 -print('test_item : ', test_item) - -if test_item == 51: - # 晉凱的測試項目 - 修改為支援多UDP連接 - 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) - - # === 修改：支援多個UDP連接 === - ports = ["udp:127.0.0.1:14550", - "udp:127.0.0.1:14570", - "/dev/ttyUSB0", - "/dev/ttyUSB1"] # 可以根據需要調整端口 - mavlink_sockets = [] - mavlink_objects = [] - - # 循環創建多個連接 - for i, port in enumerate(ports): - try: - print(f"連接到 UDP:{port}") - mavlink_socket = mavutil.mavlink_connection(port) - mavlink_object = mo.mavlink_object(mavlink_socket) - - # 設定通道流動（所有連接使用相同參數） - mavlink_object.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147] - mavlink_object.multiplexingToReturn = [] - - # 啟動通道 - mavlink_object.run() - - # 保存連接引用 - mavlink_sockets.append(mavlink_socket) - mavlink_objects.append(mavlink_object) - - print(f"UDP:{port} 連接成功，socket_id: {mavlink_object.socket_id}") - - except Exception as e: - print(f"連接 UDP:{port} 失敗: {e}") - continue - - print(f"成功建立 {len(mavlink_sockets)} 個 UDP 連接") - - print('waiting for mavlink data ...') - time.sleep(3) # 等待足夠時間讓所有device object收到MAVLink訊息 - - # 顯示檢測到的系統 - print('目前所有的系統：') - for sysid in analyzer.mavlink_systems: - system = analyzer.mavlink_systems[sysid] - socket_id = getattr(system, 'socket_id', 'Unknown') - print(f" 系統ID: {sysid}, socket_id: {socket_id}") - - # === 為所有檢測到的系統創建topics === - topic_creation_start = time.time() - topics_created = 0 - - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - try: - print(f"為系統 {sysid}, 組件 {compid} 創建topics...") - - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) - - topics_created += 1 - print(f"系統 {sysid}_{compid} topics創建完成") - - except Exception as e: - print(f"為系統 {sysid}_{compid} 創建topics失敗: {e}") - continue - - topic_creation_end = time.time() - print(f"總共為 {topics_created} 個系統創建topics，耗時: {topic_creation_end - topic_creation_start:.2f} 秒") - - print("start emit info") - - # === 主循環：支援多連接 === - start_time = time.time() - loop_count = 0 - - # while time.time() - start_time < running_time: - while True: - try: - # ROS2 發布 - analyzer.emit_info() # 這邊是測試 node 的運行 - - # 每10次循環顯示一次詳細狀態 - loop_count += 1 - if loop_count % 10 == 0: # 每5秒顯示一次（0.5s * 10） - print(f"\n=== 狀態更新 (第 {loop_count} 次循環) ===") - #print(f"連接的端口: {udp_ports[:len(mavlink_sockets)]}") - print(f"檢測到的系統數: {len(analyzer.mavlink_systems)}") - - for sysid in analyzer.mavlink_systems: - system = analyzer.mavlink_systems[sysid] - socket_id = getattr(system, 'socket_id', 'Unknown') - print(f" 系統 {sysid}: socket_id={socket_id}") - - for compid in system.components: - component = system.components[compid] - - # 安全地獲取msg_count - msg_count = getattr(component, 'msg_count', 0) - if isinstance(msg_count, dict): - total_count = sum(msg_count.values()) if msg_count else 0 - else: - total_count = msg_count if msg_count else 0 - - print(f" 組件 {compid}: 收到 {total_count} 條消息") - - # 顯示emitParams狀態 - if hasattr(component, 'emitParams') and component.emitParams: - param_count = len(component.emitParams) - print(f" emitParams: {param_count} 項數據") - else: - print(f" emitParams: 無數據") - - # 重置消息計數 - try: - system.resetComponentPacketCount(compid) - except Exception as e: - print(f" 重置計數失敗: {e}") - - print("===============================") - - time.sleep(0.5) - - except KeyboardInterrupt: - print("\n使用者中斷程序...") - break - except Exception as e: - print(f"主循環錯誤: {e}") - break - - # === 清理資源 === - print("開始清理資源...") - - # 清理ROS2 - try: - analyzer.destroy_node() - print("ROS2 node 已清理") - except Exception as e: - print(f"清理ROS2 node失敗: {e}") - - try: - rclpy.shutdown() - print("ROS2 已關閉") - except Exception as e: - print(f"關閉ROS2失敗: {e}") - - # 清理所有mavlink objects - for i, mavlink_obj in enumerate(mavlink_objects): - try: - print(f"停止 mavlink_object {i+1} (UDP:{ports[i]}, socket_id: {mavlink_obj.socket_id})") - mavlink_obj.stop() - mavlink_obj.thread.join() - print(f"mavlink_object {i+1} 已停止") - except Exception as e: - print(f"停止 mavlink_object {i+1} 失敗: {e}") - - # 關閉所有mavlink sockets - for i, mavlink_sock in enumerate(mavlink_sockets): - try: - print(f"關閉 UDP 連接 {ports[i]}") - mavlink_sock.close() - except Exception as e: - print(f"關閉 UDP 連接 {ports[i]} 失敗: {e}") - - # 清理analyzer - try: - print("停止 analyzer") - analyzer.stop() - analyzer.thread.join() - print("analyzer 已停止") - except Exception as e: - print(f"停止 analyzer 失敗: {e}") - - print(f"清理完成，共處理了 {len(mavlink_sockets)} 個 UDP 連接") - print('<=== End of Program') - -elif test_item == 54: - # 文鈞的測試項目 - 5輸入2輸出版本 + 結合test51的ROS2功能 - # 加入詳細調試信息 - print('===> Start of Program .Test ', test_item) - - # === ROS2 初始化 (來自test51新版本) === - rclpy.init() - print("ROS2 初始化完成") - - # 1) 啟動 bridge（它已自動建立所有 publisher） - bridge = mo.mavlink_bridge() - - try: - bridge._init_node() - - # 添加Node初始化檢查和修復 - if not hasattr(bridge, '_default_callback_group'): - print("警告：Node 初始化不完整，嘗試修復...") - from rclpy.node import Node - # 強制重新初始化為正確的 Node - Node.__init__(bridge, 'mavlink_bridge_fixed') - print("Node 重新初始化完成") - else: - print("Node 初始化成功") - - except Exception as e: - print(f"Node初始化失敗: {e}") - print("嘗試備用初始化方法...") - - # 備用方法：創建一個新的Node實例 - from rclpy.node import Node - - class BackupNode(Node): - def __init__(self): - super().__init__('mavlink_bridge_backup') - - backup_node = BackupNode() - - # 將備用node的方法附加到bridge對象 - bridge.create_publisher = backup_node.create_publisher - bridge.destroy_node = backup_node.destroy_node - bridge._backup_node = backup_node - - print("備用Node創建完成") - - print("ROS2 bridge 初始化完成") - - # 雙輸出連接設定 (連接到兩個不同的GCS) - gcs_outputs = [ - "udpout:127.0.0.1:14500", # GCS 1 - "udpout:127.0.0.1:14600" # GCS 2 - ] - - # 建立輸出連接物件 - mavlink_objects_out = [] - mavlink_sockets_out = [] - - # 設定5個輸入連接（修改為實際測試可用的端口） - device_inputs = [ - "udp:127.0.0.1:14550", # 無人機1 (UDP) - "udp:127.0.0.1:14570", # 無人機2 (UDP) - "/dev/ttyUSB0", # 無人機3 (UDP) - "/dev/ttyUSB1", # 無人機4 (UDP) - "/dev/ttyUSB2", # 無人機5 (UDP) - ] - - # 建立輸入連接 - mavlink_objects_in = [] - mavlink_sockets_in = [] - - for i, output_conn in enumerate(gcs_outputs): - print(f"建立 GCS {i+1} 輸出連接: {output_conn}") - mavlink_out = mavutil.mavlink_connection(output_conn) - obj_out = mo.mavlink_object(mavlink_out) - obj_out.multiplexingToAnalysis = [0] # 只分析心跳訊息 - mavlink_objects_out.append(obj_out) - mavlink_sockets_out.append(mavlink_out) - - # 設定GCS到所有設備的轉發關係 - for i, obj_out in enumerate(mavlink_objects_out): - for j, obj_in in enumerate(mavlink_objects_in): - obj_out.multiplexingToSwap[obj_in.socket_id] = [-1, ] # GCS→所有設備 - print(f"設定 GCS {i+1} (socket_id: {obj_out.socket_id}) → 設備 {j+1} (socket_id: {obj_in.socket_id}) 轉發") - - for i, input_conn in enumerate(device_inputs): - print(f"連接設備 {i+1} 輸入: {input_conn}") - try: - # UDP連接 - mavlink_in = mavutil.mavlink_connection(input_conn) - print(f" UDP連接 {input_conn}") - - obj_in = mo.mavlink_object(mavlink_in) - - # === 設置消息分析類型 (來自test51新版本) === - obj_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147] # HEARTBEAT + 常用訊息 + TIMESYNC - print(f" 設備 {i+1} 設置分析消息類型: {obj_in.multiplexingToAnalysis}") - - mavlink_objects_in.append(obj_in) - mavlink_sockets_in.append(mavlink_in) - - # 設定設備到所有GCS的轉發關係 - for j, obj_out in enumerate(mavlink_objects_out): - obj_in.multiplexingToSwap[obj_out.socket_id] = [-1, ] # 設備→所有GCS - print(f"設定設備 {i+1} (socket_id: {obj_in.socket_id}) → GCS {j+1} (socket_id: {obj_out.socket_id}) 轉發") - - except Exception as e: - print(f"警告：無法建立連接 {input_conn}，錯誤：{e}") - print(f"跳過設備 {i+1}") - continue - - # 做一個空的通道驗證 可以拿來 debug - mavlink_object_none = mo.mavlink_object(None) - - print(f"\n成功建立 {len(mavlink_objects_in)} 個輸入連接") - print(f"成功建立 {len(mavlink_objects_out)} 個輸出連接") - - # 啟動所有輸入通道 - for i, obj in enumerate(mavlink_objects_in): - obj.run() - print(f"啟動輸入通道 {i+1}") - - # 啟動所有輸出通道 - for i, obj in enumerate(mavlink_objects_out): - obj.run() - print(f"啟動輸出通道 {i+1}") - - # === 等待MAVLink數據 (來自test51新版本) === - print("waiting for mavlink data...") - time.sleep(3) # 增加等待時間 - print("=== connection established! ===") - - # 顯示目前偵測到的 sysid 清單 - print("Current sysid list:", list(bridge.mavlink_systems.keys())) - for sysid in bridge.mavlink_systems: - print(bridge.mavlink_systems[sysid]) - - # 顯示轉發設定摘要 - print("\n=== 系統配置摘要 ===") - print(f"輸入設備數量: {len(mavlink_objects_in)}") - print("輸入設備類型:") - for i, input_conn in enumerate(device_inputs[:len(mavlink_objects_in)]): - device_type = "串口" if input_conn.startswith("/dev/tty") else "UDP" - print(f" 設備 {i+1}: {input_conn} ({device_type})") - print(f"GCS數量: {len(mavlink_objects_out)}") - print("輸出GCS:") - for i, output_conn in enumerate(gcs_outputs): - print(f" GCS {i+1}: {output_conn}") - print("轉發規則:") - print(" - 每個設備的所有訊息 → 所有GCS") - print(" - 每個GCS的所有訊息 → 所有設備") - print("MAVLink分析:") - print(" - 消息類型: [0, 30, 32, 33, 74, 111, 147] (HEARTBEAT, ATTITUDE, LOCAL_POSITION_NED, GLOBAL_POSITION_INT, VFR_HUD, TIMESYNC, BATTERY_STATUS)") - print("ROS2 Topics: 已自動建立所有publisher") - print("===================\n") - - # === 主運行循環 (來自test51新版本) + 調試信息 === - print("開始ROS2 topics發布...") - - try: - last_timesync = time.time() - show_time = time.time() - message_count = 0 - ros2_publish_count = 0 - - # 調試用計數器 - debug_counters = { - 'swap_messages': 0, - 'analysis_messages': 0, - 'return_messages': 0, - 'ros2_publishes': 0 - } - - while rclpy.ok() and time.time() - last_timesync < running_time: - now = time.time() - - # === 調試：檢查各種queue的消息 === - # 檢查 swap_queues - try: - test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False) - print('none object 收到訊息: ', test) - debug_counters['swap_messages'] += 1 - except queue.Empty: - pass - - # 檢查 fixed_stream_bridge_queue - try: - while not mo.fixed_stream_bridge_queue.empty(): - msg = mo.fixed_stream_bridge_queue.get(block=False) - debug_counters['analysis_messages'] += 1 - message_count += 1 - except queue.Empty: - pass - - # 檢查 return_packet_processor_queue - try: - while not mo.return_packet_processor_queue.empty(): - msg = mo.return_packet_processor_queue.get(block=False) - debug_counters['return_messages'] += 1 - message_count += 1 - except queue.Empty: - pass - - # 每秒發送 TIMESYNC (來自test51新版本) - if now - last_timesync >= 1.0: - timesync_sent = 0 - # 對每個輸入設備發送 TIMESYNC request - for i, mavlink_socket in enumerate(mavlink_sockets_in): - try: - mavlink_socket.mav.timesync_send(0, int(now * 1e9)) - timesync_sent += 1 - except Exception as e: - print(f"發送 TIMESYNC 到設備 {i+1} 失敗: {e}") - - last_timesync = now - - # ROS2 發布 (來自test51新版本) - try: - bridge.emit_info() # 將所有 emitParams 發布到 ROS topic - debug_counters['ros2_publishes'] += 1 - ros2_publish_count += 1 - except Exception as e: - print(f"[ERROR] ROS2 發布失敗: {e}") - - # 狀態報告 (更頻繁的調試輸出) - if (time.time() - show_time) >= 2: # 每2秒顯示一次狀態 - show_time = time.time() - print(f"\n=== 調試狀態報告===") - - # 顯示消息統計 - print(f"總消息數: {message_count}, ROS2發布次數: {ros2_publish_count}") - - # 重置調試計數器 - debug_counters = {k: 0 for k in debug_counters} - - if len(bridge.mavlink_systems) > 0: - for sysid in bridge.mavlink_systems: - system = bridge.mavlink_systems[sysid] - print(f"系統 {sysid}: socket_id={getattr(system, 'socket_id', 'N/A')}") - - for compid in system.components: - component = system.components[compid] - msg_count = component.msg_count - - system.resetComponentPacketCount(compid) - else: - print("目前沒有檢測到任何MAVLink系統") - - # 顯示各通道的狀態 - print(f"輸入通道數量: {len(mavlink_objects_in)} (運行中)") - print(f"輸出通道數量: {len(mavlink_objects_out)} (運行中)") - print("ROS2發布狀態: 運行中") - - # 顯示queue狀態 - print(f"Queue 狀態:") - print(f" fixed_stream_bridge_queue: {mo.fixed_stream_bridge_queue.qsize()}") - print(f" return_packet_processor_queue: {mo.return_packet_processor_queue.qsize()}") - for i, q in enumerate(mo.swap_queues): - print(f" swap_queue[{i}]: {q.qsize()}") - - print("===================\n") - - time.sleep(0.1) # 更快的循環，更及時的調試信息 - - except KeyboardInterrupt: - print("\n使用者中斷程序...") - pass - - # === 程序結束清理 (來自test51新版本) === - print("正在關閉所有連接...") - - # -------- 清理 ROS2 -------- - try: - bridge.destroy_node() - except Exception as e: - print(f"清理主Node失敗: {e}") - - # 清理備用node（如果存在） - if hasattr(bridge, '_backup_node'): - try: - bridge._backup_node.destroy_node() - print("備用Node已清理") - except Exception as e: - print(f"清理備用Node失敗: {e}") - - rclpy.shutdown() - - # 關閉輸入通道 - for i, obj in enumerate(mavlink_objects_in): - device_type = "UDP" - print(f"關閉設備 {i+1} ({device_type}) 輸入通道") - obj.stop() - obj.thread.join() - mavlink_sockets_in[i].close() - - # 關閉輸出通道 - for i, obj in enumerate(mavlink_objects_out): - print(f"關閉 GCS {i+1} 輸出通道") - obj.stop() - obj.thread.join() - mavlink_sockets_out[i].close() - - # 關閉分析器 - bridge.stop() - bridge.thread.join() - print('<=== End of Program') \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md new file mode 100644 index 0000000..efd48cd --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -0,0 +1,190 @@ + +這個檔案整理 此專案下 程式代碼的流程與思路 +只會挑出重要的變數與方法描述 +以利後續開發使用 + +# 開發此專案的注意事項 +- 預設 autopilot 的 component id = 1 +- 不允許 system id 重複 +- 增加一個固定數值監控然後要到 ros2 topic + - mavlinkROS2Node.py 檔案內 + - PublishRateController.topic_intervals 建立 + - VehicleStatusPublisher._publish_vehicle_status 登記 + - VehicleStatusPublisher._publish_XXX 實作 + - mavlinkObject.py 檔案內 + - mavlink_bridge.message_handlers 登記 + - mavlink_bridge._handle_XXX 實作 + - mavlink_object.bridge_msg_types 登記 (這個可以用介面調) + - mavlinkVehicleView.py 檔案內 + - 注意對應的資料存放區 + + +--- +# 檔案結構 + +特別注意: +1. 有標註 [async method] 都是不該被直接呼叫的內部方法 + +- *valuable* 這個是變數 **沒有括號** +- *method (parameters...)* 這個是方法 **有括號** + +## mainOrchestrator.py : 程式進入點 + +### **[Class]** Orchestrator + 最上層的發配資源與啟動終端機面板的調配者 +- *self.manager* 存放 async_io_manager 實例 +- *self.bridge* 存放 mavlink_bridge 實例 +- *self.plumber* 存放 serial_manager 實例 +- *self.vehicle_registry* 存放 vehicle_registry 實例 + +- *self.panel_thread* 面板的執行緒 +- *self.panelState* 暫存面板與調配者互動的資料流動區 + - 面板運行狀態 + - 面板操作結果 + - 其他模組的運行狀態 +--- +- *mainLoop()* 核心方法 + - 更新個模組狀態到 *self.panelState* + - 對應面板來的操作指令 +--- + 對於 async_io_manager 控制實現 +- *create_udp_object()* +- *delete_udp_object()* +- *add_target_to_object()* +- *remove_target_from_object()* +--- + 關於載具管理與檢視 +- *_update_vehicles_list()* +- *_prepare_vehicle_info()* +--- + 關於 serial_manager 控制實現 +- *create_serial_port_object()* + + +### **[Class]** ControlPanel + 面板的核心運行物件 + 把自己的變數 獨立出來都放到 PanelState 去 +- *panel_thread()* 核心方法 + - 主選單的引入 + - 主選單下所有的按鍵操作 + - 定義所有人為操作後續面板執行緒行為 +- *menu_tree()* 基礎選單的定義檔 +--- + 關於 udp object 的操作 +- *create_object_list_menu()* object 選單的定義檔 +- *show_object_info()* 顯示 object 資訊 +- *select_target_socket()* object 對於轉拋功能的操作 +--- + 關於 serial 的操作 +- *create_serial_port_menu()* +- *create_linked_serial_menu()* +- *show_linked_serial_info()* +--- + 關於載具檢視與操作 +- *create_vehicles_list_menu()* +- *show_vehicle_info()* + +### **[Class]** PanelState + 作為面板執行緒(ControlPanel)與調配者(Orchestrator)溝通的管道 + 不包含具體實作方法 是 ControlPanel 的延伸 +- *self.panel_info_msg_list* 顯示在面板上的資訊訊息 + +## mavlinkObject.py + +### 全域變數 +- *stream_bridge_ring* +- *return_packet_ring* + +### **[Class]** mavlink_bridge + 唯一實例 + 實際去解析 mavlink 封包的地方 + 接收 stream_bridge_ring 與 return_packet_ring 的資料 + 這邊是比較偏自動化 不會被操作的 +- *self.thread* 自己的執行緒 +--- +- *_run_thread()* 核心方法 +- *_handle_XXXXX()* 每一種單項 mavlink 封包的解析 +- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法 +- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理 + +### **[Class]** async_io_manager + 唯一實例 + 異步 event loop + 沒有核心方法 + 這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作) + +- *self.thread* 自己的執行緒 +- *self.managed_objects* 資料結構 socket_id: mavlink_object +--- +- *add_mavlink_object(mavlink_object)* [call method] 把一個 mavlink_object 物件加入管理 +- *_async_add_mavlink_object(mavlink_object)* [async method] 對應上面的內部方法 不該直接使用 +- *remove_mavlink_object(socket_id)* [call method] 從管理區把指定 mavlink_object 移除 + +### **[Class]** mavlink_object + 儲存 mavlink socket + 處理 mavlink 封包分流的地方 +- *cls.mavlinkObjects* 資料結構 { socket_id(序號) : mavlink_object(物件實例) } +- *self.mavlink_socket* 從 pymavlink 繼承的socket物件 +- *self.state* 描述這個 socket 物件的狀態 +--- +- *process_data()* [async method] 核心方法 +- *remove_target_socket()* *add_target_socket()* +- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出 + +## serialManager.py + 看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件 + +### **[Class]** serial_manager + 異步 event loop + 管理 mavlink_object 的地方 +- *self.thread* 自己的執行緒 +- *self.loop* 自己的事件迴圈 +--- +- *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通 +- *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序 +- *remove_serial_link()* [call method] 關閉指定的 serial 端口 +- *_async_remove_serial_link()* [async method] + +### **[Class]** serial_object + 被塞在 serial_manager 裡面 + 只是一個變數物件 + 用來被儲存 serial 的資訊 +- *self.transport* +- *self.protocol* +- *self.udp_handler* UDP 端口物件 +- *self.serial_handler* Serial 端口物件 + +### **[Class]** UDPHandler + 處理 UDP 收發的端口 作為一個端口物件 + 作為 UDP OutBound 使用 所以不會佔用系統監聽資源 +- *self.transport* 自己的傳輸物件 +--- +- *datagram_received()* 先加碼成 Xbee 再呼叫 Serial 端口物件送出 + +### **[Class]** SerialHandler + 處理 Serial 收發的端口 作為一個端口物件 +- *self.transport* 自己的傳輸物件 +--- +- *data_received()* 先組合 Serial 封包 再解碼 再呼叫 UDP 端口物件送出 + +## mavlinkVehicleView.py + 這個檔案是作為載具的資訊暫存庫使用 會搭配 ROS2 的功能 再做利用 + +# 開發記錄 + +## 已實現功能 +1. mavlink 分流解析 +2. mavlink socket 建立 +3. mavlink socket 轉拋 proxy +4. 建立 Serial 轉 UDP 連結 並管理 +5. 建立 serial 連線 +6. 各單元模組化 +7. 終端機介面控制 +8. 基礎載具流量觀測 +9. 載具狀態收集與彙整 +10. a. ros2 topic 應用開發介面 + +### 待開發功能 +5-1. 建立 serial 連線 並可以對接收器下達AT指令 +5-2. 模組化 serial 連線機制 以利後期擴容其他模組 +10. a. ros2 應用開發介面 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py new file mode 100644 index 0000000..ae89d72 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -0,0 +1,1597 @@ + +''' + +主要調配流程的程式 + +這個檔案包含 Terminal Utility Layer (TUL) 作為人機互動介面，並調用 mavlinkDevice 和 mavlinkObject 來處理 MAVLink 通訊和物件管理。 + +''' + +import os +import time +import sys + +import curses +import threading +import queue +import signal + +from pymavlink import mavutil + +# 自定義的 import +from . import mavlinkObject as mo +from . import serialManager as sm +from . import mavlinkVehicleView as mvv +from . import mavlinkROS2Nodes as mros + +from .utils import RingBuffer, setup_logger +from .utils import acquireSerial, acquirePort +from .utils.acquirePort import find_available_port + +logger = setup_logger(os.path.basename(__file__)) +VERSION_NO = "v0.59" + +class PanelState: + def __init__(self): + self.panel_status = "Idle" + termination_start_time = None + self.mavlink_bridge_state = "Stopped" + self.object_manager_state = "Stopped" + self.serial_manager_state = "Stopped" + self.ros2_manager_state = "Stopped" + self.socket_object_list = [] # 已有的 mavlink object + self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string + self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 + + # 關於創建通道時的暫存資訊 + self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊 + self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊 + + # 關於顯示通道資訊 + self.socket_info_single = { + "socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "", + "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "", + "InfoReady": False} # 暫存單一 socket 的資訊 + self.serial_info_single = { + "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "", + "InfoReady": False} # 暫存單一 serial 連結的資訊 + + # 關於顯示載具資訊 + self.connected_vehicles_dict = {} # {(sysid, compid): {...基本資訊...}} + self.vehicle_info_single = { + "sysid": 0, + "compid": 0, + "vehicle_type": "", + "component_type": "", + "mav_autopilot": "", + "socket_id": None, + "connection_type": "", + "packet_stats": {}, + "msg_type_counts": {}, + "prev_stats": {}, # 用於計算變化率 + "InfoReady": False + } + + def intoSTART(self): + self.panel_status = "Running" + + def intoTERMINATION(self): + self.termination_start_time = time.time() + self.panel_status = "Terminating" + + def intoENGINEER(self): + self.panel_status = "Engineer" + + def intoSTOPPED(self): + self.panel_status = "Stopped" + + # def set_user_input(self, text): + # self.user_input = text + +class MenuNode: + def __init__(self, name, desc="", action=None, children=None): + self.name = name + self.desc = desc + self.action = action # 可以是函式或特殊字串 + self.children = children or [] # 子選單列表 + +class ControlPanel: + def __init__(self): + pass + + def input_dialog(stdscr, prompt="請輸入文字: "): + """顯示輸入對話框""" + height, width = stdscr.getmaxyx() + + # 建立輸入視窗 + dialog_height = 5 + dialog_width = min(60, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + # 建立視窗邊框 + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(1, 2, prompt) + dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消") + dialog_win.refresh() + + # 輸入區域 + input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2) + input_win.keypad(True) + + curses.echo() + curses.curs_set(1) + + user_input = "" + + while True: + input_win.clear() + input_win.addstr(0, 0, user_input[-dialog_width+8:]) # 顯示輸入內容（滾動） + input_win.refresh() + + ch = input_win.getch() + + if ch == 27: # ESC + user_input = None + break + elif ch in (curses.KEY_ENTER, 10, 13): # Enter + break + elif ch in (curses.KEY_BACKSPACE, 127, 8): # Backspace + user_input = user_input[:-1] + elif 32 <= ch <= 126: # 可打印字符 + user_input += chr(ch) + + curses.noecho() + curses.curs_set(0) + + # 清理視窗 + del input_win + del dialog_win + stdscr.clear() + stdscr.refresh() + + return user_input + + # ================ 關於 主要選單 的部份 =================== + + def menu_tree(self): + """建立多層選單結構""" + return MenuNode("Main Menu", children=[ + MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[ + MenuNode("New+", children=[ + MenuNode("UDP InBound", children=[ + MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"), + ]), + MenuNode("UDP OutBound", children=[ + MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"), + MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"), + MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"), + ]), + ]), + MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"), + ]), + MenuNode("Serial Manager", "Serial 連接埠選項", children=[ + MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"), + MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"), + ]), + MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"), + MenuNode("Engineer Mode", "工程模式", children=[ + MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), + MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), + MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), + ]), + MenuNode("Shutdown", "關閉整個系統", children=[ + MenuNode("Return", "繼續運行", "BACK"), + MenuNode("Confirm", "關閉系統", "QUIT"), + ]), + ]) + + def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event): + stdscr = None + + def cleanup(): + """清理 curses 狀態""" + if stdscr: + stdscr.keypad(False) + curses.nocbreak() + curses.echo() + curses.endwin() + + def pre_panel_shutdown(): + # 先關閉所有模組 再關閉面板 + cmd_q.put("SHUTDOWN_BRIDGE") + cmd_q.put("SHUTDOWN_MANAGER") + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + def draw_menu(screen): + nonlocal stdscr + stdscr = screen + + curses.curs_set(0) + stdscr.nodelay(False) # 阻塞讀鍵 + stdscr.keypad(True) + + # 選單導航狀態 + menu_stack = [self.menu_tree()] # 選單堆疊 + idx_stack = [0] # 索引堆疊 + + state.intoSTART() # 設定狀態為運行中 + + while not stop_evt.is_set(): + + current_menu = menu_stack[-1] + current_idx = idx_stack[-1] + + # 獲取終端機尺寸 + height, width = stdscr.getmaxyx() + # 簡單暴力的限制視窗的大小 + MIN_HEIGHT = ( + 2 + # 邊界 + 6 + # 狀態列 操作說明列 一個空白 + 11+ # 最大選單 與 空白區 + 5 # 訊息區域 + ) + if height < MIN_HEIGHT or width < 60: + logger.error("Terminal size too small for Control Panel.") + break + + stdscr.clear() + stdscr.border() + + # 更新模組狀態顯示 + stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + stdscr.addstr(3, 2, f"Mavlink Bridge State : {state.mavlink_bridge_state}") + stdscr.addstr(4, 2, f"Socket Object number : {len(state.socket_object_list)}") + stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}") + stdscr.addstr(3, 36, f"ROS2 Manager State : {state.ros2_manager_state}") + + # 顯示當前選單項目 + start_line = 6 + for i, child in enumerate(current_menu.children): + marker = "➤ " if i == current_idx else " " + # 動態顯示已輸入的值 + desc = child.desc + if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]: + desc = f"{child.desc} [{state.udp_info_temp['IP']}]" + elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]: + desc = f"{child.desc} [{state.udp_info_temp['Port']}]" + elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]: + desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]" + elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]: + desc = f"{child.desc} [{state.serial_info_temp['Baud']}]" + elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP": + link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No" + desc = f"{child.desc} [{link_status}]" + + line = f"{marker}{child.name:15s} – {desc}" + attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL + stdscr.addstr(start_line + i, 4, line, attr) + + # 顯示訊息區域 + # info_start_line = start_line + len(current_menu.children) + 1 + info_start_line = height - 8 + max_msg_lines = 5 # 最多顯示 5 行訊息 + current_time = time.time() + + # 清理過時的訊息 + state.panel_info_msg_list = [ + (msg, timestamp) for msg, timestamp in state.panel_info_msg_list + if current_time - timestamp < 2.0 #秒數 + ] + + # 只顯示最新的 max_msg_lines 條訊息 + display_msgs = state.panel_info_msg_list[-max_msg_lines:] + + for i, msg_data in enumerate(display_msgs): + if info_start_line + i >= help_line - 1: # 避免超出邊界 + break + msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data + # 截斷過長的訊息 + max_msg_width = width - 6 + if len(msg) > max_msg_width: + msg = msg[:max_msg_width-3] + "..." + + stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD) + + + + # 操作說明 + # help_line = start_line + len(current_menu.children) + 2 + help_line = height - 2 + stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM) + stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM) + + stdscr.refresh() + + # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作 + # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈 + # 超過幾秒沒有反應就強制關閉 + if state.panel_status == "Terminating": + if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點 + logger.warning("Control Panel forced shutdown after timeout.") + state.intoSTOPPED() + # stop_evt.set() + # continue + break + time.sleep(0.1) + if (state.mavlink_bridge_state == "Stopped" and + state.object_manager_state == "Stopped" and + state.serial_manager_state == "Stopped"): + state.intoSTOPPED() + # stop_evt.set() + break + continue + + # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt + stdscr.timeout(100) + ch = stdscr.getch() + + if ch == -1: # 沒有操作 + continue + + # 處理按鍵 + if ch in (curses.KEY_UP, ord('k')): + idx_stack[-1] = (current_idx - 1) % len(current_menu.children) + + elif ch in (curses.KEY_DOWN, ord('j')): + idx_stack[-1] = (current_idx + 1) % len(current_menu.children) + + elif ch == (ord('O')): + # 進入工程模式 + state.intoENGINEER() + + elif ch == (ord('o')): + # 離開工程模式 + state.intoSTART() + + elif ch == curses.KEY_LEFT: + # 返回上層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + + elif ch == curses.KEY_RIGHT: + # 進入下層 (但不執行動作) + selected = current_menu.children[current_idx] + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif ch in (ord('q'), 27): + if state.panel_status == "Engineer": + state.intoTERMINATION() + pre_panel_shutdown() + + elif ch in (curses.KEY_ENTER, 10, 13): + selected = current_menu.children[current_idx] + + # 處理不同類型的動作 + if selected.children: # 有子選單 + menu_stack.append(selected) + idx_stack.append(0) + + elif selected.action == "BACK": + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "QUIT": + state.intoTERMINATION() + pre_panel_shutdown() + + elif selected.action == "TEXT_UDP_IP": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ") + if result is not None: + state.udp_info_temp["IP"] = result + + elif selected.action == "TEXT_UDP_PORT": + result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ") + if result is not None: + state.udp_info_temp["Port"] = result + + elif selected.action == "CREATE_UDP_INBOUND": + cmd_q.put("CREATE_UDP_INBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() + + elif selected.action == "CREATE_UDP_OUTBOUND": + cmd_q.put("CREATE_UDP_OUTBOUND") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # menu_stack.pop() + # idx_stack.pop() + + elif selected.action == "TEXT_BAUD_SERIAL": + result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ") + if result is not None: + try: + baud_rate = int(result) + except ValueError: + state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time())) + state.serial_info_temp["Baud"] = baud_rate + + elif selected.action == "SET_SERIAL_COMM_XBEE": + state.serial_info_temp["CommunicationType"] = "XBee(API-AT)" + menu_stack.pop() + idx_stack.pop() + elif selected.action == "SET_SERIAL_COMM_TELEMETRY": + state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)" + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES": + state.serial_info_temp["Go2Middleware"] = True + menu_stack.pop() + idx_stack.pop() + elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO": + state.serial_info_temp["Go2Middleware"] = False + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "CREATE_SERIAL_PORT": + state.serial_info_temp["Port"] = menu_stack[-1].name # 從選單取得 Port 名稱 + cmd_q.put("CREATE_SERIAL_PORT") + # 確認後回到上兩層 + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_SERIAL_RES": + created_list_menu = self.create_serial_port_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "LIST_SERIAL_LINKS": + created_list_menu = self.create_linked_serial_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_LINKED_SERIAL": + # 顯示 Serial 連結詳細資訊 + if hasattr(selected, 'serial_id'): + cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id)) + self.show_linked_serial_info(stdscr, selected.serial_id, state) + + elif selected.action == "REMOVE_LINKED_SERIAL": + # 移除 Serial 連結 + if hasattr(selected, 'serial_id'): + cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 一樣退兩層 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "LIST_MAV_OBJECT": + # 動態生成 mavlink_object 列表選單 + created_list_menu = self.create_object_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action in ("PREV_PAGE", "NEXT_PAGE"): + if hasattr(selected, 'page'): + current_list_menu = menu_stack[-1] + menu_stack.pop() + idx_stack.pop() + + # 依據選單種類 重新建立分頁 + if current_list_menu.name == "Serial Port List": + created_list_menu = self.create_serial_port_menu(state, page=selected.page) + elif current_list_menu.name == "Object List": + created_list_menu = self.create_object_list_menu(state, page=selected.page) + elif current_list_menu.name == "Linked Serial List": + created_list_menu = self.create_linked_serial_menu(state, page=selected.page) + elif current_list_menu.name == "Connected Vehicles": + created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) + else: + # 不支援的選單類型，回到原本的選單 + menu_stack.append(current_list_menu) + idx_stack.append(0) + continue + + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_MAV_OBJECT": + # 顯示物件詳細資訊 + if hasattr(selected, 'socket_id'): + cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id)) + self.show_object_info(stdscr, selected.socket_id, state) + + elif selected.action == "REMOVE_MAV_OBJECT": + # 移除物件 + if hasattr(selected, 'socket_id'): + cmd_q.put(("REMOVE_OBJECT", selected.socket_id)) + # 返回上層（回到列表） + if len(menu_stack) > 1: + menu_stack.pop() + idx_stack.pop() + # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好 + menu_stack.pop() + idx_stack.pop() + + elif selected.action == "MAVOBJ_MAKE_LINK": + # 建立轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id)) # 雙向連結 + + elif selected.action == "MAVOBJ_CANCEL_LINK": + # 取消轉發連結 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True) + if target_id is not None: + cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id)) + cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id)) # 雙向取消連結 + + elif selected.action == "MAVOBJ_ADD_TARGET": + # 添加目標端口 + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + if hasattr(selected, 'socket_id'): + target_id = self.select_target_socket(stdscr, selected.socket_id, state) + if target_id is not None: + cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id)) + + elif selected.action == "STOP_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_MANAGER") + + elif selected.action == "STOP_BRIDGE": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_BRIDGE") + + elif selected.action == "STOP_SERIAL_MANAGER": + if state.panel_status != "Engineer": + state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time())) + continue # 只有在工程模式下才能操作 + cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + + elif selected.action == "INSPECT_VEHICLES": + # 進入載具檢視選單 + cmd_q.put("UPDATE_VEHICLES_LIST") + created_list_menu = self.create_vehicles_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_VEHICLE": + # 顯示載具詳細資訊 + if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): + cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) + self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) + + elif callable(selected.action): + # 執行函式 + cmd_q.put(selected.action) + + try: + curses.wrapper(draw_menu) + except KeyboardInterrupt: + pass + finally: + cleanup() + + # ================ 關於 mavlink object 的部份 =================== + + def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 mavlink_object 列表選單（支持分頁）""" + children = [] + + if not state.socket_object_list: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.socket_object_list) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + for socket_id in state.socket_object_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"), + MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"), + MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"), + MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"), + MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 socket_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.socket_id = socket_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_object_info(self, stdscr, socket_id, state: PanelState): + """顯示物件詳細資訊的對話框""" + + start = time.time() + while not state.socket_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD) + + # 這裡顯示基本資訊 + dialog_win.addstr(2, 2, f"Socket ID : {socket_id}") + dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}") + # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', ''))) + dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}") + dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}") + show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', ''))) + dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}") + show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', ''))) + dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}") + dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}") + show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', ''))) + dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}") + + state.socket_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False): + """選擇目標 socket 的對話框""" + height, width = stdscr.getmaxyx() + dialog_height = min(15, len(state.socket_object_list) + 5) + dialog_width = min(50, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.keypad(True) + + title = "選擇要移除的目標" if remove_mode else "選擇轉發目標" + available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id] + + if not available_sockets: + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + dialog_win.addstr(2, 2, "沒有可用的目標") + dialog_win.addstr(4, 2, "按任意鍵返回...") + dialog_win.refresh() + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + selected_idx = 0 + + while True: + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD) + + for i, socket_id in enumerate(available_sockets): + marker = "➤" if i == selected_idx else " " + attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL + dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr) + + dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消") + dialog_win.refresh() + + ch = dialog_win.getch() + + if ch in (curses.KEY_UP, ord('k')): + selected_idx = (selected_idx - 1) % len(available_sockets) + elif ch in (curses.KEY_DOWN, ord('j')): + selected_idx = (selected_idx + 1) % len(available_sockets) + elif ch in (curses.KEY_ENTER, 10, 13): + result = available_sockets[selected_idx] + del dialog_win + stdscr.clear() + stdscr.refresh() + return result + elif ch == 27: # ESC + del dialog_win + stdscr.clear() + stdscr.refresh() + return None + + # ================ 關於 serial link 的部份 =================== + + def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 serial port 列表選單（支持分頁）""" + children = [] + + # 獲取可用的 Serial 連接埠列表 + # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓 + serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + + if not serial_ports: + children.append(MenuNode("(Empty)", "目前沒有串口設備", None)) + else: + total_items = len(serial_ports) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的串口 + for port in serial_ports[start_idx:end_idx]: + port_menu = MenuNode(f"{port}", children=[ + MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[ + MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"), + # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"), + ]), + MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"), + MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[ + MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"), + MenuNode("No", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"), + ]), + MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 port 附加到每個子選單項目上 + for child in port_menu.children: + child.port = port + children.append(port_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能""" + children = [] + + if not state.linked_serial_dict: + children.append(MenuNode("(Empty)", "目前沒有連結口", None)) + else: + total_items = len(state.linked_serial_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # 顯示當前頁的物件 + linked_serial_id_list = list(state.linked_serial_dict.keys()) + for serial_id in linked_serial_id_list[start_idx:end_idx]: + # 為每個 socket 創建子選單 + obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[ + MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"), + MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"), + # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"), + MenuNode("GoUp", "回到列表", "BACK"), + ]) + # 將 serial_id 附加到每個子選單項目上 + for child in obj_menu.children: + child.serial_id = serial_id + children.append(obj_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_linked_serial_info(self, stdscr, serial_id, state: PanelState): + """顯示 Serial 連結詳細資訊的對話框""" + + start = time.time() + while not state.serial_info_single.get('InfoReady', False): + # 太久沒有回應 + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time())) + return + time.sleep(0.05) # 等待資訊準備好 + + height, width = stdscr.getmaxyx() + dialog_height = 15 + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.border() + dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD) + + # 從 linked_serial_dict 獲取資訊 + serial_info = state.linked_serial_dict.get(serial_id, {}) + + if not serial_info: + dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊") + else: + # 顯示基本資訊 + dialog_win.addstr(2, 2, f"Serial ID : {serial_id}") + dialog_win.addstr(3, 2, f"Serial Port : {state.serial_info_single.get('serial_port', 'N/A')}") + dialog_win.addstr(4, 2, f"Baudrate : {state.serial_info_single.get('baudrate', 'N/A')}") + dialog_win.addstr(5, 2, f"Communication : {state.serial_info_single.get('receiver_type', 'N/A')}") + dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}") + dialog_win.addstr(7, 2, f"Status : {state.serial_info_single.get('status', 'Running')}") + + # 如果有額外資訊可以繼續添加 + if 'thread_alive' in serial_info: + thread_status = "Alive" if serial_info['thread_alive'] else "Stopped" + dialog_win.addstr(8, 2, f"Thread Status : {thread_status}") + + state.serial_info_single['InfoReady'] = False # 重置狀態以便下次使用 + + dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...") + dialog_win.refresh() + + dialog_win.getch() + del dialog_win + stdscr.clear() + stdscr.refresh() + + # ================ 關於載具檢視的部份 =================== + + def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): + """動態創建 已連線載具 列表選單（支持分頁） + 每個 vehicle-component 組合都是獨立的選單項目 + """ + children = [] + + if not state.connected_vehicles_dict: + children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None)) + else: + total_items = len(state.connected_vehicles_dict) + total_pages = (total_items + items_per_page - 1) // items_per_page + start_idx = page * items_per_page + end_idx = min(start_idx + items_per_page, total_items) + + # vehicle_id_list 現在是 (sysid, compid) 的元組列表 + vehicle_comp_list = list(state.connected_vehicles_dict.keys()) + + # 顯示當前頁的物件 + for sysid, compid in vehicle_comp_list[start_idx:end_idx]: + # 建立顯示名稱 + display_name = f"Vehicle #{sysid} - Comp #{compid}" + desc = f"載具 {sysid} 組件 {compid}" + + vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") + # 將 sysid 和 compid 附加到選單項目上 + vehicle_menu.sysid = sysid + vehicle_menu.compid = compid + children.append(vehicle_menu) + + # 添加分頁控制 + if total_pages > 1: + children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None)) + if page > 0: + prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE") + prev_node.page = page - 1 + children.append(prev_node) + if page < total_pages - 1: + next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE") + next_node.page = page + 1 + children.append(next_node) + + children.append(MenuNode("GoUp", "回到上層選單", "BACK")) + menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children) + menu.current_page = page + return menu + + def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): + """顯示載具組件詳細資訊（動態更新，顯示變化率）""" + + # 等待資訊準備 + start = time.time() + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) + return + time.sleep(0.05) + + info = state.vehicle_info_single + height, width = stdscr.getmaxyx() + dialog_height = min(22, height - 4) + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 + dialog_win.keypad(True) + + # MAV_TYPE 名稱對應 + MAV_TYPE_NAMES = { + 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", + 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", + 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" + } + + # 動態更新迴圈 + last_update = time.time() + while True: + current_time = time.time() + + # 每 1 秒重新請求資料 + if current_time - last_update >= 1.0: + # 觸發資料更新（透過 Orchestrator） + cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) + # 等待新資料準備好 + wait_start = time.time() + state.vehicle_info_single['InfoReady'] = False + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - wait_start > 0.5: # 最多等 0.5 秒 + break + time.sleep(0.01) + # 更新 info 參照 + info = state.vehicle_info_single + last_update = current_time + + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) + + # === 基礎資訊 === + dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) + dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) + + # # MAV Type # 這個用不到 + # mav_type = info.get('vehicle_type', 'N/A') + # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) + # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") + + # Component Type + dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") + + # Autopilot Type + if info.get('mav_autopilot') is not None: + dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") + + # Connection Info + dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") + dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") + + # === 封包統計 === + stats = info.get('packet_stats', {}) + dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) + + received = stats.get('received', 0) + lost = stats.get('lost', 0) + loss_rate = stats.get('loss_rate', 0.0) + last_seq = stats.get('last_seq', 'N/A') + + # 計算變化 + received_delta = stats.get('received_delta', 0) + lost_delta = stats.get('lost_delta', 0) + + # 顯示變化率 + recv_str = f"{received:6d}" + if received_delta > 0: + recv_str += f" (+{received_delta}↑)" + + lost_str = f"{lost:4d}" + if lost_delta > 0: + lost_str += f" (+{lost_delta}↑)" + + dialog_win.addstr(8, 2, f"Received : {recv_str}") + dialog_win.addstr(8, 32, f"Lost : {lost_str}") + dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") + dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") + + # 最後接收時間 + last_msg_time = stats.get('last_msg_time') + if last_msg_time: + time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) + elapsed = current_time - last_msg_time + dialog_win.addstr(10, 2, f"Last Time : {time_str}") + dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") + else: + dialog_win.addstr(10, 2, "Last Time : N/A") + + # === 訊息類型分佈 === + dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) + + msg_counts = info.get('msg_type_counts', {}) + + # MAVLink 訊息名稱對應（縮寫版本） + MSG_NAMES = { + 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", + 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", + 74: "VFR_HUD", 147: "BATT_ST" + } + + # 顯示前 12 個最常見的訊息類型（兩列各 6 個） + msg_items = list(msg_counts.items())[:12] + line = 13 + for i, (msg_id, count) in enumerate(msg_items): + msg_name = MSG_NAMES.get(msg_id, "???") + delta = stats.get(f'msg_delta_{msg_id}', 0) + + # 格式化數據 + if delta > 0: + data_str = f"{count}(+{delta}↑)" + else: + data_str = f"{count}" + + # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) + display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" + + # 左列(偶數索引)或右列(奇數索引) + col = 2 if i % 2 == 0 else 36 + row = line + (i // 2) + + if row >= dialog_height - 3: # 避免超出邊界 + break + + dialog_win.addstr(row, col, display_str) + + # 確保跳過顯示區域 + line = line + 6 + + dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...") + dialog_win.refresh() + + # 檢查是否有按鍵（非阻塞） + ch = dialog_win.getch() + if ch in (ord('R'), ord('r')): + cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid)) + elif ch in (ord('C'), ord('c')): + cmd_q.put(("UNREGISTER_VEHICLE", sysid)) + break + elif ch != -1: # 有按鍵則退出 + break + + # 短暫延遲 + time.sleep(0.1) + + state.vehicle_info_single['InfoReady'] = False + del dialog_win + stdscr.clear() + stdscr.refresh() + + + +class Orchestrator: + def __init__(self, stop_sig): + self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個) + self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...] + + self.vehicle_registry = mvv.vehicle_registry + + # === 1) 面板部分的準備 === + self.cmd_q = queue.Queue() + self.panelState = PanelState() # 面板的狀態儲存 + self.cPanel = ControlPanel() # 面板的功能 + self.panel_thread = None + + # === 2) async_io_manager & mavlink_bridge 部分的準備 === + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + self.manager = mo.async_io_manager() + self.bridge = mo.mavlink_bridge() + + # === 3) serial_manager 部分的準備 === + self.plumber = sm.serial_manager() + + # === 4) ros 部分的準備 === + self.fc_ros_manager = mros.ros2_manager + if not self.fc_ros_manager.initialized: + self.fc_ros_manager.initialize() + self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = { + 'position': 1.0, + 'attitude': 0.0, + 'velocity': 0.0, + 'battery': 1.0, + 'vfr_hud': 1.0, + 'mode': 0.0, + 'summary': 1.0, + } + + def engageWholeSystem(self): + """啟動整個系統""" + # === 1) 面板部分的啟動 === + self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) + self.panel_thread.start() + + # === 2) async_io_manager & mavlink_bridge 部分的啟動 === + self.manager.start() + self.bridge.start() + + # === 3) serial_manager 部分的啟動 === + self.plumber.start() + + # === 4) ros 部分的啟動 === + self.fc_ros_manager.start() + + def mainLoop(self): + logger.info("Main orchestrator started <-") + try: + # while not self.stop_evt.is_set(): + while self.panel_thread.is_alive(): + + # A. 更新模組狀態 + if self.manager.running: + self.panelState.object_manager_state = 'Running' + else: + self.panelState.object_manager_state = 'Stopped' + + socketid_list = self.manager.get_managed_objects() + self.panelState.socket_object_list = socketid_list + + if self.bridge.running: + self.panelState.mavlink_bridge_state = 'Running' + else: + self.panelState.mavlink_bridge_state = 'Stopped' + + if self.plumber.running: + self.panelState.serial_manager_state = 'Running' + else: + self.panelState.serial_manager_state = 'Stopped' + + linked_serial_dict = self.plumber.get_serial_link() + self.panelState.linked_serial_dict = linked_serial_dict + + if self.fc_ros_manager.running: + self.panelState.ros2_manager_state = 'Running' + else: + self.panelState.ros2_manager_state = 'Stopped' + + # B. 更新載具列表（從 vehicle_registry 獲取） + self._update_vehicles_list() + + # 取出面板丟過來的「動作」 + try: + cmd = self.cmd_q.get_nowait() + if callable(cmd): + cmd() # 執行對應動作 + elif isinstance(cmd, tuple): + # 處理多參數命令 + action = cmd[0] + if action == "REMOVE_OBJECT": + socket_id = cmd[1] + # 先移除所有跟他關聯的 target sockets + for s_id in mo.mavlink_object.mavlinkObjects: + s_obj = mo.mavlink_object.mavlinkObjects[s_id] + if socket_id in s_obj.target_sockets: + self.remove_target_from_object(s_id, socket_id) + # 再移除該物件 + self.delete_udp_object(socket_id) + elif action == "MAVOBJ_ADD_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.add_target_to_object(source_id, target_id) + elif action == "MAVOBJ_REMOVE_TARGET": + source_id, target_id = cmd[1], cmd[2] + self.remove_target_from_object(source_id, target_id) + elif action == "INSPECT_MAV_OBJECT": + socket_id = cmd[1] + mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None) + if mav_obj: + self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type + self.panelState.socket_info_single["socket_state"] = mav_obj.state.name + self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types + self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types + self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id + self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets + self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address + self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "INSPECT_LINKED_SERIAL": + serial_id = cmd[1] + serial_obj = self.plumber.serial_objects.get(serial_id, None) + if serial_obj: + self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port + self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate + self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name + self.panelState.serial_info_single["target_port"] = serial_obj.target_port + self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好 + elif action == "REMOVE_LINKED_SERIAL": + serial_id = cmd[1] + self.plumber.remove_serial_link(serial_id) + + elif action == "INSPECT_VEHICLE": + sysid, compid = cmd[1], cmd[2] + self._prepare_vehicle_info(sysid, compid) + # elif action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug? + # logger.debug("Orchestrator: Updating vehicles list upon request") + # self._update_vehicles_list() + elif action == "RESET_VEHICLE_STATISTICS": + sysid, compid = cmd[1], cmd[2] + vehicle_sys = self.vehicle_registry.get(sysid) + vehicle_sys.reset_component_stats(compid) + elif action == "UNREGISTER_VEHICLE": + sysid = cmd[1] + self.vehicle_registry.unregister(sysid) + + elif cmd == "CREATE_UDP_INBOUND": + self.panelState.udp_info_temp["direction"] = "inbound" + self.create_udp_object() + elif cmd == "CREATE_UDP_OUTBOUND": + self.panelState.udp_info_temp["direction"] = "outbound" + self.create_udp_object() + elif cmd == "CREATE_SERIAL_PORT": + self.create_serial_port_object() + elif cmd == "SHUTDOWN_BRIDGE": + self.bridge.stop() + elif cmd == "SHUTDOWN_MANAGER": + self.manager.shutdown() + elif cmd == "SHUTDOWN_SERIAL_MANAGER": + self.plumber.shutdown() + + except queue.Empty: + pass + except Exception as e: + logger.error(f"Error processing command: {e}") + + time.sleep(0.1) + + except KeyboardInterrupt: + pass + except Exception as e: + logger.error(f"Unexpected error in main loop: {e}") + finally: + + # 驗證並確保所有模組都被下達關閉訊號 + # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 + if self.bridge.thread.is_alive(): + if self.bridge.running: + self.bridge.stop() + self.bridge.thread.join(timeout=2) + + if self.manager.thread.is_alive(): + if self.manager.running: + self.manager.shutdown() + self.manager.thread.join(timeout=2) + + if self.plumber.thread.is_alive(): + if self.plumber.running: + self.plumber.shutdown() + self.plumber.thread.join(timeout=2) + + if self.fc_ros_manager.spin_thread.is_alive(): + if self.fc_ros_manager.running: + self.fc_ros_manager.stop() + self.fc_ros_manager.spin_thread.join(timeout=2) + + # 關閉面板執行緒 + if self.panel_thread.is_alive(): + self.panel_thread.join(timeout=2) + + time.sleep(0.5) # 等待各模組穩定關閉 + + logger.info("Main orchestrator END!") + + # =============== 面板動作 - Mavlink Object =============== + + def create_udp_object(self, socket_type:str = ""): + # 監聽部分 + if self.panelState.udp_info_temp["direction"] == "inbound": + connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + # 監聽的 port 要先檢查是否被佔用 + ip = self.panelState.udp_info_temp['IP'] + port = int(self.panelState.udp_info_temp['Port']) + port_check_list = self.occupied_ip_ports.get(ip, []) + self.occupied_ip_ports.get("0.0.0.0", []) + if port in port_check_list: + self.panelState.panel_info_msg_list.append((f"Failed! Port {port} on IP {ip} occupied.", time.time()-1)) + return + # 再記錄被佔用的 port + if ip in self.occupied_ip_ports: + self.occupied_ip_ports[ip].append(port) + else: + self.occupied_ip_ports[ip] = [port] + # 外放資訊部分 + elif self.panelState.udp_info_temp["direction"] == "outbound": + connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" + + try: + # 檢測這個 connection_string 是否能成功建立 mavlink 連結 + mavlink_socket = mavutil.mavlink_connection(connection_string) + except Exception as e: + self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) + return + + # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句 + mavlink_object = mo.mavlink_object(mavlink_socket) + # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句 + self.manager.add_mavlink_object(mavlink_object) + + # 設定一下 mavlink_object 的類型描述 + if socket_type == "": + mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + else: + mavlink_object.socket_type = socket_type + + self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) + + def delete_udp_object(self, socket_id): + """移除指定的 mavlink_object""" + + mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id] + connection_string = mavlink_obj.mavlink_socket.address + strings = connection_string.split(':') + ip = strings[0] + port = int(strings[1]) + self.occupied_ip_ports[ip].remove(port) + + self.manager.remove_mavlink_object(socket_id) + + def add_target_to_object(self, source_id, target_id): + """為 mavlink_object 添加轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.add_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Added target {target_id} to socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Adding target {target_id} to socket {source_id}", time.time())) + + def remove_target_from_object(self, source_id, target_id): + """從 mavlink_object 移除轉發目標""" + if source_id in mo.mavlink_object.mavlinkObjects: + source_obj = mo.mavlink_object.mavlinkObjects[source_id] + else: + self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time())) + return + + if source_obj.remove_target_socket(target_id): + self.panelState.panel_info_msg_list.append((f"Removed target {target_id} from socket {source_id}", time.time())) + else: + self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + + # =============== 面板動作 - Vehicle Inspector =============== + + def _update_vehicles_list(self): + """更新已連線載具列表（從 vehicle_registry 獲取）""" + vehicles_dict = {} + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = self.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + # 遍歷每個載具的所有組件 + for compid, component in vehicle.components.items(): + # 使用 (sysid, compid) 作為 key + vehicles_dict[(sysid, compid)] = { + 'sysid': sysid, + 'compid': compid, + 'vehicle_type': vehicle.vehicle_type, + 'component_type': component.type.value, + 'connection_via': vehicle.connected_via.value, + 'socket_id': vehicle.custom_meta.get('socket_id', 'N/A') + } + + self.panelState.connected_vehicles_dict = vehicles_dict + + def _prepare_vehicle_info(self, sysid, compid): + """準備載具組件的詳細資訊（包含變化率計算）""" + vehicle = self.vehicle_registry.get(sysid) + if not vehicle: + logger.warning(f"Vehicle {sysid} not found") + return + + socket_id = vehicle.custom_meta.get('socket_id', 'N/A') + + component = vehicle.get_component(compid) + if not component: + logger.warning(f"Component {compid} not found in vehicle {sysid}") + return + + stats = component.packet_stats + + # 取得之前的統計資料（用於計算變化） + prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {}) + prev_received = prev_stats.get('received', 0) + prev_lost = prev_stats.get('lost', 0) + prev_msg_counts = prev_stats.get('msg_counts', {}) + + # 計算變化率 + received_delta = stats.received_count - prev_received + lost_delta = stats.lost_count - prev_lost + + # 準備訊息類型計數（排序後取前幾個） + sorted_msg_counts = dict(sorted( + stats.msg_type_count.items(), + key=lambda x: x[1], + reverse=True + )[:12]) # 取前 12 個最常見的 + + # 計算每種訊息類型的變化 + msg_deltas = {} + for msg_id, count in sorted_msg_counts.items(): + prev_count = prev_msg_counts.get(msg_id, 0) + msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count + + # 更新 vehicle_info_single + socket_type = "N/A" + socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None) + if socket_obj: + socket_type = socket_obj.socket_type + + self.panelState.vehicle_info_single = { + "sysid": sysid, + "compid": compid, + # "vehicle_type": vehicle.vehicle_type, # 這個用不到 + "component_type": component.type.value, + "mav_autopilot": component.mav_autopilot, + "socket_id": socket_id, + "connection_type": socket_type, + "packet_stats": { + "received": stats.received_count, + "lost": stats.lost_count, + "loss_rate": (stats.lost_count / stats.received_count * 100 + if stats.received_count > 0 else 0), + "last_seq": stats.last_seq, + "last_msg_time": stats.last_msg_time, + "received_delta": received_delta, + "lost_delta": lost_delta, + **msg_deltas # 展開訊息類型的變化 + }, + "msg_type_counts": sorted_msg_counts, + "prev_stats": { # 保存當前數據用於下次計算變化 + "received": stats.received_count, + "lost": stats.lost_count, + "msg_counts": dict(stats.msg_type_count) + }, + "InfoReady": True + } + + # =============== 面板動作 - Serial Manager =============== + + def create_serial_port_object(self): + # 先檢查是否已有相同的 Serial Port 被建立 + serial_port_strs = self.panelState.linked_serial_dict.values() # linked_serial_dict 會在上面的 mainLoop 被不斷更新 + if self.panelState.serial_info_temp['Port'] in serial_port_strs: + self.panelState.panel_info_msg_list.append( + (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time()) + ) + return + + # 獲取可用的 udp port + udp_port_tmp = find_available_port(19000, 20000) + + # 定義通訊類型映射表 + COMM_TYPE_MAP = { + "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT, + # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY, # TODO: 之後再弄 + # 新增區 + } + + # 驗證輸入 + comm_type = self.panelState.serial_info_temp['CommunicationType'] + if not comm_type: + self.panelState.panel_info_msg_list.append( + ("Please select Communication Type first.", time.time()) + ) + return + + # 查找對應的通訊類型 + comm_type_tmp = COMM_TYPE_MAP.get(comm_type) + if comm_type_tmp is None: + self.panelState.panel_info_msg_list.append( + (f"Communication type '{comm_type}' not supported yet.", time.time()) + ) + return + + ret = self.plumber.create_serial_link( + serial_port=self.panelState.serial_info_temp['Port'], + baudrate=self.panelState.serial_info_temp['Baud'], + target_port=udp_port_tmp, + receiver_type=comm_type_tmp, + ) + + if not ret: + self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + return + + self.panelState.panel_info_msg_list.append((f"Created Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time())) + + # 自動建立對應的 UDP 監聽端口 + if self.panelState.serial_info_temp['Go2Middleware']: + self.panelState.udp_info_temp['IP'] = "127.0.0.1" + self.panelState.udp_info_temp['Port'] = str(udp_port_tmp) + self.panelState.udp_info_temp['direction'] = "inbound" + self.create_udp_object("SERIAL_XBEE") + + +def main(): + + stop_evt = threading.Event() + + def signal_handler(signum, frame): + """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組""" + stop_evt.set() + + # 註冊信號處理器 + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + orchestrator = Orchestrator(stop_evt) + orchestrator.engageWholeSystem() + orchestrator.mainLoop() # 程式會 block 在這邊 直到收到中斷信號 + +if __name__ == "__main__": + main() + + +''' +================= 改版記錄 ============================ + +2025.12.23 +1. 新增 serial 通道的資訊顯示完整化 +2. 新增 serial 通道刪除功能 +3. 新增 serial 直接順便開 ip object +4. 修改 避免 serial 與 ip port 重複建立相同的通道 +5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死 + +2025.01.16 +1. 新增 "移除載具" 與 "重置載具統計" 功能 +2. 修正 udp port 在移除後仍被記錄為佔用的問題 +3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 +4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 +5. 系統納入 mavlink ROS2 Manager 模組 + +''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 88046b8..c8cfaed 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,60 +1,88 @@ ''' -# 不同的匯流排只有單一種通訊協定 -# 匯流排接到訊息後透過 queue stack 傳送到橋接器 -# 會有三種 Queue 分別作為 1. 固定串流橋接器 2. 回傳封包處理器 3. 轉拋串流 -# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue -# 橋接器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 +這個檔案是對 udp 進來的 mavlink 訊息做 "分流" "轉拋" 的地方 (並不會做 "分析") +概念上 把每個 udp 接口 視為一個獨立的 mavlink bus 針對 bus 來統籌管理 + +主要的重點部分: +1. stream_bridge_ring & return_packet_ring +2. mavlink_object & async_io_manager +3. mavlink_bridge + + +stream_bridge_ring & return_packet_ring: + 這兩個 ring buffer 是用來做 mavlink 訊息的分流 + stream_bridge_ring 這邊的資訊比較是給會固定更新的資訊 (例如 HEARTBEAT, ATTITUDE 之類的) + return_packet_ring 比較是處理指令後的回應封包 (例如 PARAM_VALUE, MISSION_ITEM 等等) + +mavlink_object: + 每個 mavlink bus 都會有一個 mavlink_object + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 內容中沒有獨立的執行緒 只有一個個 asyncio function 會被放到 async_io_manager 裡面執行 + + 關於分流與轉拋的具體實現是在 process_data 這個 asyncio function 裡面 + +async_io_manager: + 首先它紀律並管理所有 mavlink_object 實例 + 有自己一個獨立的執行緒 執行 asyncio loop (mavlink_object 裡面的 asyncio function 都會被放到這個 loop 裡面執行) + +mavlink_bridge: + 專門處理 stream_bridge_ring 裡面的訊息流 + 會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖 + + -# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 -# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlink_object 裡面 -# 這邊的 main 會是用來初始化 mavlink_object 並啟動他的 run function ''' + # 基礎功能的 import -import threading -import queue +import os +import signal import time +import threading +import asyncio +from enum import Enum, auto +from collections import deque +# from typing import Dict, List, Optional, Set, Any, Tuple # mavlink 的 import from pymavlink import mavutil -# ROS2 的 import -from rclpy.node import Node -import std_msgs.msg - # 自定義的 import -from mavlinkDevice import mavlink_device, mavlink_systems -from mavlinkPublish import mavlink_publisher -from theLogger import setup_logger +from .mavlinkVehicleView import ( + vehicle_registry, + VehicleView, + VehicleComponent, + ComponentType, + ConnectionType +) +from .utils import RingBuffer, setup_logger # ====================== 分割線 ===================== -logger = setup_logger("mavlinkObject.py") +logger = setup_logger(os.path.basename(__file__)) -fixed_stream_bridge_queue = queue.Queue() -return_packet_processor_queue = queue.Queue() -swap_queues = [] # 這個 list 用來存放轉拋串流的 queue # 這些 queue 同時也是各自 mavlink_object 的 output buffer +stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255) +return_packet_ring = RingBuffer(capacity=256, buffer_id=254) # ====================== 分割線 ===================== -class mavlink_bridge(Node, mavlink_publisher): +# 使用 vehicle_registry 來管理所有的載具視圖 +# vehicle_registry 是從 mavlinkVehicleView 導入的全域實例 + +class mavlink_bridge: ''' 這個 class 就是 固定串流橋接器 - 是用來接收 mavlink 訊息 並進行橋接 - 這個地方是針對 fixed_stream_bridge_queue 的資料做處理的 + 是用來接收 mavlink 訊息 並進行橋接處理 + 這個地方是針對 stream_bridge_ring 的資料做處理的 記錄有 mavlink bus 上有那些 system id 和 component id 為了每個 system id 都有一個對應的 device object - 並且看是否有重複 system id - - 整段代碼包含兩大區塊 thread 和 node - - thread 區塊內會對 fixed_stream_bridge_queue 進行監聽 並且將收到的訊息進行處理 - 其中 HEARTBEAT 是一個特殊類別 用來初始化整個 device object - - node 區塊則是處理 ros2 的 publisher 和 subscriber 訂閱相關 - 藉由控制 ros2 的機制再把 device object 的資訊發送出去 - - ps. 我限制了這個 class 只能有一個 instance + + 此類別負責： + 1. 從 stream_bridge_ring 接收訊息 + 2. 管理 mavlink_systems（device 和 component objects） + 3. 處理接收到的訊息並更新對應的 component 狀態 + 4. 提供發送訊息的介面，將訊息路由到正確的 mavlink_object + + ps. 此 class 為 singleton，只能有一個 instance ''' _instance = None _lock = threading.Lock() # 確保多線程安全 @@ -68,139 +96,257 @@ class mavlink_bridge(Node, mavlink_publisher): def __init__(self): if not hasattr(self, "initialized"): # 防止重複初始化 self.initialized = True + self.thread = None + self.running = False - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems + # 初始化訊息處理器字典 (msg_id -> handler_function) + self._init_message_handlers() + else: + logger.error('mavlink_bridge instance already exists. Do not create another one.') - # 當 object 建立時會直接運行 thread 直到消滅 + def _init_message_handlers(self): + """初始化訊息處理器映射表，提高處理效率""" + self.message_handlers = { + 0: self._handle_heartbeat, # HEARTBEAT + 30: self._handle_attitude, # ATTITUDE + 32: self._handle_local_position, # LOCAL_POSITION_NED + 33: self._handle_global_position, # GLOBAL_POSITION_INT + 74: self._handle_vfr_hud, # VFR_HUD + 147: self._handle_battery_status, # BATTERY_STATUS + } + + def start(self): + """啟動 mavlink_bridge 的運作""" + if not self.running: self.running = True - self.thread = threading.Thread(target=self._run_thread) + self.thread = threading.Thread(target=self._run_thread, name="MavlinkBridge") self.thread.start() else: - logger.error('mavlink_bridge instance already exists. Do not create another one.') + logger.warning("mavlink_bridge is already running") def stop(self): + """停止 mavlink_bridge 的運作""" self.running = False + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) # === Thread 區塊 === def _run_thread(self): - # start_time = time.time() # debug - logger.info('Start of mavlink_bridge._run_thread') - # 從 Queue fixed_stream_bridge_queue 中取出 mavlink 資料 並呼叫相對應的 function 進行後處理 + """主執行緒：從 stream_bridge_ring 接收並處理 mavlink 訊息""" + logger.info('mavlink_bridge started <-') + while self.running: - if fixed_stream_bridge_queue.empty(): + # 檢查是否有訊息 + if stream_bridge_ring.is_empty(): + time.sleep(0.001) # 避免忙碌等待 continue - msg_pack = fixed_stream_bridge_queue.get() - - msg = msg_pack[2] + + # 取出訊息包：(socket_id, timestamp, mavlink_msg) + msg_pack = stream_bridge_ring.get() + socket_id, timestamp, msg = msg_pack[0], msg_pack[1], 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: - this_device = mavlink_device() # 創建一個新的 device object - self.mavlink_systems[sysid] = this_device - this_device.socket_id = msg_pack[0] - this_device.sysid = sysid - else: - this_device = self.mavlink_systems[sysid] - - # 若該 component id 存在 則直接使用該 component object - # 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object - # 若該 component id 不存在 又不是 heartbeat 則不處理 - if compid in self.mavlink_systems[sysid].components: - this_component = self.mavlink_systems[sysid].components[compid] - elif msg_id == 0: - # 只有透過 heartbeat 可以創建一個新的 component object - this_component = this_device.mavlink_component() - this_device.components[msg.get_srcComponent()] = this_component - this_component.mav_type = msg.type - this_component.mav_autopilot = msg.autopilot - this_component.sysid = sysid # ★ 新增 - this_component.compid = compid + + # 確保 VehicleView 存在 + vehicle = vehicle_registry.get(sysid) + if vehicle is None: + vehicle = vehicle_registry.register(sysid) + # 存儲 socket_id 到自定義 meta 中 + vehicle.custom_meta['socket_id'] = socket_id + + # 確保 VehicleComponent 存在 + component = vehicle.get_component(compid) + if component is None: + if msg_id == 0: # 只有透過 HEARTBEAT 才能創建新的 component + # 根據 mav_type 判斷 component 類型 + comp_type = self._determine_component_type(msg.type) + component = vehicle.add_component(compid, comp_type) + component.mav_type = msg.type + component.mav_autopilot = msg.autopilot + else: + # component 不存在且非 HEARTBEAT，忽略此訊息 + continue + + # 使用處理器字典分發訊息處理 + if msg_id in self.message_handlers: + try: + self.message_handlers[msg_id](vehicle, component, msg, timestamp) + except Exception as e: + logger.error(f'Error handling message type {msg_id}: {e}') else: - continue - - # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓ - - if msg_id == 0: # HEARTBEAT 處理 - this_component.emitParams['heartbeat'] = msg - self.ensure_all_publishers(sysid, this_component) - - 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 == 111: #TIME_SYNC - round_trip_time = std_msgs.msg.Float64() - round_trip_time.data = (int(time.time() * 1e9) - msg.ts1) / 1e6 - if(round_trip_time.data < 1e6): - this_component.emitParams['ping'] = round_trip_time - - elif msg_id == 147: # BATTERY_STATUS 處理 - this_component.emitParams['battery'] = msg - - # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↑↑↑↑↑↑↑↑↑↑↑↑ + logger.debug(f'Unhandled message type: {msg_id} / {msg.get_type()}') + + logger.info('mavlink_bridge END!') + + def _determine_component_type(self, mav_type: int) -> ComponentType: + """根據 MAV_TYPE 判斷組件類型""" + # MAV_TYPE 定義： + # 0=通用, 1=固定翼, 2=四旋翼, 3=直升機, 4=天線追蹤器, 5=GCS, 6=飛船, + # 26=雲台, 27=ADSB, 28=降落傘等 + if mav_type == 6: # MAV_TYPE_GCS + return ComponentType.GCS + elif mav_type == 26: # MAV_TYPE_GIMBAL + return ComponentType.GIMBAL + elif mav_type == 30: # MAV_TYPE_CAMERA + return ComponentType.CAMERA + elif mav_type in [1, 2, 3, 4, 13, 14, 15, 19, 20, 21, 22]: # 各種飛行器類型 + return ComponentType.AUTOPILOT + else: + return ComponentType.OTHER + + # === 訊息處理器 === + def _handle_heartbeat(self, vehicle, component, msg, timestamp): + """處理 HEARTBEAT 訊息 (msg_id: 0)""" + component.status.mode.base_mode = msg.base_mode + component.status.mode.custom_mode = msg.custom_mode + component.status.mode.mode_name = mavutil.mode_string_v10(msg) + component.status.mode.timestamp = timestamp + component.status.system_status = msg.system_status + component.status.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 + # print("get mode:", mavutil.mode_string_v10(msg)) # debug + + def _handle_attitude(self, vehicle, component, msg, timestamp): + """處理 ATTITUDE 訊息 (msg_id: 30)""" + component.status.attitude.roll = msg.roll + component.status.attitude.pitch = msg.pitch + component.status.attitude.yaw = msg.yaw + component.status.attitude.rollspeed = msg.rollspeed + component.status.attitude.pitchspeed = msg.pitchspeed + component.status.attitude.yawspeed = msg.yawspeed + component.status.attitude.timestamp = timestamp + + def _handle_local_position(self, vehicle, component, msg, timestamp): + """處理 LOCAL_POSITION_NED 訊息 (msg_id: 32)""" + # LOCAL_POSITION_NED 提供相對位置資訊 + component.status.position.relative_altitude = -msg.z # NED 座標系，z 為負表示高度 + component.status.position.timestamp = timestamp + # 也可以存儲到 custom_status 中保留原始資料 + component.status.custom_status['local_position'] = { + 'x': msg.x, 'y': msg.y, 'z': msg.z, + 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz + } + + def _handle_global_position(self, vehicle, component, msg, timestamp): + """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)""" + component.status.position.latitude = msg.lat / 1e7 # 轉換為度 + component.status.position.longitude = msg.lon / 1e7 # 轉換為度 + component.status.position.altitude = msg.alt / 1000.0 # 轉換為公尺 + component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺 + component.status.position.timestamp = timestamp + + def _handle_vfr_hud(self, vehicle, component, msg, timestamp): + """處理 VFR_HUD 訊息 (msg_id: 74)""" + component.status.vfr.airspeed = msg.airspeed + component.status.vfr.groundspeed = msg.groundspeed + component.status.vfr.heading = msg.heading + component.status.vfr.throttle = msg.throttle + component.status.vfr.climb = msg.climb + component.status.vfr.timestamp = timestamp + + def _handle_battery_status(self, vehicle, component, msg, timestamp): + """處理 BATTERY_STATUS 訊息 (msg_id: 147)""" + # 計算電池總電壓（mV 轉 V） + if hasattr(msg, 'voltages') and msg.voltages[0] != 65535: + # voltages 是一個陣列，包含各個電池單元的電壓 + total_voltage = sum(v for v in msg.voltages if v != 65535) / 1000.0 + component.status.battery.voltage = total_voltage + + component.status.battery.current = msg.current_battery / 100.0 if msg.current_battery != -1 else None # cA 轉 A + component.status.battery.remaining = msg.battery_remaining if msg.battery_remaining != -1 else None # 百分比 + component.status.battery.temperature = msg.temperature / 100.0 if hasattr(msg, 'temperature') and msg.temperature != 32767 else None # 轉換為攝氏 + component.status.battery.timestamp = timestamp + + # === 訊息發送功能 === + def send_message(self, message_bytes, target_sysid=None, target_socket_id=None, broadcast=False): + """ + 發送訊息到指定的 mavlink_object + + Args: + message_bytes: 準備好的 mavlink 封包 (bytes) + target_sysid: 目標系統 ID (可選，用於自動查找對應的 socket) + target_socket_id: 目標 socket ID (可選，直接指定) + + Returns: + bool: 是否成功發送 + + 使用方式： + 1. broadcast: 廣播到所有活動的 mavlink_object + 2. 指定 target_socket_id：直接發送到該 socket + 3. 指定 target_sysid：自動查找該系統對應的 socket 並發送 + """ + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: {type(message_bytes)}") + return False - # 若未定義的訊息類型則不處理 並跳出訊息 + # 情況 1: 廣播到所有活動的 mavlink_object + if broadcast: + success_count = 0 + for socket_id, mav_obj in mavlink_object.mavlinkObjects.items(): + if self._send_to_socket(message_bytes, socket_id): + success_count += 1 + + return success_count > 0 + + # 情況 2: 直接指定 socket_id + if target_socket_id is not None: + return self._send_to_socket(message_bytes, target_socket_id) + + # 情況 3: 透過 sysid 查找對應的 socket + if target_sysid is not None: + vehicle = vehicle_registry.get(target_sysid) + if vehicle and 'socket_id' in vehicle.custom_meta: + socket_id = vehicle.custom_meta['socket_id'] + return self._send_to_socket(message_bytes, socket_id) else: - 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') - - # === Node 區塊 === - def _init_node(self): - logger.info('Start of mavlink_bridge._init_node') - super().__init__('mavlink_bridge') # 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(): + logger.warning(f"System ID {target_sysid} not found or no socket_id") + return False + + logger.warning("No target specified for sending message. WTF ARE YOU DOING?") + return False # 若無指定任何目標，則返回失敗 + + def _send_to_socket(self, message_bytes, socket_id): + """ + 將訊息發送到指定的 mavlink_object + + Args: + message_bytes: mavlink 封包 + socket_id: 目標 socket ID + + Returns: + bool: 是否成功 + """ + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False + + mav_obj = mavlink_object.mavlinkObjects[socket_id] + return mav_obj.message_put_queue(message_bytes) + +# 定義 mavlink_object 的狀態 +class MavlinkObjectState(Enum): + INIT = auto() # 初始化狀態 + RUNNING = auto() # 運行中狀態 + SHUTTINGDOWN = auto() # 關閉中狀態 + ERROR = auto() # 錯誤狀態 + CLOSED = auto() # 已關閉狀態 + +class mavlink_object: ''' 每個 mavlink bus 都會有一個 mavlink_object - 其中主要是 thread 做統計封包與分流 - 分流表的控制在三個 list 分別為 - multiplexingToAnalysis : 這個 list 是用來分流到固定串流橋接器的 - multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 - multiplexingToSwap : 這個 list 是用來分流到轉拋串流的 - 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 + 使用 asyncio 處理資料流 用 RingBuffer 來分配訊息 + 直接透過 socket 寫出 ''' - mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } - socket_num = 0 # 用來記錄目前的 socket 數量 + mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } + socket_num = 0 # 用來記錄目前的 socket 數量 + def __new__(cls, *args, **kwargs): # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號 # socket_id 從 0 開始 - instance = super().__new__(cls) socket_id = 0 for k in cls.mavlinkObjects.keys(): @@ -214,193 +360,449 @@ class mavlink_object(): return instance def __init__(self, socket): + # 登入所需的 socket self.mavlink_socket = socket - # 這邊變數是執行的時候被使用的 不要直接寫入它 - self._multiplexingList = [] - # 存放要發送的訊息的 queue 或稱 buffer - self.output_buffer = queue.Queue() - if self.socket_id >= len(swap_queues): - swap_queues.append(self.output_buffer) - else: - swap_queues[self.socket_id] = self.output_buffer - - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems - - # 這三個 list 用來分配不同的訊息到不同的 queue - self.multiplexingToAnalysis = [ - 0, # HEARTBEAT # 挺必要的項目 - # 24, # GPS_RAW_INT - # 30, # ATTITUDE - # 33, # GLOBAL_POSITION_INT - # 74, # VFR_HUD - # 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [] - self.multiplexingToSwap = [ - [] for _ in range(len(swap_queues)) - ] - - # 刷新其他 mavlink_object 的 multiplexingToSwap - for k, object in self.mavlinkObjects.items(): - if (k != self.socket_id) and (len(object.multiplexingToSwap) <= self.socket_id): - object.multiplexingToSwap.append([]) - object.updateMultiplexingList() - - logger.info('mavlink_object instance {} created'.format(self.socket_id)) - def __del__(self): - # 停下自己的 thread - if self.mavlink_socket != None: - self.mavlink_socket.close() - self.stop() + # 用於主線程發送消息的緩衝區 + self.outgoing_msgs = deque() + + # 記錄訊息過濾類型 (可選) + self.bridge_msg_types = set([0, 30, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS + self.return_msg_types = set() + + # 轉發到別的 mavlink object 作為目標端口 的列表 + self.target_sockets = set() + + # 物件變數 + self.task = None # Task reference + self.dirtyDataCount = 0 # 髒資料計數器 + self.dirtyDataMax = 10 # 髒資料容許閾值 + + self.state = MavlinkObjectState.INIT + # logger.info(f'mavlink_object instance {self.socket_id} created') # 先註解掉避免太多 log 但是為了 debug 保留 + + # 附加參數 (並非 mavlink_object 運行本體必要 但是要給上層結構運用的) + # 若這個 socket 是 另一個"主要 socket"的備用連接 則設定為"主要 socket id" + self.primary_socket_id = None # None 表示不是備用連接 + # socket type + self.socket_type = 'UNDEFINED' # 可選 'UDP_INBOUND', 'SERIAL_XBEE'...etc - # 移除其他 mavlink_object 的 multiplexingToSwap - for k, object in self.mavlinkObjects.items(): - if (k != self.socket_id) and (len(object.multiplexingToSwap) > self.socket_id): - object.multiplexingToSwap[self.socket_id] = [] - object.updateMultiplexingList() - - # 移除自己的 swap_queues - swap_queues[self.socket_id] = None + def __del__(self): + # # 先移除其他 socket 指向這個 socket 的目標 # 還是不要在這邊做了 畢竟本來就有判斷 object 不活躍就不轉拋 + # for mavlink_obj in self.mavlinkObjects.values(): + # if self.socket_id in mavlink_obj.target_sockets: + # mavlink_obj.remove_target_socket(self.socket_id) + # 關閉 socket + if hasattr(self, 'mavlink_socket') and self.mavlink_socket: + try: + self.mavlink_socket.close() + except: + pass + # 處理 class 的 instance 記錄 - self.socket_num -= 1 - self.mavlinkObjects.pop(self.socket_id) # 刪除這個 instance - - # logger 模組在這邊會一直掛掉 找不到問題 先用 print 代替 - # print('mavlink_object instance {} deleted'.format(self.socket_id)) # debug - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + if hasattr(self.__class__, 'socket_num'): + self.__class__.socket_num -= 1 + + if hasattr(self.__class__, 'mavlinkObjects') and hasattr(self, 'socket_id'): + if self.socket_id in self.__class__.mavlinkObjects: + self.__class__.mavlinkObjects.pop(self.socket_id) + + # 這段不知道怎麼了 反正會一直讓 logger ERROR 我先關掉 # try: - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) + # logger.info(f'mavlink_object instance {self.socket_id} deleted') # except Exception as e: # print(f"Error logging in __del__: {e}") - # if 'logger' in globals() and logger: - # logger.info('mavlink_object instance {} deleted'.format(self.socket_id)) - - def run(self): - # TODO 檢查 socket 是否有連線 - self.thread = threading.Thread(target=self._run_thread) - self.running = self.updateMultiplexingList() - self.thread.start() - - def stop(self): - self.running = False - - def _run_thread(self): - logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) - logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) - while self.running: - timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 + async def process_data(self): + """處理 mavlink 數據的主要 asyncio 協程""" + # logger.info(f'Start of mavlink_object id: {self.socket_id}') # 先註解掉避免太多 log 但是為了 debug 保留 + + while self.state == MavlinkObjectState.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.") - 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 + mavlinkMsg = self.mavlink_socket.recv_msg() - if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega. etc...' + if mavlinkMsg: # 統計收到的訊息 & 透過 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 類型 - device = self.mavlink_systems[sysid] - mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) + + # 更新封包統計資訊 + vehicle = vehicle_registry.get(sysid) + if vehicle: # 只有當這個 system id 已經被註冊過才會記錄統計 + component = vehicle.get_component(compid) + if component: + component.update_packet_stats( + mavlinkMsg.get_seq(), + mavlinkMsg.get_msgId(), + timestamp + ) + + # 分發訊息到 RingBuffer + msg_id = mavlinkMsg.get_msgId() + + if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types): + stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg)) - comp = device.components.get(compid) - if comp and comp.loss_rate_publisher: - if timestamp - comp.last_loss_publish_time >= 1.0: - loss_rate = comp.loss_rate # loss_rate 是在 updateComponentPacketCount 中計算並儲存的欄位 - msg = std_msgs.msg.Float64() - msg.data = float(loss_rate) - comp.loss_rate_publisher.publish(msg) - comp.last_loss_publish_time = timestamp - - # break # Debug function can put here you can see the input data from mavlink - - # 將訊息依照 multiplexing list 分發到不同的 queue - for i in range(len(self._multiplexingList)): - if (self._multiplexingList[i] == []): - continue - elif (mavlinkMsg.get_msgId() in self._multiplexingList[i]) or (self._multiplexingList[i][0] == -1): - if i == 0: - fixed_stream_bridge_queue.put((self.socket_id,timestamp,mavlinkMsg)) - elif i == 1: - return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) - else: - _queue = swap_queues[i-2] - # _queue.put((self.socket_id,timestamp,mavlinkMsg)) # 測試看看 也許不需要別的資訊 只需要封包 - _queue.put(mavlinkMsg) - - # 處理要送出的封包 - # 如果 有資料在 output_buffer 中則將其取出並發送 - # 沒有就略過發送 - try: - mavlinkMsg_send = self.output_buffer.get(block=False) - except queue.Empty: - mavlinkMsg_send = None - # except Exception as e: - # logger.error(f"Error getting data from output_buffer: {e}") - # mavlinkMsg_send = None - - if mavlinkMsg_send: - # self.mavlink_socket.mav.send(mavlinkMsg_send) - self.mavlink_socket.write(mavlinkMsg_send.get_msgbuf()) # 這邊會將封包寫入 socket 中 - - # thread 結束 - logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) - - def updateMultiplexingList(self): - ''' - 更新 multiplexing list 並做簡單的檢查 - ''' - # 檢查 multiplexingToAnalysis 與 multiplexingToReturn 是否有 -1 值 - check = (-1 in self.multiplexingToAnalysis) or (-1 in self.multiplexingToReturn) - if check: - logger.error('MultiplexingToAnalysis or MultiplexingToReturn NOT all type spilt. socket id : {}'.format(self.socket_id)) + if (msg_id in self.return_msg_types or -1 in self.return_msg_types): + return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg)) + + # 將全部接收到的訊息轉發給目標列表中的 mavlink_object + for target_socket in self.target_sockets: + if target_socket in self.mavlinkObjects: + target_obj = self.mavlinkObjects[target_socket] + if target_obj.state == MavlinkObjectState.RUNNING: + target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf()) + + if self.outgoing_msgs: + send_msg = self.outgoing_msgs.popleft() + self.mavlink_socket.write(send_msg) + + + # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權 + await asyncio.sleep(0.001) + + logger.info(f'End of mavlink_object id: {self.socket_id}') + self.state = MavlinkObjectState.CLOSED + + def add_target_socket(self, target_socket_id): + """添加一個目標端口用於轉發""" + if (target_socket_id != self.socket_id) and (target_socket_id != self.primary_socket_id): + if target_socket_id not in self.target_sockets: + self.target_sockets.add(target_socket_id) + logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}") + return True + return False # 已存在 + return False # 不能添加自己 也不能添加主要 socket + + def remove_target_socket(self, target_socket_id): + """移除目標端口""" + if target_socket_id in self.target_sockets: + self.target_sockets.remove(target_socket_id) + logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}") + return True + return False # 不存在 + + def set_bridge_message_types(self, msg_types): + """設置需要分流到橋接器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.bridge_msg_types = set(msg_types) + return True + logger.error(f"Invalid bridge message types: {msg_types}") + return False + + def set_return_message_types(self, msg_types): + """設置需要分流到回傳處理器的訊息類型""" + if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types): + self.return_msg_types = set(msg_types) + return True + logger.error(f"Invalid return message types: {msg_types}") + return False + + def message_put_queue(self, message_bytes): + """ + 從主線程向此 mavlink_object 的 socket 發送數據 + 將數據添加到簡單的列表中，由 asyncio 任務處理 + + Args: + message_bytes: 要發送的 mavlink 消息的字節數據 + + Returns: + bool: 是否成功添加消息到列表 + """ + # 狀態檢查 + if self.state != MavlinkObjectState.RUNNING: + logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running") + return False + + # 基本數據類型檢查（輕量級） + if not isinstance(message_bytes, (bytes, bytearray)): + logger.error(f"Invalid message type: expected bytes/bytearray, got {type(message_bytes)}") + return False + + # 基本長度檢查（MAVLink v1.0 最小8字節，v2.0最小12字節） + if len(message_bytes) < 8: + logger.error(f"Message too short: {len(message_bytes)} bytes (minimum 8)") + return False + + # MAVLink 起始標記檢查（輕量級） + if len(message_bytes) > 0 and message_bytes[0] not in (0xFE, 0xFD): # v1.0: 0xFE, v2.0: 0xFD + logger.error(f"Invalid MAVLink start marker: 0x{message_bytes[0]:02X}") return False + + # 緩衝區大小保護（防止記憶體耗盡） + if len(self.outgoing_msgs) > 1000: # 可調整的閾值 + logger.warning(f"Outgoing message buffer full for mavlink_object {self.socket_id}") + return False + + self.outgoing_msgs.append(message_bytes) + return True + +class async_io_manager: + """ + 管理所有 mavlink_object 實例的 asyncio 任務 + 提供單一線程來處理所有 mavlink 通道的數據 + + 首先 async_io_manager 是 singleton 的 所以只能有一個實例 + + 這個 async_io_manager 是藉由 start 方法來啟動的 + + start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop + 然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務 + """ + + _instance = None + _lock = threading.Lock() + + def __new__(cls, *args, **kwargs): + with cls._lock: + if cls._instance is None: + cls._instance = super(async_io_manager, cls).__new__(cls) + return cls._instance + + def __init__(self): + if not hasattr(self, 'initialized'): + self.initialized = True + self.loop = None + self.running = False + # self.main_task = None + self.thread = None - # 檢查 multiplexingToSwap 與 swap_queues 的長度是否一致 而且 swap_queues 的長度不能為 0 - check = len(self.multiplexingToSwap) != len(swap_queues) or len(swap_queues) == 0 - if check: - logger.error('Multiplexing Queue not fit List , Please check the list. socket id : {}'.format(self.socket_id)) + def __del__(self): + self.loop = None + self.thread = None + + def start(self): + """ + 啟動 async_io_manager + + """ + if self.running: + logger.warning("async_io_manager already running") + return + + self.running = True + + # 啟動獨立線程 命名為 AsyncIOManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="AsyncIOManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 + self.thread.start() + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("async_io_manager thread started <-") + return True + else: + logger.error("async_io_manager failed to start") return False + + def shutdown(self): + """停止 async_io_manager 和其管理的所有 mavlink_object""" + + # 自己在 running 狀態下才執行停止程序 + if not self.running: + return + + # 停止所有被管理的 mavlink_object 所屬的 task + for socket_id in list(mavlink_object.mavlinkObjects.keys()): + self.remove_mavlink_object(socket_id) + + # 停止自己的 task + self.running = False - # 對應到自己的 multiplexingToSwap 必需為空 避免對自己迴圈轉拋 + # 解開事件循環的阻塞 + self.loop.call_soon_threadsafe(self.loop.stop) + + # print("mark A", len(asyncio.all_tasks(self.loop))) # debug + + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) + if self.thread.is_alive(): + logger.warning("async_io_manager thread did not stop gracefully") + os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序 + + logger.info("async_io_manager thread END!") + + def _run_event_loop(self): + """在獨立線程中運行事件循環""" try: - self.multiplexingToSwap[self.socket_id] = [] - except IndexError: - logger.error('Multiplexing List of self socket id should be void. socket id : {}'.format(self.socket_id)) - return False + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + logger.info("async_io_manager event loop started <-") + + # 創建主任務 + # main_task = self.loop.create_task(self._main_task()) + + # 運行事件循環 + self.loop.run_forever() + + except Exception as e: + logger.error(f"Error in async_io_manager event loop: {e}") + finally: + # 清理 + if self.loop: + self.loop.close() + self.loop = None + self.running = False + logger.info("async_io_manager event loop END!") + + def add_mavlink_object(self, mavlink_obj: mavlink_object): + """添加 mavlink_object""" + # 一個防呆 確保有 event loop 與 _main_task 正在運作 + if not self.running or not self.loop: + logger.error("Cannot add mavlink_object: async_io_manager is not running") + return False + + socket_id = mavlink_obj.socket_id + + # 檢查該對象是否已經在運行中 + if socket_id in mavlink_object.mavlinkObjects: + existing_obj = mavlink_object.mavlinkObjects[socket_id] + if existing_obj.state == MavlinkObjectState.RUNNING: + logger.warning(f"mavlink_object {socket_id} already managed") + return False + + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_add_mavlink_object(mavlink_obj), + self.loop + ) - # 組合 multiplexing list - multiL_tmp = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToSwap + try: + # 等待結果，設定合理的超時時間 + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout adding mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to add mavlink_object {socket_id}: {e}") + return False + + async def _async_add_mavlink_object(self, mavlink_obj): + """在事件循環線程中同步執行""" + socket_id = mavlink_obj.socket_id - # 檢查 multiplexing list 格式是否有錯誤 # 全部都要是 list 每個 list 裡面都要是 int - check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in multiL_tmp) - if not check: - logger.error('Multiplexing List Format Error, Please check the list. socket id : {}'.format(self.socket_id)) - return False # 若有錯誤則回傳 False + try: + task = asyncio.create_task(mavlink_obj.process_data()) + mavlink_obj.task = task + mavlink_obj.state = MavlinkObjectState.RUNNING + mavlink_obj.outgoing_msgs.clear() + logger.info(f"Added mavlink_object {socket_id} into manager.") + return True + except Exception as e: + logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}") + return False + + def remove_mavlink_object(self, socket_id: int): + """移除 mavlink_object""" - # 更新 multiplexing list - self._multiplexingList = multiL_tmp + # 一個防呆 確保有 event loop 正在運作 + if not self.loop: + return False + + # 同樣使用 run_coroutine_threadsafe + future = asyncio.run_coroutine_threadsafe( + self._async_remove_mavlink_object(socket_id), + self.loop + ) + + try: + result = future.result(timeout=3.0) + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing mavlink_object {socket_id}") + return False + except Exception as e: + logger.error(f"Failed to remove mavlink_object {socket_id}: {e}") + return False + + async def _async_remove_mavlink_object(self, socket_id): + """在事件循環線程中同步執行""" + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False + + mavlink_obj = mavlink_object.mavlinkObjects[socket_id] + mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN + + if not mavlink_obj.task.done(): + mavlink_obj.task.cancel() + + # 等待一秒或者 task完全結束 + timeout = 1.0 + start_time = asyncio.get_event_loop().time() + while not mavlink_obj.task.done(): + if asyncio.get_event_loop().time() - start_time > timeout: + break + await asyncio.sleep(0.1) - return True + # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理） + if mavlink_obj.task.done(): + mavlink_obj.state = MavlinkObjectState.CLOSED + logger.info(f"Removed mavlink_object {socket_id} from manager.") + return True + else: + mavlink_obj.state = MavlinkObjectState.ERROR + logger.warning(f"mavlink_object {socket_id} task did not terminate in time") + return False + + def get_managed_objects(self): + """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）""" + return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items() + if obj.state == MavlinkObjectState.RUNNING] # ====================== 分割線 ===================== -# 整合到 ros2 之後的程式進入點 -def main_node(): +if __name__ == '__main__': pass -if __name__ == '__main__': - pass +''' +================= 改版記錄 ============================ + +2025年 6月 20日 +1. mavlink_object 中 由於 Queue 的效能太差 會完全移除 + 其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現 + 而 multiplexingToSwap 會完全被移除代替方式下一條描述 +2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流 + (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object) + 因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移 + 並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出 +3. mavlink_object 需要加上 state 去管理其狀態 +4. mavlink_object 需要加上 target port 去管理寫出的目標 +5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道 +6. 基於第1,2項 updateMultiplexingList 會被完全移除 +7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object +8. 基於第1項全域變數 fixed_stream_bridge_queue return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除 + + +2025年 11月 15日 +1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行 +2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式) +3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 +4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 +5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 +6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +7. 移除迴圈內的 try except 堆疊 增加效能 +8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 + +2026年 01月 15日 +1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects +2. async_io_manager 的 _stop_event 無效變數移除 + +''' + diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py index a0ce087..5dc3ca1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py @@ -9,66 +9,48 @@ publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> ''' -from pymavlink import mavutil +import os +# ROS2 的 import 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") +# 自定義的 import +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) class mavlink_publisher(): prefix_path = 'MavLinkBus' - TOPIC_CREATORS = ( - "create_state", - "create_attitude", - "create_local_position_pose", - "create_local_position_velocity", - "create_global_global", - "create_global_rel", - "create_vfr_hud", - "create_battery", - "create_ping" - ) - - def ensure_all_publishers(self, sysid: int, component): - """若還沒替這個 component 建立過 publisher，則一次補齊。""" - if getattr(component, "_pub_ready", False): - return # 已做過就跳過 - for fn in self.TOPIC_CREATORS: - getattr(self, fn)(sysid, component) - # 發布丟包率 - if not hasattr(component, "loss_rate_publisher") or component.loss_rate_publisher is None: - target_topic = 'packet_loss_rate' - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - component.loss_rate_publisher = self.create_publisher(std_msgs.msg.Float64, topic_name, 10) - component.loss_rate_topic_name = topic_name - - component._pub_ready = True # 打個旗標以免重複 - logger.info("✔︎ sysid=%d compid=%d → 所有 topic 已註冊", sysid, component.compid) - - def create_state(self, sysid, component_obj): + def create_flightMode(self, sysid, component_obj): # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'state' + 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(mavros_msgs.msg.State, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_state] + publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) + component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] return True - def packEmit_state(cls, emitParams, publisher): - if 'heartbeat' not in emitParams: # ← 沒資料就直接返回 - return - mav_msg = emitParams['heartbeat'] - msg = mavros_msgs.msg.State() - msg.mode = mavutil.mode_string_v10(mav_msg) - msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0 + def packEmit_flightMode(cls, emitParams, publisher): + msg_str = emitParams['flightMode_mode'] + msg = std_msgs.msg.String() + msg.data = msg_str publisher.publish(msg) pass @@ -82,13 +64,18 @@ class mavlink_publisher(): def create_attitude(self, sysid, component_obj): target_topic = 'attitude' + + try: + _ = component_obj.emitParams['attitude'] + 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.Imu, topic_name, 1) component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] def packEmit_attitude(self, emitParams, publisher): - if 'attitude' not in emitParams: # ← 沒資料就直接返回 - return 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) @@ -104,13 +91,16 @@ class mavlink_publisher(): 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): - if 'local_position' not in emitParams: - return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Point() msg.x = mav_msg.x @@ -121,13 +111,16 @@ class mavlink_publisher(): 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): - if 'local_position' not in emitParams: - return mav_msg = emitParams['local_position'] msg = geometry_msgs.msg.Vector3() msg.x = mav_msg.vx @@ -138,13 +131,16 @@ class mavlink_publisher(): 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): - if 'global_position' not in emitParams: - return mav_msg = emitParams['global_position'] msg = sensor_msgs.msg.NavSatFix() msg.latitude = mav_msg.lat/1e7 @@ -155,13 +151,16 @@ class mavlink_publisher(): 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): - if 'global_position' not in emitParams: - return mav_msg = emitParams['global_position'] msg = std_msgs.msg.Float64() msg.data = float(mav_msg.relative_alt/1e3) @@ -170,13 +169,16 @@ class mavlink_publisher(): 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): - if 'vfr_hud' not in emitParams: - return mav_msg = emitParams['vfr_hud'] msg = mavros_msgs.msg.VfrHud() msg.airspeed = mav_msg.airspeed @@ -190,30 +192,21 @@ class mavlink_publisher(): 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): - if 'battery' not in emitParams: - return mav_msg = emitParams['battery'] msg = sensor_msgs.msg.BatteryState() msg.voltage = mav_msg.voltages[0]/1e3 publisher.publish(msg) pass - - def create_ping(self, sysid, component_obj): - target_topic = 'ping' - 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_ping] - def packEmit_ping(cls, emitParams, publisher): - if 'ping' not in emitParams: - return - mav_msg = emitParams['ping'] - publisher.publish(mav_msg) - pass # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py new file mode 100644 index 0000000..a95fb26 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -0,0 +1,902 @@ +""" +MAVLink ROS2 Nodes +包含兩個獨立的 ROS2 Node： +1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics +2. MavlinkCommandService - 提供 MAVLink 指令 service 介面 + +從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計 +""" + +import os +import time +import math +import threading +from typing import Dict, Optional + +# ROS2 imports +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor + +# ROS2 Message imports +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 自定義 imports +from . import mavlinkVehicleView as mvv +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ============================================================================ +# 頻率控制器 +# ============================================================================ + +class PublishRateController: + """發布頻率控制器 - 按時間間隔控制發布頻率""" + + def __init__(self): + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 1/4】 + # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔 + # 例如：'ekf_status': 1.0, # EKF 狀態 + # ═══════════════════════════════════════════════════════════════ + # 各 topic 的發布間隔（秒） + self.topic_intervals = { + 'position': 0.5, # GPS位置 + 'attitude': 0.5, # 姿態 + 'velocity': 0.5, # 速度 + 'battery': 1.0, # 電池 + 'vfr_hud': 0.5, # VFR HUD + 'mode': 1.0, # 飛行模式 + 'summary': 1.0, # 載具摘要 + # 在這裡新增更多 topics... + } + # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} + self.last_publish_time: Dict[tuple, float] = {} + + def should_publish(self, sysid: int, topic: str) -> bool: + """ + 檢查是否應該發布此 topic + + Args: + sysid: 系統ID + topic: topic 名稱 + + Returns: + bool: True 表示應該發布 + """ + key = (sysid, topic) + now = time.time() + + # 當間隔設定為0或負數時 關閉該 topic 的發布 + interval = self.topic_intervals.get(topic, 0) + if interval <= 0: + return False + + # 首次發布 + if key not in self.last_publish_time: + self.last_publish_time[key] = now + return True + + # 檢查時間間隔 + if now - self.last_publish_time[key] >= interval: + self.last_publish_time[key] = now + return True + + return False + + def reset(self): + """重置所有計時器""" + self.last_publish_time.clear() + + +# ============================================================================ +# VehicleStatusPublisher Node +# ============================================================================ + +class VehicleStatusPublisher(Node): + """ + 載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics + + 職責: + - 定期從 vehicle_registry 讀取載具狀態 + - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz） + - 發布標準 ROS2 消息類型 + - 檢測訂閱者，按需發布 + """ + topicString_prefix = f'/fc_network/vehicle' + + def __init__(self): + super().__init__('vehicle_status_publisher') + + # 頻率控制器 + self.rate_controller = PublishRateController() + + # fc_publishers 字典 {(sysid, topic_name): publisher} + self.fc_publishers: Dict[tuple, any] = {} + + # 定時器：以較高頻率檢查 vehicle_registry 並發布 + # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率 + self.timer_period = 0.1 # 100ms + self.timer = self.create_timer(self.timer_period, self.timer_callback) + + # 狀態標誌 + self.running = True + + # logger.info("VehicleStatusPublisher initialized") + + def timer_callback(self): + """定時器回調 - 檢查所有載具並發布狀態""" + if not self.running: + return + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = mvv.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + self._publish_vehicle_status(vehicle) + + def _publish_vehicle_status(self, vehicle: mvv.VehicleView): + """ + 發布單個載具的所有狀態 + + Args: + vehicle: VehicleView 實例 + """ + sysid = vehicle.sysid + + # 假設只有一個 autopilot component (component_id=1) + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 2/4】 + # 若要新增 topic，請在此處調用對應的發布方法 + # 例如：self._publish_ekf_status(sysid, status) + # ═══════════════════════════════════════════════════════════════ + # 發布各種狀態（通過頻率控制器判斷是否發布） + self._publish_position(sysid, status) + self._publish_attitude(sysid, status) + self._publish_velocity(sysid, status) + self._publish_battery(sysid, status) + self._publish_vfr_hud(sysid, status) + self._publish_mode(sysid, status) + self._publish_summary(vehicle) + # 在這裡新增更多 publish 方法調用... + + def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): + """ + 獲取或創建 publisher + + Args: + sysid: 系統ID + topic: topic 名稱 + msg_type: ROS2 消息類型 + qos: QoS 設置 + + Returns: + publisher 對象 + """ + key = (sysid, topic) + if key not in self.fc_publishers: + topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}' + publisher = self.create_publisher(msg_type, topic_name, qos) + self.fc_publishers[key] = publisher + logger.info(f"Created publisher: {topic_name}") + return self.fc_publishers[key] + + def _publish_position(self, sysid: int, status: mvv.ComponentStatus): + """發布 GPS 位置""" + if not self.rate_controller.should_publish(sysid, 'position'): + return + + pos = status.position + if pos.latitude is None or pos.longitude is None: + return + + publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix) + + # 檢查是否有訂閱者 + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.NavSatFix() + msg.latitude = pos.latitude + msg.longitude = pos.longitude + msg.altitude = pos.altitude if pos.altitude is not None else 0.0 + + # GPS 狀態資訊 + gps = status.gps + if gps.fix_type is not None: + msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus + + publisher.publish(msg) + + def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus): + """發布姿態（IMU）""" + if not self.rate_controller.should_publish(sysid, 'attitude'): + return + + att = status.attitude + if att.roll is None: + return + + publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.Imu() + + # 歐拉角轉四元數 + qx, qy, qz, qw = self._euler_to_quaternion( + att.roll, att.pitch, att.yaw + ) + msg.orientation.x = qx + msg.orientation.y = qy + msg.orientation.z = qz + msg.orientation.w = qw + + # 角速度 + if att.rollspeed is not None: + msg.angular_velocity.x = att.rollspeed + msg.angular_velocity.y = att.pitchspeed + msg.angular_velocity.z = att.yawspeed + + publisher.publish(msg) + + def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus): + """發布速度""" + if not self.rate_controller.should_publish(sysid, 'velocity'): + return + + vfr = status.vfr + if vfr.groundspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped) + + if publisher.get_subscription_count() == 0: + return + + msg = geometry_msgs.msg.TwistStamped() + msg.header.stamp = self.get_clock().now().to_msg() + + # 使用 VFR HUD 的地速和航向計算速度分量 + if vfr.heading is not None: + heading_rad = math.radians(vfr.heading) + msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad) + msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad) + + # 爬升率作為 z 軸速度 + if vfr.climb is not None: + msg.twist.linear.z = vfr.climb + + publisher.publish(msg) + + def _publish_battery(self, sysid: int, status: mvv.ComponentStatus): + """發布電池狀態""" + if not self.rate_controller.should_publish(sysid, 'battery'): + return + + bat = status.battery + if bat.voltage is None: + return + + publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState) + + if publisher.get_subscription_count() == 0: + return + + msg = sensor_msgs.msg.BatteryState() + msg.voltage = bat.voltage + + if bat.current is not None: + msg.current = bat.current + + if bat.remaining is not None: + msg.percentage = bat.remaining / 100.0 + + if bat.temperature is not None: + msg.temperature = bat.temperature + + publisher.publish(msg) + + def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus): + """發布 VFR HUD""" + if not self.rate_controller.should_publish(sysid, 'vfr_hud'): + return + + vfr = status.vfr + if vfr.airspeed is None: + return + + publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud) + + if publisher.get_subscription_count() == 0: + return + + msg = mavros_msgs.msg.VfrHud() + msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0 + msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0 + msg.heading = vfr.heading if vfr.heading is not None else 0 + msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0 + msg.climb = vfr.climb if vfr.climb is not None else 0.0 + + # altitude 需要從 position 獲取 + if status.position.altitude is not None: + msg.altitude = status.position.altitude + + publisher.publish(msg) + + def _publish_mode(self, sysid: int, status: mvv.ComponentStatus): + """發布飛行模式""" + if not self.rate_controller.should_publish(sysid, 'mode'): + return + + mode = status.mode + if mode.mode_name is None: + return + + publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + msg = std_msgs.msg.String() + msg.data = mode.mode_name + publisher.publish(msg) + + def _publish_summary(self, vehicle: mvv.VehicleView): + """ + 發布載具摘要資訊（自定義格式，使用 String 暫時代替） + TODO: 未來可以定義專門的 VehicleSummary.msg + """ + sysid = vehicle.sysid + + if not self.rate_controller.should_publish(sysid, 'summary'): + return + + publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String) + + if publisher.get_subscription_count() == 0: + return + + # 獲取 autopilot component + component = vehicle.get_component(1) + if not component: + return + + status = component.status + + # 構建摘要資訊（JSON 格式字串） + import json + summary = { + 'sysid': sysid, + 'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0, + 'autopilot': component.mav_autopilot if component.mav_autopilot else 0, + 'socket_id': vehicle.custom_meta.get('socket_id', -1), # 重要！ + 'armed': status.armed if status.armed is not None else False, + # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, + 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN", + # 'latitude': status.position.latitude if status.position.latitude else 0.0, + # 'longitude': status.position.longitude if status.position.longitude else 0.0, + # 'altitude': status.position.altitude if status.position.altitude else 0.0, + # 'battery_percent': status.battery.remaining if status.battery.remaining else 0, + 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0, + 'connection_type': vehicle.connected_via.value, + 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, + } + + msg = std_msgs.msg.String() + msg.data = json.dumps(summary) + publisher.publish(msg) + + # ═══════════════════════════════════════════════════════════════ + # 【新增 Topic 位置 3/4】 + # 若要新增 topic，請在此處實作對應的發布方法 + # 方法命名規則：def _publish_(self, sysid: int, status: mvv.ComponentStatus): + # 例如： + # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus): + # """發布 EKF 狀態""" + # if not self.rate_controller.should_publish(sysid, 'ekf_status'): + # return + # + # ekf = status.ekf + # if ekf.flags is None: + # return + # + # publisher = self._get_or_create_publisher(sysid, 'ekf_status', ... + # # ... 實作發布邏輯 + # ═══════════════════════════════════════════════════════════════ + + @staticmethod + def _euler_to_quaternion(roll, pitch, yaw): + """ + 歐拉角轉四元數 + + Args: + roll: 橫滾角 (弧度) + pitch: 俯仰角 (弧度) + yaw: 偏航角 (弧度) + + Returns: + tuple: (qx, qy, qz, qw) + """ + 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 stop(self): + """停止發布""" + self.running = False + # logger.info("VehicleStatusPublisher stopped") + + +# ============================================================================ +# MavlinkCommandService Node +# ============================================================================ + +class MavlinkCommandService(Node): + """ + MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令 + + 職責: + - 作為 service server，等待 client 請求 + - 接收請求，組裝 MAVLink 封包 + - 調用 mavlinkObject 發送封包 + - 處理 ACK 等待和超時（未來實現） + + 設計理念：回歸 MAVLink 純粹結構 + - 只負責將 ROS2 請求轉換為 MAVLink 封包 + - 不預設功能（如 ARM/DISARM），保持模組化 + - 高層應用可透過此 service 實現各種功能 + """ + + def __init__(self): + super().__init__('mavlink_command_service') + + # ═══════════════════════════════════════════════════════════════════ + # ROS2 Service 架構說明： + # + # 1. Service 定義：由 .srv 檔案定義（Request + Response） + # - Request: client 發送的請求內容 + # - Response: server 回傳的結果 + # + # 2. Service Server 創建： + # self.create_service(srv_type, service_name, callback_function) + # - srv_type: service 的訊息類型（需要自定義或使用標準） + # - service_name: service 的名稱（client 用此名稱呼叫） + # - callback_function: 處理請求的回調函數 + # + # 3. Callback 函數： + # def callback(self, request, response): + # # request: 包含 client 發送的數據 + # # response: 需要填充並返回給 client + # return response + # + # 4. Service Client 使用方式（在其他程式中）： + # client = node.create_client(srv_type, service_name) + # request = srv_type.Request() + # future = client.call_async(request) # 異步調用 + # # 或 response = client.call(request) # 同步調用 + # ═══════════════════════════════════════════════════════════════════ + + # 由於 ROS2 自定義 service 需要 .srv 檔案編譯 + # 這裡先使用標準 String service 作為簡化實現 + # TODO: 未來可創建專門的 .srv 檔案 + from std_srvs.srv import Trigger + from example_interfaces.srv import SetBool + + # ═══════════════════════════════════════════════════════════════════ + # Service 1: 發送 MAVLink Message（通用介面） + # 使用 Trigger 作為臨時實現，未來應使用自定義 service + # ═══════════════════════════════════════════════════════════════════ + # TODO: 創建 SendMavlinkMessage.srv + # Request: + # uint8 target_sysid + # uint8 target_compid + # uint16 message_id + # string fields_json # JSON 格式的字段數據 + # bool wait_response + # uint16 response_msgid + # float32 timeout + # Response: + # bool success + # string response_json + # string error_message + + # 暫時使用簡化版本（僅示範架構） + self.srv_send_message = self.create_service( + Trigger, + '/mavlink/send_message', + self.handle_send_message + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 2: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════ + self.srv_command_long = self.create_service( + Trigger, + '/mavlink/send_command_long', + self.handle_command_long + ) + + # ═══════════════════════════════════════════════════════════════════ + # Service 3: 參數請求 + # ═══════════════════════════════════════════════════════════════════ + self.srv_param_request = self.create_service( + Trigger, + '/mavlink/param_request', + self.handle_param_request + ) + + # 狀態標記 + self.running = True + + # mavlinkObject 的引用（將由外部設置） + self.mavlink_analyzer = None + + logger.info("MavlinkCommandService initialized") + + def set_mavlink_analyzer(self, mavlink_analyzer): + """ + 設置 mavlink_analyzer 引用 + + Args: + mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例 + """ + self.mavlink_analyzer = mavlink_analyzer + logger.info("MavlinkCommandService: mavlink_analyzer set") + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 MAVLink Message + # ═══════════════════════════════════════════════════════════════════════ + def handle_send_message(self, request, response): + """ + 處理發送 MAVLink 訊息的請求 + + ROS2 Service Callback 說明： + - 此函數會在 client 調用 service 時被執行 + - request: 包含 client 傳入的參數 + - response: 需要填充結果並返回給 client + - 必須 return response + + Args: + request: Trigger.Request (暫時使用，未來改為自定義) + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received send_message request") + + # 檢查 mavlink_analyzer 是否已設置 + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + logger.error(response.message) + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數（target_sysid, message_id, fields 等） + # 2. 使用 pymavlink 組裝 MAVLink 封包 + # 3. 調用 mavlink_analyzer.send_message() 發送 + # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應 + + # 暫時返回成功（示範用） + response.success = True + response.message = "Message sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 發送 COMMAND_LONG + # ═══════════════════════════════════════════════════════════════════════ + def handle_command_long(self, request, response): + """ + 處理發送 COMMAND_LONG 的請求 + + COMMAND_LONG (MAVLink message ID=76): + - 用於發送簡單命令給載具 + - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等 + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received command_long request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析 COMMAND_LONG 參數 + # - target_sysid, target_compid + # - command (MAV_CMD_xxx) + # - param1~param7 + # 2. 組裝 COMMAND_LONG 封包 + # 3. 發送並等待 COMMAND_ACK (message ID=77) + # 4. 解析 ACK 結果（ACCEPTED/FAILED 等） + + response.success = True + response.message = "Command sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # Service Handler: 參數請求 + # ═══════════════════════════════════════════════════════════════════════ + def handle_param_request(self, request, response): + """ + 處理參數讀取請求 + + MAVLink 參數協議： + - PARAM_REQUEST_READ (ID=20): 請求讀取參數 + - PARAM_VALUE (ID=22): 參數值回應 + - PARAM_SET (ID=23): 設置參數值 + + ═══════════════════════════════════════════════════════════════════ + 【使用 mavlinkObject 回應機制的步驟】 + + 1. 設置回應訊息類型： + self.mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + + 2. 發送請求封包： + message_bytes = ... # 組裝 PARAM_REQUEST_READ + self.mavlink_analyzer.send_message( + message_bytes, + target_sysid=1 + ) + + 3. 監聽回應（在獨立線程或定時器中）： + from ..fc_network_adapter import mavlinkObject as mo + + # 等待回應（帶超時） + timeout = 3.0 + start_time = time.time() + while time.time() - start_time < timeout: + items = mo.return_packet_ring.get_all() + for socket_id, timestamp, msg in items: + if msg.get_type() == 'PARAM_VALUE': + # 找到回應！ + param_id = msg.param_id + param_value = msg.param_value + # 處理回應... + return + time.sleep(0.01) # 短暫等待 + + # 超時處理 + + 4. 清理（可選）： + self.mavlink_analyzer.set_return_message_types([]) # 清空 + mo.return_packet_ring.clear() # 清空緩衝區 + + 注意事項： + - return_packet_ring 是全域的，所有 mavlink_object 共用 + - 需要通過 socket_id 或 sysid 來識別回應來源 + - 實際使用時建議實現專門的回應管理器 + ═══════════════════════════════════════════════════════════════════ + + Args: + request: Trigger.Request + response: Trigger.Response + + Returns: + response: 填充後的回應 + """ + logger.info("Received param_request") + + if self.mavlink_analyzer is None: + response.success = False + response.message = "Error: mavlink_analyzer not set" + return response + + # TODO: 實際實現 + # 1. 從 request 解析參數名稱或索引 + # 2. 設置 mavlink_analyzer.set_return_message_types([22]) # PARAM_VALUE + # 3. 發送 PARAM_REQUEST_READ + # 4. 監聽 return_packet_ring，等待 PARAM_VALUE + # 5. 解析回應並填充到 response + + response.success = True + response.message = "Param request sent (placeholder implementation)" + return response + + # ═══════════════════════════════════════════════════════════════════════ + # 【新增 Service 位置 4/4】 + # 若要新增 service，請在此處添加新的 handler 方法 + # 並在 __init__ 中創建對應的 service server + # ═══════════════════════════════════════════════════════════════════════ + + def stop(self): + """停止服務""" + self.running = False + logger.info("MavlinkCommandService stopped") + + +# ============================================================================ +# ROS2 節點管理器 +# ============================================================================ + +class fc_ros_manager: + """ + MAVLink ROS2 節點管理器 + + 管理兩個獨立的 ROS2 Node： + - VehicleStatusPublisher + - MavlinkCommandService + + 提供統一的啟動/停止介面給 mainOrchestrator + """ + + def __init__(self): + self.initialized = False + self.running = False + + # 两个 node 实例 + self.status_publisher: Optional[VehicleStatusPublisher] = None + self.command_service: Optional[MavlinkCommandService] = None + + # Executor & Thread + self.spin_thread: Optional[threading.Thread] = None + self.executor: Optional[MultiThreadedExecutor] = None + + def initialize(self): + """初始化 ROS2 环境和 nodes""" + if self.initialized: + logger.warning("fc_ros_manager already initialized") + return False + + try: + # 初始化 ROS2 + rclpy.init() + + # 創建節點 node + self.status_publisher = VehicleStatusPublisher() + self.command_service = MavlinkCommandService() + + # 創建執行者 MultiThreadedExecutor + self.executor = MultiThreadedExecutor() + self.executor.add_node(self.status_publisher) + self.executor.add_node(self.command_service) + + self.initialized = True + # logger.info("fc_ros_manager initialized") + return True + + except Exception as e: + logger.error(f"Failed to initialize fc_ros_manager: {e}") + return False + + def start(self): + """啟動 ROS2 nodes (在獨立執行緒中運行 executor) """ + if not self.initialized: + logger.error("fc_ros_manager initialize failed or not called") + return False + + if self.running: + logger.warning("fc_ros_manager already running") + return False + + try: + self.running = True + + self.spin_thread = threading.Thread( + target=self._spin_executor, + daemon=True, + name="ROS2ExecutorThread" + ) + self.spin_thread.start() + + logger.info("fc_ros_manager started <-") + return True + + except Exception as e: + logger.error(f"Failed to start fc_ros_manager: {e}") + self.running = False + return False + + def _spin_executor(self): + """在 thread 中運行的 executor""" + try: + # logger.info("ROS2 executor spinning...") + while self.running: + self.executor.spin_once(timeout_sec=0.1) + except Exception as e: + logger.error(f"fc_ros_manager error in spinning executor: {e}") + + def stop(self): + """停止 ROS2 nodes""" + if not self.running: + logger.warning("fc_ros_manager not running") + return False + + try: + # 標記停止 + self.running = False + + # 停止各個 node + if self.status_publisher: + self.status_publisher.stop() + if self.command_service: + self.command_service.stop() + + # 等待 spin 執行緒結束 + if self.spin_thread and self.spin_thread.is_alive(): + self.spin_thread.join(timeout=2.0) + + logger.info("fc_ros_manager thread END!") + return True + + except Exception as e: + logger.error(f"Error stopping fc_ros_manager: {e}") + return False + + def shutdown(self): + """完全關閉並清理資源""" + if self.running: + self.stop() + + if self.initialized: + try: + # 銷毀 nodes + if self.status_publisher: + self.status_publisher.destroy_node() + if self.command_service: + self.command_service.destroy_node() + + # 關閉 ROS2 + if rclpy.ok(): + rclpy.shutdown() + + self.initialized = False + logger.info("fc_ros_manager Node END!") + + except Exception as e: + logger.error(f"Error during shutdown: {e}") + + def get_status(self) -> dict: + return { + 'initialized': self.initialized, + 'running': self.running, + 'status_publisher_active': self.status_publisher is not None and self.status_publisher.running, + 'command_service_active': self.command_service is not None, + } + + +# ============================================================================ +# 全域實例 +# ============================================================================ + +# 全域管理器實例（供 mainOrchestrator 使用） +ros2_manager = fc_ros_manager() + + +''' +================= 改版記錄 ============================ + +2026.01.20 +1. 重構自 mavlinkPublish.py (該檔案將被棄用) +2. 提供 fc_ros_manager 統一管理介面 +3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態 +4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布 +5. 預留 MavlinkCommandService 結構（稍後實現） + +''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py new file mode 100644 index 0000000..b924c42 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -0,0 +1,453 @@ +""" +VehicleView - Pure State Container +純粹的狀態容器，不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 +只提供狀態存取介面，由外部手動餵資料（push state） +""" + +import os +from typing import Dict, Optional, Any +from dataclasses import dataclass, field +from enum import Enum + +from .utils import setup_logger + +logger = setup_logger(os.path.basename(__file__)) + + +# ====================== Enums ===================== + +class ConnectionType(Enum): + """連接類型""" + SERIAL = "serial" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +class ComponentType(Enum): + """組件類型""" + AUTOPILOT = "autopilot" + GCS = "gcs" + CAMERA = "camera" + GIMBAL = "gimbal" + OTHER = "other" + + +class RFModuleType(Enum): + """RF模組類型""" + XBEE = "xbee" + UDP = "udp" + TCP = "tcp" + OTHER = "other" + + +# ====================== Data Classes ===================== + +@dataclass +class Position: + """位置資訊""" + latitude: Optional[float] = None # 緯度 (度) + longitude: Optional[float] = None # 經度 (度) + altitude: Optional[float] = None # 海拔高度 (公尺) + relative_altitude: Optional[float] = None # 相對高度 (公尺) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Attitude: + """姿態資訊""" + roll: Optional[float] = None # 橫滾角 (弧度) + pitch: Optional[float] = None # 俯仰角 (弧度) + yaw: Optional[float] = None # 偏航角 (弧度) + rollspeed: Optional[float] = None # 橫滾速度 (弧度/秒) + pitchspeed: Optional[float] = None # 俯仰速度 (弧度/秒) + yawspeed: Optional[float] = None # 偏航速度 (弧度/秒) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class FlightMode: + """飛行模式資訊""" + base_mode: Optional[int] = None # MAVLink base mode + custom_mode: Optional[int] = None # 自定義模式 + mode_name: Optional[str] = None # 模式名稱 (例如: "GUIDED", "AUTO") + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class Battery: + """電池資訊""" + voltage: Optional[float] = None # 電壓 (V) + current: Optional[float] = None # 電流 (A) + remaining: Optional[int] = None # 剩餘電量 (%) + temperature: Optional[float] = None # 溫度 (攝氏) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class EKF: + """EKF狀態資訊""" + flags: Optional[int] = None # EKF 旗標 + velocity_variance: Optional[float] = None # 速度變異 + pos_horiz_variance: Optional[float] = None # 水平位置變異 + pos_vert_variance: Optional[float] = None # 垂直位置變異 + compass_variance: Optional[float] = None # 羅盤變異 + terrain_alt_variance: Optional[float] = None # 地形高度變異 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class GPS: + """GPS資訊""" + fix_type: Optional[int] = None # GPS fix 類型 (0=無, 1=無fix, 2=2D, 3=3D, 4=DGPS, 5=RTK) + satellites_visible: Optional[int] = None # 可見衛星數 + eph: Optional[int] = None # GPS HDOP 水平精度因子 + epv: Optional[int] = None # GPS VDOP 垂直精度因子 + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class VFR: + """VFR HUD資訊""" + airspeed: Optional[float] = None # 空速 (m/s) + groundspeed: Optional[float] = None # 地速 (m/s) + heading: Optional[int] = None # 航向 (度) + throttle: Optional[int] = None # 油門 (%) + climb: Optional[float] = None # 爬升率 (m/s) + timestamp: Optional[float] = None # 時間戳記 + + +@dataclass +class ComponentStatus: + """組件狀態容器""" + position: Position = field(default_factory=Position) + attitude: Attitude = field(default_factory=Attitude) + mode: FlightMode = field(default_factory=FlightMode) + battery: Battery = field(default_factory=Battery) + ekf: EKF = field(default_factory=EKF) + gps: GPS = field(default_factory=GPS) + vfr: VFR = field(default_factory=VFR) + + # 系統狀態 + system_status: Optional[int] = None # MAV_STATE + armed: Optional[bool] = None # 解鎖狀態 + + # 其他自定義狀態 + custom_status: Dict[str, Any] = field(default_factory=dict) + + +@dataclass +class PacketStats: + """封包統計資訊""" + received_count: int = 0 # 接收封包數 + lost_count: int = 0 # 遺失封包數 + last_seq: Optional[int] = None # 最後序號 + last_msg_time: Optional[float] = None # 最後訊息時間 + msg_type_count: Dict[int, int] = field(default_factory=dict) # 各類訊息計數 {msg_type: count} + + +@dataclass +class RFStatus: + """RF模組狀態""" + rssi: Optional[int] = None # 信號強度 + noise: Optional[int] = None # 噪音水平 + at_response: Optional[str] = None # AT 命令回應 + link_quality: Optional[int] = None # 連接品質 + timestamp: Optional[float] = None # 時間戳記 + custom_status: Dict[str, Any] = field(default_factory=dict) # 其他自定義狀態 + + +@dataclass +class SocketInfo: + """Socket連接資訊""" + ip: Optional[str] = None # IP位址 + port: Optional[int] = None # 埠號 + local_ip: Optional[str] = None # 本地IP + local_port: Optional[int] = None # 本地埠號 + connected: bool = False # 連接狀態 + + +# ====================== Component Class ===================== + +class VehicleComponent: + """載具組件 - 代表一個 MAVLink component""" + + def __init__(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER): + self.component_id = component_id + self.type = comp_type + + # MAVLink 組件資訊 + self.mav_type: Optional[int] = None # MAV_TYPE + self.mav_autopilot: Optional[int] = None # MAV_AUTOPILOT + + # 狀態容器 + self.status = ComponentStatus() + + # 參數容器 (只存放，不主動下載) + self.parameters: Dict[str, Any] = {} + + # 封包統計 + self.packet_stats = PacketStats() + + def update_packet_stats(self, seq: int, msg_type: int, timestamp: float) -> None: + """ + 更新封包統計 + + Args: + seq: 訊息序號 + msg_type: 訊息類型 + timestamp: 時間戳記 + """ + stats = self.packet_stats + + # 檢查遺失封包 + if stats.last_seq is not None: + expected_seq = (stats.last_seq + 1) % 256 + diff = seq - expected_seq + if diff < 0: + diff += 256 + stats.lost_count += diff + + # 更新統計資訊 + stats.received_count += 1 + stats.last_seq = seq + stats.last_msg_time = timestamp + + # 更新訊息類型計數 + if msg_type in stats.msg_type_count: + stats.msg_type_count[msg_type] += 1 + else: + stats.msg_type_count[msg_type] = 1 + + def reset_packet_stats(self) -> None: + """重置封包統計""" + self.packet_stats = PacketStats() + + def set_parameter(self, param_name: str, param_value: Any) -> None: + """設定參數 (手動餵入)""" + self.parameters[param_name] = param_value + + def get_parameter(self, param_name: str, default: Any = None) -> Any: + """取得參數""" + return self.parameters.get(param_name, default) + + def __str__(self) -> str: + return (f"Component(id={self.component_id}, type={self.type.value}, " + f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, " + f"lost={self.packet_stats.lost_count})") + + +# ====================== RF Module Class ===================== + +class RFModule: + """RF模組資訊容器""" + + def __init__(self, rf_type: RFModuleType = RFModuleType.OTHER): + self.type = rf_type + self.status = RFStatus() + self.socket_info = SocketInfo() + + def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None: + """更新RSSI""" + self.status.rssi = rssi + if timestamp: + self.status.timestamp = timestamp + + def update_at_response(self, response: str, timestamp: Optional[float] = None) -> None: + """更新AT命令回應""" + self.status.at_response = response + if timestamp: + self.status.timestamp = timestamp + + def update_socket_info(self, ip: str = None, port: int = None, + local_ip: str = None, local_port: int = None, + connected: bool = None) -> None: + """更新Socket資訊""" + if ip is not None: + self.socket_info.ip = ip + if port is not None: + self.socket_info.port = port + if local_ip is not None: + self.socket_info.local_ip = local_ip + if local_port is not None: + self.socket_info.local_port = local_port + if connected is not None: + self.socket_info.connected = connected + + def __str__(self) -> str: + return (f"RFModule(type={self.type.value}, rssi={self.status.rssi}, " + f"connected={self.socket_info.connected})") + + +# ====================== Main VehicleView Class ===================== + +class VehicleView: + """ + 載具視圖 - 純狀態容器 + + 特點: + 1. 只有狀態容器，沒有任何主動通訊功能 + 2. 不主動通訊、不背景下載參數、不搶 RF/MAVLink 流量 + 3. 可以手動餵資料 (push state) + 4. 可擴充 (支援 RF 模組狀態) + """ + + # TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了 + + def __init__(self, sysid: int): + # Meta 資訊 + self.sysid = sysid + self.kind: Optional[str] = None # 載具種類描述 (例如: "Copter", "Plane") + self.vehicle_type: Optional[int] = None # MAV_TYPE + self.connected_via: ConnectionType = ConnectionType.OTHER + + # 組件容器 {component_id: VehicleComponent} + self.components: Dict[int, VehicleComponent] = {} + + # RF模組 + self.rf_module: Optional[RFModule] = None + + # 其他自定義meta資訊 + self.custom_meta: Dict[str, Any] = {} + + def add_component(self, component_id: int, comp_type: ComponentType = ComponentType.OTHER) -> VehicleComponent: + """ + 新增組件 + + Args: + component_id: 組件ID + comp_type: 組件類型 + + Returns: + VehicleComponent: 新增的組件 + """ + if component_id not in self.components: + self.components[component_id] = VehicleComponent(component_id, comp_type) + # logger.debug(f"Added component {component_id} to system {self.sysid}") + return self.components[component_id] + + def get_component(self, component_id: int) -> Optional[VehicleComponent]: + """取得組件""" + return self.components.get(component_id) + + def remove_component(self, component_id: int) -> bool: + """移除組件""" + if component_id in self.components: + del self.components[component_id] + # logger.debug(f"Removed component {component_id} from system {self.sysid}") + return True + return False + + def reset_component_stats(self, component_id: int) -> None: + """重置指定組件的封包統計""" + component = self.get_component(component_id) + if component: + component.reset_packet_stats() + # logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}") + + def set_rf_module(self, rf_type: RFModuleType) -> RFModule: + """設定RF模組""" + self.rf_module = RFModule(rf_type) + return self.rf_module + + def get_rf_module(self) -> Optional[RFModule]: + """取得RF模組""" + return self.rf_module + + def __str__(self) -> str: + comp_list = ", ".join([str(cid) for cid in self.components.keys()]) + return (f"VehicleView(sysid={self.sysid}, kind={self.kind}, " + f"connected_via={self.connected_via.value}, " + f"components=[{comp_list}], " + f"rf_module={self.rf_module is not None})") + + def to_dict(self) -> Dict[str, Any]: + """轉換為字典格式 (方便序列化/除錯)""" + return { + 'meta': { + 'sysid': self.sysid, + 'kind': self.kind, + 'vehicle_type': self.vehicle_type, + 'connected_via': self.connected_via.value, + 'custom_meta': self.custom_meta + }, + 'components': { + cid: { + 'component_id': comp.component_id, + 'type': comp.type.value, + 'mav_type': comp.mav_type, + 'mav_autopilot': comp.mav_autopilot, + 'packet_stats': { + 'received': comp.packet_stats.received_count, + 'lost': comp.packet_stats.lost_count, + 'last_seq': comp.packet_stats.last_seq, + 'last_msg_time': comp.packet_stats.last_msg_time + }, + 'parameters_count': len(comp.parameters) + } + for cid, comp in self.components.items() + }, + 'rf_module': { + 'type': self.rf_module.type.value, + 'rssi': self.rf_module.status.rssi, + 'socket_connected': self.rf_module.socket_info.connected + } if self.rf_module else None + } + + +# ====================== Registry ===================== + +class VehicleViewRegistry: + """載具視圖註冊表 - 管理所有的 VehicleView""" + + def __init__(self): + self._vehicles: Dict[int, VehicleView] = {} + + def register(self, sysid: int) -> VehicleView: + """註冊新的載具視圖""" + if sysid not in self._vehicles: + self._vehicles[sysid] = VehicleView(sysid) + logger.info(f"Registered new VehicleView for system {sysid}") + return self._vehicles[sysid] + + def get(self, sysid: int) -> Optional[VehicleView]: + """取得載具視圖""" + return self._vehicles.get(sysid) + + def unregister(self, sysid: int) -> bool: + """註銷載具視圖""" + if sysid in self._vehicles: + del self._vehicles[sysid] + logger.info(f"Unregistered VehicleView for system {sysid}") + return True + return False + + def get_all(self) -> Dict[int, VehicleView]: + """取得所有載具視圖""" + return self._vehicles.copy() + + def clear(self) -> None: + """清空所有載具視圖""" + self._vehicles.clear() + logger.info("Cleared all VehicleViews") + + def __len__(self) -> int: + return len(self._vehicles) + + def __contains__(self, sysid: int) -> bool: + return sysid in self._vehicles + + +# ====================== Global Instance ===================== + +# 全域註冊表實例 +vehicle_registry = VehicleViewRegistry() + +''' +================= 改版記錄 ============================ + +2026.01.16 +1. 新增 重置指定組件的封包統計 功能 + +''' + diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py new file mode 100644 index 0000000..731a950 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -0,0 +1,611 @@ +''' + + +''' + +# 基礎功能的 import +import asyncio +import serial_asyncio + +import os +import sys +import serial +import signal +import time +import threading +import struct +from enum import Enum, auto + +# # XBee 模組 +# from xbee.frame import APIFrame + +# 自定義的 import +from .utils import setup_logger + +# ====================== 分割線 ===================== + +logger = setup_logger(os.path.basename(__file__)) + +# ====================== 分割線 ===================== +class XBeeFrameHandler: + """XBee API Frame 處理器""" + + @staticmethod + def parse_at_command_response(frame: bytes) -> dict: + """解析 AT Command Response (0x88)""" + if len(frame) < 8: + return None + + frame_type = frame[3] + if frame_type != 0x88: + return None + + frame_id = frame[4] + at_command = frame[5:7] + status = frame[7] + data = frame[8:] if len(frame) > 8 else b'' + + return { + 'frame_id': frame_id, + 'command': at_command, + 'status': status, + 'data': data, + 'is_ok': status == 0x00 + } + + @staticmethod + def parse_receive_packet(frame: bytes) -> dict: + # """解析 RX Packet (0x90) - 未來擴展用""" + # if len(frame) < 15 or frame[3] != 0x90: + # return None + + # return { + # 'source_addr': frame[4:12], + # 'reserved': frame[12:14], + # 'options': frame[14], + # 'data': frame[15:-1] + # } + pass + return None + + @staticmethod + def encapsulate_data(data: bytes, dest_addr64: bytes = b'\x00\x00\x00\x00\x00\x00\xFF\xFF', frame_id = 0x01) -> bytes: + """ + 將數據封裝為 XBee API 傳輸幀 + + 使用 XBee API 格式封裝數據: + - 傳輸請求幀 (0x10) + - 使用廣播地址 + - 添加適當的頭部和校驗和 + """ + frame_type = 0x10 + 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) + + @staticmethod + def decapsulate_data(data: bytes): + # 這裡可以根據需要進行數據解封裝 + + # XBee API 幀格式: + # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) + + # 檢查幀起始符 (0x7E) + if not data or len(data) < 5 or data[0] != 0x7E: + return data + + # 獲取數據長度 (不包括校驗和) + # length = (data[1] << 8) + data[2] + length = (data[1] << 8) | data[2] + + # 檢查幀完整性 + if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 + return data + + # 提取API標識符和數據 + frame_type = data[3] + # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 + + # 根據不同的幀類型進行處理 + if frame_type == 0x90: # 例如，這是"接收數據包"類型 + rf_data_start = 3 + 12 + return data[rf_data_start:3 + length] + else: + return None + return data + + +class ATCommandHandler: + """AT 指令回應處理器""" + + def __init__(self, serial_port: str): + self.serial_port = serial_port + self.handlers = { + b'DB': self._handle_rssi, + b'SH': self._handle_serial_high, + b'SL': self._handle_serial_low, + # 可擴展其他 AT 指令 + } + + def handle_response(self, response: dict): + """根據 AT 指令類型分派處理""" + if not response or not response['is_ok']: + if response: + print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}") + return + + command = response['command'] + handler = self.handlers.get(command) + + if handler: + handler(response['data']) + else: + print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}") + + def _handle_rssi(self, data: bytes): + """處理 DB (RSSI) 回應""" + if not data: + return + + rssi_value = data[0] + now = time.time() + + # 檢查是否最近有收到 MAVLink + last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0) + if now - last_mavlink_time > 0.5: + print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + return + + # 取得對應的 sysid + sysid = serial_to_sysid.get(self.serial_port) + if sysid is None: + print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + return + + # 記錄 RSSI + rssi_history[sysid].append(-rssi_value) + time_history[sysid].append(now) + # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + + def _handle_serial_high(self, data: bytes): + # """處理 SH (Serial Number High) - 範例""" + # if len(data) >= 4: + # serial_high = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + pass + + def _handle_serial_low(self, data: bytes): + # """處理 SL (Serial Number Low) - 範例""" + # if len(data) >= 4: + # serial_low = int.from_bytes(data[:4], 'big') + # print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + pass + +# ====================== 分割線 ===================== + +class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 + def __init__(self, udp_handler, serial_port_str): + self.udp_handler = udp_handler # UDP 的傳輸物件 + self.serial_port_str = serial_port_str + self.at_handler = ATCommandHandler(serial_port_str) + + self.buffer = bytearray() # 用於緩存接收到的資料 + self.transport = None # Serial 自己的傳輸物件 + # self.first_data = True # 標記是否為第一次收到資料 + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + if hasattr(self.udp_handler, 'set_serial_handler'): + self.udp_handler.set_serial_handler(self) + # logger.info(f"Serial port {self.serial_port_str} connected.") # debug + + # Serial 收到資料的處理過程 + def data_received(self, data): + # 1. 把收到的資料加入緩衝區 + self.buffer.extend(data) + + # 2. 需要完整的 header 才能解析 + while len(self.buffer) >= 3: + # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) + if self.buffer[0] != 0x7E: + self.buffer.pop(0) # 如果不是就丟掉 + continue + + # 4. 讀取 payload 長度 + length = (self.buffer[1] << 8) | self.buffer[2] + full_length = 3 + length + 1 + + # 5. 等待完整封包 + if len(self.buffer) < full_length: + break + + # 6. 提取完整 frame 並從緩衝區移除 + an_frame = self.buffer[:full_length] + del self.buffer[:full_length] + + # 7. 判斷 frame 類型 + frame_type = an_frame[3] + + if frame_type == 0x88: + # 處理 AT Command 回應 + # response = XBeeFrameHandler.parse_at_command_response(an_frame) + # self.at_handler.handle_response(response) + pass + + elif frame_type == 0x90: + # Receive Packet (RX) payload 先解碼 + processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame)) + # 轉換失敗就捨棄了 + if processed_data is None: + continue + # 再透過 UDP 送出 + self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)) + + elif frame_type == 0x8B: + pass + + else: + # 其他類型的 frame 未來可擴展處理 現在忽略 + logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}") + + # # RSSI + # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包 + # # frame[5:7] == b'DB' -> API 封包的DB參數 + # status = frame[7] # + # if status == 0x00 and len(frame) > 8: # status == 0x00 -> 這個封包是有效封包 + # rssi_value = frame[8] + # now = time.time() + + # # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port + # last_time = serial_last_mavlink_time.get(self.serial_port, 0) + # if now - last_time <= 0.5: + # sysid = serial_to_sysid.get(self.serial_port, None) + # if sysid is not None: + # rssi_history[sysid].append(-rssi_value) + # time_history[sysid].append(now) + # # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + # else: + # print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") + # else: + # print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") + # else: + # print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}") + + +class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 + + LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP + + def __init__(self, target_port): + self.target_port = target_port # 目標 UDP 端口 + + self.serial_handler = None # Serial 的傳輸物件 + self.transport = None # UDP 自己的傳輸物件 + self.remote_addr = None # 儲存動態獲取的遠程地址 # debug + # self.has_processed = False # 測試模式用 處理數據旗標 # debug + + def connection_made(self, transport): + self.transport = transport + # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug + + def set_serial_handler(self, serial_handler): + self.serial_handler = serial_handler + + # UDP 收到資料的處理過程 + def datagram_received(self, data, addr): + # 儲存對方的地址（這樣就能向同一個來源回傳數據） + # self.remote_addr = addr # debug + # print(f"Received UDP data from {addr}, setting as remote address") + + processed_data = XBeeFrameHandler.encapsulate_data(data) + + if self.serial_handler: + self.serial_handler.transport.write(processed_data) + +#================================================================== + +class SerialReceiverType(Enum): + """連接類型""" + TELEMETRY = auto() + XBEEAPI2AT = auto() + OTHER = auto() + + +class serial_manager: + + class serial_object: + def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc + self.baudrate = baudrate + self.receiver_type = receiver_type + self.target_port = target_port # 指向的 UPD 端口 + + self.transport = None # TODO 這個變數可能沒有作用 + self.protocol = None # TODO 這個變數可能沒有作用 + self.udp_handler = None + self.serial_handler = None + + def __init__(self): + self.thread = None + self.loop = None + self.running = False + self.serial_count = 0 + self.serial_objects = {} # serial id num : serial_object + + def __del__(self): + self.loop = None + self.thread = None + + def start(self): + + if self.running: + logger.warning("serial_manager already running") + return + + self.running = True + + # 啟動獨立線程 命名為 SerialManager + self.thread = threading.Thread( + target=self._run_event_loop, + name="SerialManager" + ) + self.thread.daemon = False # 不設為 daemon，確保正確關閉 + self.thread.start() + + # 等待 _run_event_loop 建立事件循環的物件 self.loop + start_timeout = 2.0 + start_time = time.time() + while not self.loop and time.time() - start_time < start_timeout: + time.sleep(0.1) + + # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop + if self.loop: + logger.info("serial_manager thread started <-") + return True + else: + logger.error("serial_manager failed to start") + return False + + def shutdown(self): + """停止 serial_manager 和其管理的所有 serial_object""" + # 自己在 running 狀態下才執行停止程序 + if not self.running: + logger.warning("serial_manager is not running") + return + + # 停止所有被管理的 serial_object + for serial_id in list(self.serial_objects.keys()): + self.remove_serial_link(serial_id) + + # 停止自己 + self.running = False + + # 解開事件循環的阻塞 + if self.loop: + self.loop.call_soon_threadsafe(self.loop.stop) + + # 等待線程結束 + if self.thread and self.thread.is_alive(): + self.thread.join(timeout=5.0) + if self.thread.is_alive(): + logger.warning("serial_manager thread did not stop gracefully") + + logger.info("serial_manager thread END!") + + def _run_event_loop(self): + """在獨立線程中運行 asyncio 事件循環""" + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + + # # 為每個 serial_object 建立連接 + # for serial_obj in self.serial_objects: + # coro = serial_asyncio.create_serial_connection( + # self.loop, + # lambda: SerialProtocol(serial_obj.receiver_type), + # serial_obj.serial_port, + # baudrate=serial_obj.baudrate + # ) + # transport, protocol = self.loop.run_until_complete(coro) + # serial_obj.transport = transport + # serial_obj.protocol = protocol + + try: + self.loop.run_forever() + finally: + self.loop.close() + + def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + + if not self.running or not self.loop: + logger.error("Event loop not running, cannot create serial link") + return False + + # 檢查 serial port 有效 + if not self.check_serial_port(serial_port, baudrate): + logger.error(f"Serial port {serial_port} validation failed") + return False + + # 使用 run_coroutine_threadsafe 執行協程並獲取結果 + future = asyncio.run_coroutine_threadsafe( + self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type), + self.loop + ) + + try: + # 等待結果，設定合理的超時時間 + result = future.result(timeout=5.0) + if result: + logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}") + return True + except asyncio.TimeoutError: + logger.error(f"Timeout creating serial link for {serial_port}") + return False + except Exception as e: + logger.error(f"Failed to create serial link for {serial_port}: {e}") + return False + + async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + """在事件循環線程中執行實際的連接創建""" + try: + # 創建 serial_object 實例 + serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) + + # 建立 UDP 處理器並指定目標端口位置 + serial_obj.udp_handler = UDPHandler(target_port) + + # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 + udp_transport, udp_protocol = await self.loop.create_datagram_endpoint( + lambda: serial_obj.udp_handler, + local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 + ) + serial_obj.transport = udp_transport + serial_obj.protocol = udp_protocol + + # logger.info(f"UDP endpoint created for {serial_port}") # debug + + # 建立 Serial 處理器，將 UDP 處理器傳給它 + serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port) + + # 建立 Serial 連接 + serial_transport, _ = await serial_asyncio.create_serial_connection( + self.loop, + lambda: serial_obj.serial_handler, + serial_port, + baudrate=baudrate + ) + + # logger.info(f"Serial connection created for {serial_port}") # debug + + # 將 serial_object 加入管理列表 + serial_id = self.serial_count + 1 + self.serial_objects[serial_id] = serial_obj + self.serial_count += 1 + + # logger.info(f"Serial object {serial_id} added to manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_create_serial_link for {serial_port}: {str(e)}") + # 清理已創建的資源 + if 'serial_obj' in locals(): + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + return False + + def remove_serial_link(self, serial_id): + """移除串口連接（線程安全方式）""" + # 確保事件循環正在運行 + if not self.loop: + logger.error("Event loop not running") + return False + + # 檢查 serial_id 是否存在 + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not found") + return False + + # 使用 run_coroutine_threadsafe 執行協程 + future = asyncio.run_coroutine_threadsafe( + self._async_remove_serial_link(serial_id), + self.loop + ) + + try: + result = future.result(timeout=3.0) + if result: + logger.info(f"Remove Serial Link {serial_id}") + return result + except asyncio.TimeoutError: + logger.error(f"Timeout removing serial link {serial_id}") + return False + except Exception as e: + logger.error(f"Failed to remove serial link {serial_id}: {e}") + return False + + async def _async_remove_serial_link(self, serial_id): + """在事件循環線程中執行實際的連接移除""" + if serial_id not in self.serial_objects: + logger.warning(f"Serial object {serial_id} not in managed list") + return False + + try: + serial_obj = self.serial_objects[serial_id] + + # 關閉 UDP transport + if hasattr(serial_obj, 'transport') and serial_obj.transport: + serial_obj.transport.close() + + # 關閉 Serial transport + if hasattr(serial_obj, 'serial_handler') and serial_obj.serial_handler: + if hasattr(serial_obj.serial_handler, 'transport') and serial_obj.serial_handler.transport: + serial_obj.serial_handler.transport.close() + + # 從管理列表中移除 + del self.serial_objects[serial_id] + # logger.info(f"Serial object {serial_id} removed from manager") # debug + return True + + except Exception as e: + logger.error(f"Exception in _async_remove_serial_link for {serial_id}: {str(e)}") + return False + + def get_serial_link(self): + ret = {} # serial id num : serial_port string + for key, obj in self.serial_objects.items(): + ret[key] = obj.serial_port + return ret + + @staticmethod + def check_serial_port(serial_port, baudrate): + """檢查串口是否存在與可用""" + # 檢查設備是否存在 + if not os.path.exists(serial_port): + logger.error(f"Serial Device {serial_port} Not Found") + return False + + # 檢查是否有權限訪問設備 + try: + if not os.access(serial_port, os.R_OK | os.W_OK): + logger.error(f"No permission to access {serial_port}") + return False + except Exception as e: + logger.error(f"Cannot Access Serial Device {serial_port}: {str(e)}") + return False + + # 檢查是否被占用 + try: + # 嘗試打開串口 + ser = serial.Serial(serial_port, baudrate) + ser.close() # 打開成功後立即關閉 + return True + except serial.SerialException as e: + logger.error(f"Serial Device {serial_port} is Occupied or Inaccessible: {str(e)}") + return False + except Exception as e: + logger.error(f"Unknown Error: {str(e)}") + return False + + +if __name__ == '__main__': + sm = serial_manager() + sm.start() + + SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 + SERIAL_BAUDRATE = 115200 + UDP_REMOTE_PORT = 14571 + sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT) + + linked_serial = sm.get_serial_link() + print(linked_serial) + time.sleep(10) + + sm.remove_serial_link(1) + time.sleep(3) + sm.shutdown() \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py new file mode 100644 index 0000000..921faf4 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py @@ -0,0 +1,7 @@ +""" +共用工具模組 +""" +from .ringBuffer import RingBuffer +from .theLogger import setup_logger + +__all__ = ['RingBuffer', 'setup_logger'] \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py new file mode 100644 index 0000000..0c6a873 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquirePort.py @@ -0,0 +1,129 @@ +import socket +import random +import os + + +def get_used_ports(): + """ + 從 /proc/net/tcp 和 /proc/net/udp 讀取系統已占用的 port + 直接讀取 Linux 系統資訊，避免暴力嘗試 + + Returns: + set: 已被占用的 port 號集合 + """ + used_ports = set() + + # 讀取 TCP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/tcp', '/proc/net/tcp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + # local_address 格式: "0100007F:1F90" (hex) + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + # 讀取 UDP 占用的 port (包含 IPv4 和 IPv6) + for filepath in ['/proc/net/udp', '/proc/net/udp6']: + if os.path.exists(filepath): + try: + with open(filepath, 'r') as f: + lines = f.readlines()[1:] # 跳過標題行 + for line in lines: + parts = line.split() + if len(parts) > 1: + local_addr = parts[1] + port_hex = local_addr.split(':')[1] + port = int(port_hex, 16) + used_ports.add(port) + except (IOError, PermissionError): + pass + + return used_ports + + +def is_port_available(port): + """ + 測試指定 port 是否可用 (TCP 和 UDP 都測試) + + Args: + port (int): 要測試的 port 號 + + Returns: + bool: True 表示可用，False 表示被占用 + """ + # 測試 TCP + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + # 測試 UDP + try: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(('', port)) + except OSError: + return False + + return True + + +def find_available_port(start_port=1024, end_port=65535): + """ + 在指定的 port 區間內隨機找出一個未被占用的 port + 使用 Linux /proc/net 系統資訊來過濾已占用的 port，避免暴力嘗試 + 確保 TCP 和 UDP 都可用 + + Args: + start_port (int): 起始 port 號 (預設 1024) + end_port (int): 結束 port 號 (預設 65535) + + Returns: + int: 可用的 port 號，如果找不到則返回 None + """ + if start_port < 1 or end_port > 65535 or start_port >= end_port: + raise ValueError("Port 範圍必須在 1-65535 之間，且起始 port 必須小於結束 port") + + # 從系統讀取已占用的 port + used_ports = get_used_ports() + + # 計算可用的 port 列表 (排除已占用的) + available_ports = [p for p in range(start_port, end_port + 1) if p not in used_ports] + + if not available_ports: + return None + + # 隨機打亂順序 + random.shuffle(available_ports) + + # 從可用列表中挑選，再用 socket 雙重確認 TCP 和 UDP 都可用 + for port in available_ports: + if is_port_available(port): + return port + + # 如果都不可用 + return None + + +if __name__ == "__main__": + # 使用範例 + port = find_available_port(8000, 9000) + if port: + print(f"找到可用的 port: {port}") + else: + print("找不到可用的 port") + + # 自訂範圍範例 + port = find_available_port(10000, 20000) + if port: + print(f"在 10000-20000 範圍找到可用的 port: {port}") diff --git a/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py new file mode 100644 index 0000000..1e1a73f --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/acquireSerial.py @@ -0,0 +1,111 @@ +""" +Serial Port Discovery Utility + +This module provides functions to discover available serial ports on the system. +It uses glob pattern matching to find serial device files in /dev/. +""" + +import glob +from typing import List, Union + + +def get_serial_ports() -> List[str]: + """ + 獲取系統中所有可用的串口設備列表 + + 在 Linux 系統中，會搜尋以下模式的設備： + - /dev/ttyUSB* (USB 串口設備) + - /dev/ttyACM* (USB CDC ACM 設備) + - /dev/ttyS* (標準串口) + + Returns: + List[str]: 包含所有找到的串口設備路徑的列表 + + Example: + >>> ports = get_serial_ports() + >>> print(ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1', '/dev/ttyS0'] + """ + serial_ports = [] + + # 搜尋不同類型的串口設備 + patterns = [ + '/dev/ttyUSB*', # USB 串口轉換器 + '/dev/ttyACM*', # USB CDC ACM 設備（如 Arduino） + '/dev/ttyS*', # 標準串口 + ] + + for pattern in patterns: + serial_ports.extend(glob.glob(pattern)) + + # 排序以便於閱讀 + serial_ports.sort() + + return serial_ports + + +def get_serial_ports_with_filter(filter_patterns: Union[str, List[str]] = None) -> List[str]: + """ + 獲取串口設備列表，可選擇性地使用自訂篩選模式 + + Args: + filter_patterns (Union[str, List[str]], optional): + 單一或多個 glob 模式 + - 字串: '/dev/ttyUSB*' + - 列表: ['/dev/ttyUSB*', '/dev/ttyACM*'] + 如果為 None，則使用預設模式搜尋所有串口 + + Returns: + List[str]: 符合條件的串口設備路徑列表 + + Example: + >>> # 單一 pattern + >>> ports = get_serial_ports_with_filter('/dev/ttyUSB*') + >>> print(ports) + ['/dev/ttyUSB0', '/dev/ttyUSB1'] + + >>> # 多個 patterns + >>> ports = get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*']) + >>> print(ports) + ['/dev/ttyACM0', '/dev/ttyUSB0', '/dev/ttyUSB1'] + """ + if filter_patterns is None: + return get_serial_ports() + + # 統一轉成 list 處理 + if isinstance(filter_patterns, str): + filter_patterns = [filter_patterns] + + serial_ports = [] + for pattern in filter_patterns: + serial_ports.extend(glob.glob(pattern)) + + serial_ports.sort() + return serial_ports + + +if __name__ == "__main__": + # 使用範例 + print("=== Serial Port Discovery ===\n") + + # 1. 獲取所有串口設備 + all_ports = get_serial_ports() + print(f"找到 {len(all_ports)} 個串口設備:") + for port in all_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 2. 只搜尋 USB 串口 + usb_ports = get_serial_ports_with_filter('/dev/ttyUSB*') + print(f"找到 {len(usb_ports)} 個 USB 串口設備:") + for port in usb_ports: + print(f" - {port}") + + print("\n" + "="*30 + "\n") + + # 3. 只搜尋 ACM 設備 + acm_ports = get_serial_ports_with_filter('/dev/ttyACM*') + print(f"找到 {len(acm_ports)} 個 ACM 設備:") + for port in acm_ports: + print(f" - {port}") diff --git a/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py new file mode 100644 index 0000000..6728426 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/ringBuffer.py @@ -0,0 +1,231 @@ +# import array +import threading +import ctypes +from typing import Any, Optional, Tuple + +class RingBuffer: + """ + 高效能無鎖環形緩衝區，使用原子操作避免鎖定 + 用於在不同執行緒間高效傳遞資料 + """ + # 緩衝區計數器，用於自動分配 buffer_id + _buffer_counter = 0 + _counter_lock = threading.Lock() + + def __init__(self, capacity: int = 256, buffer_id: int = None): + """ + 初始化環形緩衝區 + + Args: + capacity: 緩衝區容量 (必須是 2 的次方) + buffer_id: 緩衝區 ID，如果為 None 則自動分配 + """ + # 確保容量是 2 的次方，便於使用位運算取模 + if capacity & (capacity - 1) != 0: + # 找到大於等於 capacity 的最小 2 的次方 + capacity = 1 << (capacity - 1).bit_length() + + # 分配緩衝區 ID + if buffer_id is None: + with RingBuffer._counter_lock: + buffer_id = RingBuffer._buffer_counter + RingBuffer._buffer_counter += 1 + + self.buffer_id = buffer_id + self.capacity = capacity + self.mask = capacity - 1 # 用於快速取模 + self.buffer = [None] * capacity + + # 原子計數器作為讀寫指標 + self.write_index = ctypes.c_uint64(0) + self.read_index = ctypes.c_uint64(0) + + # 用於檢測上次操作的執行緒 ID + self._last_read_thread = None + self._last_write_thread = None + + # 添加同時讀寫統計 + self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數 + self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數 + self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數 + self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數 + self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數 + + # 記錄各執行緒的操作次數 + self.thread_write_counts = {} # {thread_id: count} + self.thread_read_counts = {} # {thread_id: count} + + # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作） + self._stats_lock = threading.Lock() + + def put(self, item: Any) -> bool: + """ + 將項目存入緩衝區 + + Args: + item: 要存入的項目 + + Returns: + bool: 成功寫入返回 True，緩衝區已滿返回 False + """ + # 更新寫入統計 + self.total_write_count.value += 1 + + # 檢測上次寫入的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒寫入次數 + with self._stats_lock: + self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時寫入 + if self._last_write_thread is not None and current_thread != self._last_write_thread: + self.concurrent_write_count.value += 1 + + self._last_write_thread = current_thread + + # 原子獲取當前寫入位置 + current = self.write_index.value + next_pos = (current + 1) & self.mask + + # 檢查緩衝區是否已滿 + if next_pos == self.read_index.value: + self.overflow_count.value += 1 + return False + + # 寫入資料並原子更新寫入位置 + self.buffer[current] = item + self.write_index.value = next_pos + return True + + def get(self) -> Optional[Any]: + """ + 從緩衝區讀取項目 + + Returns: + 項目或 None（如果緩衝區為空） + """ + # 更新讀取統計 + self.total_read_count.value += 1 + + # 檢測上次讀取的執行緒 + current_thread = threading.get_ident() + + # 記錄當前執行緒讀取次數 + with self._stats_lock: + self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1 + + # 檢測是否為不同執行緒同時讀取 + if self._last_read_thread is not None and current_thread != self._last_read_thread: + self.concurrent_read_count.value += 1 + + self._last_read_thread = current_thread + + # 檢查緩衝區是否為空 + if self.read_index.value == self.write_index.value: + return None + + # 原子獲取當前讀取位置並讀取資料 + current = self.read_index.value + item = self.buffer[current] + + # 原子更新讀取位置 + self.read_index.value = (current + 1) & self.mask + return item + + def get_all(self) -> list: + """ + 獲取緩衝區中的所有項目 + + Returns: + list: 所有可用項目的列表 + """ + items = [] + while True: + item = self.get() + if item is None: + break + items.append(item) + return items + + def size(self) -> int: + # 返回目前緩衝區內項目數量 + return (self.write_index.value - self.read_index.value) & self.mask + + def is_empty(self) -> bool: + # 檢查緩衝區是否為空 + return self.read_index.value == self.write_index.value + + def is_full(self) -> bool: + # 檢查緩衝區是否已滿 + return ((self.write_index.value + 1) & self.mask) == self.read_index.value + + def clear(self) -> None: + """清空緩衝區""" + self.read_index.value = self.write_index.value + + def get_stats(self) -> dict: + """ + 獲取緩衝區統計資訊 + + Returns: + dict: 包含各種統計數據的字典 + """ + with self._stats_lock: + stats = { + "buffer_id": self.buffer_id, + "capacity": self.capacity, + "current_size": self.size(), + "is_full": self.is_full(), + "is_empty": self.is_empty(), + "total_writes": self.total_write_count.value, + "total_reads": self.total_read_count.value, + "concurrent_writes": self.concurrent_write_count.value, + "concurrent_reads": self.concurrent_read_count.value, + "overflow_count": self.overflow_count.value, + "write_threads": len(self.thread_write_counts), + "read_threads": len(self.thread_read_counts), + "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value), + "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value), + "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3], + "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3], + } + return stats + + def print_stats(self) -> None: + """輸出當前緩衝區統計資訊""" + stats = self.get_stats() + + print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===") + print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}") + print(f"Total Write Operations: {stats['total_writes']}") + print(f"Total Read Operations: {stats['total_reads']}") + print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})") + print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})") + print(f"Buffer Overflow Count: {stats['overflow_count']}") + print(f"Writing Threads: {stats['write_threads']}") + print(f"Reading Threads: {stats['read_threads']}") + + print("Top Writer Threads:") + for thread_id, count in stats['top_writer_threads']: + print(f" Thread {thread_id}: {count} writes") + + print("Top Reader Threads:") + for thread_id, count in stats['top_reader_threads']: + print(f" Thread {thread_id}: {count} reads") + print("=============================\n") + + def reset_stats(self) -> None: + """重置所有統計數據""" + with self._stats_lock: + self.concurrent_write_count.value = 0 + self.concurrent_read_count.value = 0 + self.total_write_count.value = 0 + self.total_read_count.value = 0 + self.overflow_count.value = 0 + self.thread_write_counts.clear() + self.thread_read_counts.clear() + + def __str__(self) -> str: + """返回緩衝區的字符串表示""" + return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})" \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py new file mode 100644 index 0000000..dce2ce7 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py @@ -0,0 +1,43 @@ +import logging +import os +from logging.handlers import TimedRotatingFileHandler + +# 全域 Logger 實例 +_global_logger = None + +def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger: + global _global_logger + + if _global_logger is None: + # 確保 logs 資料夾存在 + os.makedirs(log_dir, exist_ok=True) + + # 建立全域 Logger + _global_logger = logging.getLogger("global_logger") + _global_logger.setLevel(level) + _global_logger.propagate = False # 防止重複輸出 + + formatter = logging.Formatter( + fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s", + datefmt="%m-%d %H:%M:%S" + ) + + # 檔案輸出（每天輪替） + file_handler = TimedRotatingFileHandler( + filename=os.path.join(log_dir, "application.log"), + when="midnight", # 每天 0 點輪替 + backupCount=7, # 保留 7 天 + encoding="utf-8" + ) + file_handler.setFormatter(formatter) + _global_logger.addHandler(file_handler) + + # 終端機輸出 + console_handler = logging.StreamHandler() + console_handler.setFormatter(formatter) + _global_logger.addHandler(console_handler) + + # 為每個模組建立子 Logger，並設定名稱 + module_logger = _global_logger.getChild(name) + module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱 + return module_logger diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py index 33414cb..b28ac96 100644 --- a/src/fc_network_adapter/setup.py +++ b/src/fc_network_adapter/setup.py @@ -20,6 +20,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'mavlink_orchestrator = fc_network_adapter.mainOrchestrator:main', ], }, ) diff --git a/src/fc_network_adapter/tests/__init__.py b/src/fc_network_adapter/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/tests/demo_integration.py b/src/fc_network_adapter/tests/demo_integration.py new file mode 100644 index 0000000..11ac1d7 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_integration.py @@ -0,0 +1,277 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +# 基礎功能的 import +import queue +import time + +# ROS2 的 import +import rclpy + +# mavlink 的 import +from pymavlink import mavutil + +# 自定義的 import +from ..fc_network_adapter import mavlinkObject as mo +from ..fc_network_adapter import mavlinkVehicleView as mvv +# from ..fc_network_adapter import mavlinkDevice as md + +# ====================== 分割線 ===================== + +test_item = 1 +running_time = 3 + + +print('test_item : ', test_item) + +''' +測試項 個位數 表示分離的功能 + +測試項 1X 表示 mavlink_object 的功能 測試連線的能力 +''' + +if test_item == 1: + print('===> Start of Program .Test ', test_item) + + connection_string="udp:127.0.0.1:14591" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + # mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + time.sleep(1) + + print("mark A") + + # print("Socket IP:", mavlink_socket1.target_system) + print("Socket port:", mavlink_socket1.port.getsockname()) + + # print("=== ", dir(mavlink_socket1.port)) + + +elif test_item == 10: + # 需要開啟一個 ardupilot 的模擬器 + # 測試 mavlink_object 放入 ring buffer 的應用 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0..1:14571" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + + sock = mavlink_socket1.port + print("Socket port:", sock) + + manager.add_mavlink_object(mavlink_object1) + + start_time = time.time() + while (time.time() - start_time) < running_time: + items_a = mo.stream_bridge_ring.get_all() + items_b = mo.return_packet_ring.get_all() + try: + print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}") + except IndexError: + print("stream_bridge_ring is empty") + + try: + print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}") + except IndexError: + print("return_packet_ring is empty") + time.sleep(1) + + manager.shutdown() + + print('<=== End of Program') + +elif test_item == 11: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14571" + mavlink_socket1 = mavutil.mavlink_connection(connection_string) + mavlink_object1 = mo.mavlink_object(mavlink_socket1) + manager.add_mavlink_object(mavlink_object1) + + # 啟動 mavlink_bridge + bridge = mo.mavlink_bridge() + bridge.start() + + time.sleep(3) + + # 印出目前所有 mavlink_systems 的內容 + print('目前所有的系統 : ') + all_vehicles = mvv.vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle}") + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + if (time.time() - show_time) >= 2: + # print("mark B") + + show_time = time.time() + for sysid, vehicle in all_vehicles.items(): + for compid in vehicle.components: + comp = vehicle.get_component(compid) + print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, comp.packet_stats.received_count)) + comp.reset_packet_stats() + print("===================") + + manager.shutdown() + bridge.stop() + + print('<=== End of Program') + +elif test_item == 12: + # 需要開啟一個 ardupilot 的模擬器 與 GCS + # 這邊是測試 mavlink object 作為交換器功能的代碼 + print('===> Start of Program .Test ', test_item) + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 初始化輸入通道 + connection_string="udp:127.0.0.1:14571" + mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) + mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) + + connection_string="udp:127.0.0.1:14571" + mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) + mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) + + # 初始化輸出通道 + connection_string="udpout:127.0.0.1:14551" + mavlink_socket_out = mavutil.mavlink_connection(connection_string) + mavlink_object_out = mo.mavlink_object(mavlink_socket_out) + + manager.add_mavlink_object(mavlink_object_out) + manager.add_mavlink_object(mavlink_object_in1) + manager.add_mavlink_object(mavlink_object_in2) + + time.sleep(1) # 等待通道啟動 + + mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id) + + mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id) + mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id) + + start_time = time.time() + while (time.time() - start_time) < running_time: + + time.sleep(1) + + manager.shutdown() + + print('<=== End of Program') + + + +elif test_item == 21: + # 需要開啟一個 ardupilot 的模擬器 + # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 + + print('===> Start of Program .Test ', test_item) + # 初始化 rclpy 才能使用 node + rclpy.init() + + # 清空 ring buffer + mo.stream_bridge_ring.clear() + mo.return_packet_ring.clear() + + manager = mo.async_io_manager() + manager.start() + + # 啟動 mavlink_bridge + analyzer = mo.mavlink_bridge() + # 關於 Node 的初始化 + show_time = time.time() + analyzer._init_node() # 初始化 node + print('初始化 node 完成 耗時 : ',time.time() - show_time) + + time.sleep(0.5) # 系統 Setup 完成 + + + # 創建通道 + connection_string="udp:127.0.0.1:14560" + mavlink_socket = mavutil.mavlink_connection(connection_string) + mavlink_object3 = mo.mavlink_object(mavlink_socket) + manager.add_mavlink_object(mavlink_object3) + + print('=== waiting for mavlink data ...') + time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 + + print('目前所有的系統 : ') + for sysid in analyzer.mavlink_systems: + print(analyzer.mavlink_systems[sysid]) + + compid = 1 + sysid = 1 + start_time = time.time() + analyzer.create_flightMode(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") + + start_time = time.time() + show_time = time.time() + while time.time() - start_time < running_time: + try: + # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode']) + analyzer.emit_info() # 這邊是測試 node 的運行 + time.sleep(1) + except KeyboardInterrupt: + break + + + # 程式結束 + analyzer.destroy_node() + rclpy.shutdown() + + # 結束程式 退出所有 thread + manager.stop() + analyzer.stop() + analyzer.thread.join() + + mavlink_socket.close() + print('<=== End of Program') + + +elif test_item == 52: + print('===> Start of Program .Test ', test_item) + + manager = mo.async_io_manager() + manager.start() + + # print(manager.thread.is_alive()) + + manager.shutdown() + + time.sleep(1) + + print('manager stopped') + diff --git a/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py new file mode 100644 index 0000000..d6954d3 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_mavlinkVehicleView.py @@ -0,0 +1,331 @@ +""" +VehicleView 使用範例 +展示如何使用純狀態容器來管理 MAVLink 載具資訊 +""" + +import time +from ..fc_network_adapter.mavlinkVehicleView import ( + VehicleView, + VehicleComponent, + RFModule, + vehicle_registry, + ConnectionType, + ComponentType, + RFModuleType +) + + +def example_basic_usage(): + """基本使用範例""" + print("=== 基本使用範例 ===\n") + + # 1. 建立載具視圖 + vehicle = VehicleView(sysid=1) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + + print(f"建立載具: {vehicle}\n") + + # 2. 新增 autopilot 組件 + autopilot = vehicle.add_component( + component_id=1, + comp_type=ComponentType.AUTOPILOT + ) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + print(f"新增組件: {autopilot}\n") + + # 3. 手動餵入位置資訊 + autopilot.status.position.latitude = 25.0330 + autopilot.status.position.longitude = 121.5654 + autopilot.status.position.altitude = 100.5 + autopilot.status.position.timestamp = time.time() + + print(f"位置: 緯度={autopilot.status.position.latitude}, " + f"經度={autopilot.status.position.longitude}, " + f"高度={autopilot.status.position.altitude}m\n") + + # 4. 手動餵入姿態資訊 + autopilot.status.attitude.roll = 0.05 # 弧度 + autopilot.status.attitude.pitch = -0.02 + autopilot.status.attitude.yaw = 1.57 + autopilot.status.attitude.timestamp = time.time() + + print(f"姿態: Roll={autopilot.status.attitude.roll:.3f}, " + f"Pitch={autopilot.status.attitude.pitch:.3f}, " + f"Yaw={autopilot.status.attitude.yaw:.3f} rad\n") + + # 5. 手動餵入飛行模式 + autopilot.status.mode.base_mode = 89 + autopilot.status.mode.custom_mode = 4 + autopilot.status.mode.mode_name = "GUIDED" + autopilot.status.mode.timestamp = time.time() + + print(f"飛行模式: {autopilot.status.mode.mode_name}\n") + + # 6. 手動餵入電池資訊 + autopilot.status.battery.voltage = 12.6 + autopilot.status.battery.current = 15.2 + autopilot.status.battery.remaining = 75 + autopilot.status.battery.timestamp = time.time() + + print(f"電池: 電壓={autopilot.status.battery.voltage}V, " + f"電流={autopilot.status.battery.current}A, " + f"剩餘={autopilot.status.battery.remaining}%\n") + + +def example_packet_tracking(): + """封包追蹤範例""" + print("\n=== 封包追蹤範例 ===\n") + + vehicle = VehicleView(sysid=2) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 模擬接收封包 + timestamp = time.time() + + # 接收 HEARTBEAT (msg_type=0) + autopilot.update_packet_stats(seq=0, msg_type=0, timestamp=timestamp) + + # 接收 ATTITUDE (msg_type=30) + autopilot.update_packet_stats(seq=1, msg_type=30, timestamp=timestamp+0.1) + + # 接收 GLOBAL_POSITION_INT (msg_type=33) + autopilot.update_packet_stats(seq=2, msg_type=33, timestamp=timestamp+0.2) + + # 模擬封包遺失 (seq 跳過 3, 4, 5) + autopilot.update_packet_stats(seq=6, msg_type=0, timestamp=timestamp+0.3) + + stats = autopilot.packet_stats + print(f"封包統計:") + print(f" 接收: {stats.received_count}") + print(f" 遺失: {stats.lost_count}") + print(f" 最後序號: {stats.last_seq}") + print(f" 訊息類型計數: {stats.msg_type_count}\n") + + +def example_parameters(): + """參數管理範例""" + print("\n=== 參數管理範例 ===\n") + + vehicle = VehicleView(sysid=3) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # 手動設定參數 (不會主動下載) + autopilot.set_parameter("ARMING_CHECK", 1) + autopilot.set_parameter("ANGLE_MAX", 4500) + autopilot.set_parameter("WPNAV_SPEED", 500) + + print(f"參數數量: {len(autopilot.parameters)}") + print(f"ARMING_CHECK = {autopilot.get_parameter('ARMING_CHECK')}") + print(f"ANGLE_MAX = {autopilot.get_parameter('ANGLE_MAX')}") + print(f"WPNAV_SPEED = {autopilot.get_parameter('WPNAV_SPEED')}\n") + + +def example_rf_module(): + """RF模組範例""" + print("\n=== RF模組範例 ===\n") + + vehicle = VehicleView(sysid=4) + vehicle.connected_via = ConnectionType.SERIAL + + # 設定 XBee RF 模組 + rf = vehicle.set_rf_module(RFModuleType.XBEE) + + # 更新 Socket 資訊 + rf.update_socket_info( + ip="192.168.1.100", + port=14550, + local_ip="192.168.1.1", + local_port=14551, + connected=True + ) + + # 更新 RSSI + rf.update_rssi(rssi=-65, timestamp=time.time()) + + # 更新 AT 命令回應 + rf.update_at_response("OK", timestamp=time.time()) + + # 自定義狀態 + rf.status.custom_status['signal_quality'] = 'excellent' + rf.status.custom_status['packet_error_rate'] = 0.001 + + print(f"RF模組: {rf}") + print(f"Socket: {rf.socket_info.ip}:{rf.socket_info.port}") + print(f"RSSI: {rf.status.rssi} dBm") + print(f"AT回應: {rf.status.at_response}") + print(f"自定義狀態: {rf.status.custom_status}\n") + + +def example_multiple_components(): + """多組件範例""" + print("\n=== 多組件範例 ===\n") + + vehicle = VehicleView(sysid=5) + vehicle.kind = "Copter with Gimbal" + + # Autopilot 組件 + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.mode.mode_name = "AUTO" + + # Gimbal 組件 + gimbal = vehicle.add_component(154, ComponentType.GIMBAL) + gimbal.mav_type = 26 # MAV_TYPE_GIMBAL + gimbal.status.attitude.pitch = -0.785 # 向下45度 + gimbal.status.attitude.yaw = 0.0 + + # Camera 組件 + camera = vehicle.add_component(100, ComponentType.CAMERA) + camera.mav_type = 30 # MAV_TYPE_CAMERA + camera.status.custom_status['recording'] = True + camera.status.custom_status['photo_interval'] = 2.0 + + print(f"載具: {vehicle}") + print(f"組件數量: {len(vehicle.components)}") + for cid, comp in vehicle.components.items(): + print(f" 組件 {cid}: {comp.type.value}, MAV_TYPE={comp.mav_type}") + print() + + +def example_registry(): + """註冊表使用範例""" + print("\n=== 註冊表使用範例 ===\n") + + # 註冊多個載具 + v1 = vehicle_registry.register(sysid=1) + v1.kind = "Copter-1" + v1.add_component(1, ComponentType.AUTOPILOT) + + v2 = vehicle_registry.register(sysid=2) + v2.kind = "Plane-1" + v2.add_component(1, ComponentType.AUTOPILOT) + + v3 = vehicle_registry.register(sysid=3) + v3.kind = "Rover-1" + v3.add_component(1, ComponentType.AUTOPILOT) + + print(f"註冊表中的載具數量: {len(vehicle_registry)}") + + # 取得所有載具 + all_vehicles = vehicle_registry.get_all() + for sysid, vehicle in all_vehicles.items(): + print(f" System {sysid}: {vehicle.kind}") + + # 檢查載具是否存在 + print(f"\nSystem 2 存在? {2 in vehicle_registry}") + print(f"System 99 存在? {99 in vehicle_registry}") + + # 取得特定載具 + vehicle = vehicle_registry.get(2) + if vehicle: + print(f"\n取得載具: {vehicle}") + + # 註銷載具 + vehicle_registry.unregister(3) + print(f"\n註銷 System 3 後，剩餘載具: {len(vehicle_registry)}\n") + + +def example_serialization(): + """序列化範例 (除錯/日誌用)""" + print("\n=== 序列化範例 ===\n") + + vehicle = VehicleView(sysid=10) + vehicle.kind = "Test Copter" + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['firmware'] = 'ArduCopter 4.3.0' + vehicle.custom_meta['frame_type'] = 'X' + + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 + autopilot.status.position.altitude = 50.0 + autopilot.status.battery.voltage = 12.4 + autopilot.update_packet_stats(0, 0, time.time()) + autopilot.update_packet_stats(1, 30, time.time()) + + rf = vehicle.set_rf_module(RFModuleType.UDP) + rf.update_rssi(-70) + rf.update_socket_info(ip="192.168.1.200", port=14550, connected=True) + + # 轉換為字典 + data = vehicle.to_dict() + + print("載具資料 (字典格式):") + import json + print(json.dumps(data, indent=2, ensure_ascii=False)) + + +def example_gps_ekf(): + """GPS 與 EKF 範例""" + print("\n\n=== GPS 與 EKF 範例 ===\n") + + vehicle = VehicleView(sysid=11) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # GPS 資訊 + autopilot.status.gps.fix_type = 3 # 3D Fix + autopilot.status.gps.satellites_visible = 12 + autopilot.status.gps.eph = 120 # HDOP = 1.2 + autopilot.status.gps.epv = 180 # VDOP = 1.8 + autopilot.status.gps.timestamp = time.time() + + print(f"GPS:") + print(f" Fix Type: {autopilot.status.gps.fix_type}") + print(f" 衛星數: {autopilot.status.gps.satellites_visible}") + print(f" HDOP: {autopilot.status.gps.eph/100}") + + # EKF 資訊 + autopilot.status.ekf.flags = 0x1FF # 所有 flags 都 OK + autopilot.status.ekf.velocity_variance = 0.5 + autopilot.status.ekf.pos_horiz_variance = 1.2 + autopilot.status.ekf.pos_vert_variance = 2.0 + autopilot.status.ekf.timestamp = time.time() + + print(f"\nEKF:") + print(f" Flags: 0x{autopilot.status.ekf.flags:X}") + print(f" 速度變異: {autopilot.status.ekf.velocity_variance}") + print(f" 水平位置變異: {autopilot.status.ekf.pos_horiz_variance}") + print(f" 垂直位置變異: {autopilot.status.ekf.pos_vert_variance}\n") + + +def example_vfr_hud(): + """VFR HUD 範例""" + print("\n=== VFR HUD 範例 ===\n") + + vehicle = VehicleView(sysid=12) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + + # VFR HUD 資訊 + autopilot.status.vfr.airspeed = 15.5 # m/s + autopilot.status.vfr.groundspeed = 14.8 # m/s + autopilot.status.vfr.heading = 90 # 東方 + autopilot.status.vfr.throttle = 65 # % + autopilot.status.vfr.climb = 2.5 # m/s + autopilot.status.vfr.timestamp = time.time() + + print(f"VFR HUD:") + print(f" 空速: {autopilot.status.vfr.airspeed} m/s") + print(f" 地速: {autopilot.status.vfr.groundspeed} m/s") + print(f" 航向: {autopilot.status.vfr.heading}°") + print(f" 油門: {autopilot.status.vfr.throttle}%") + print(f" 爬升率: {autopilot.status.vfr.climb} m/s\n") + + +if __name__ == "__main__": + # 執行所有範例 + # example_basic_usage() + # example_packet_tracking() + # example_parameters() + # example_rf_module() + # example_multiple_components() + # example_registry() + # example_serialization() + # example_gps_ekf() + example_vfr_hud() + + print("\n" + "="*50) + print("所有範例執行完成！") + print("="*50) diff --git a/src/fc_network_adapter/tests/demo_ringBuffer.py b/src/fc_network_adapter/tests/demo_ringBuffer.py new file mode 100644 index 0000000..a01ed73 --- /dev/null +++ b/src/fc_network_adapter/tests/demo_ringBuffer.py @@ -0,0 +1,152 @@ +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + +import time +import threading + +from ..fc_network_adapter.utils import RingBuffer + + +def producer(buffer, count, interval=0.01): + """生產者：向緩衝區添加資料""" + print(f"Producer started (thread {threading.get_ident()})") + for i in range(count): + # 嘗試寫入數據，直到成功 + while not buffer.put(f"Item-{i}"): + print(f"Buffer full, producer waiting... (thread {threading.get_ident()})") + time.sleep(0.1) + + print(f"Produced: Item-{i}, buffer size: {buffer.size()}") + time.sleep(interval) # 模擬生產過程 + + print(f"Producer finished (thread {threading.get_ident()})") + +def consumer(buffer, max_items, interval=0.05): + """消費者：從緩衝區讀取資料""" + print(f"Consumer started (thread {threading.get_ident()})") + items_consumed = 0 + + while items_consumed < max_items: + # 嘗試讀取數據 + item = buffer.get() + if item: + print(f"Consumed: {item}, buffer size: {buffer.size()}") + items_consumed += 1 + else: + print(f"Buffer empty, consumer waiting... (thread {threading.get_ident()})") + + time.sleep(interval) # 模擬消費過程 + + print(f"Consumer finished (thread {threading.get_ident()})") + +def batch_consumer(buffer, interval=0.2): + """批量消費者：一次性讀取緩衝區中的所有資料""" + print(f"Batch consumer started (thread {threading.get_ident()})") + + for _ in range(5): # 執行5次批量讀取 + time.sleep(interval) # 等待緩衝區積累數據 + items = buffer.get_all() + if items: + print(f"Batch consumed {len(items)} items: {items}") + else: + print("Buffer empty for batch consumer") + + print(f"Batch consumer finished (thread {threading.get_ident()})") + +def demonstrate_multi_writer(): + """示範多個寫入執行緒同時操作緩衝區""" + print("\n=== Demonstrating Multiple Writers ===") + buffer = RingBuffer(capacity=80) + + # 創建多個生產者執行緒 + threads = [] + for i in range(3): + thread = threading.Thread(target=producer, args=(buffer, 5, 0.1 * (i+1))) + threads.append(thread) + thread.start() + + # 等待所有執行緒完成 + for thread in threads: + thread.join() + + buffer.print_stats() # 印出統計資訊 + + # 讀出所有剩餘資料 + remaining = buffer.get_all() + print(f"Remaining items in buffer after multiple writers: {remaining}") + +def demonstrate_basic_usage(): + """示範基本使用方式""" + print("\n=== Demonstrating Basic Usage ===") + # 創建緩衝區 + buffer = RingBuffer(capacity=20, buffer_id=7) + + # 檢查初始狀態 + print(f"Initial buffer state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 添加幾個項目 + for i in range(5): + buffer.put(f"Test-{i}") + + # 檢查狀態 + print(f"After adding 5 items - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 讀取一個項目 + item = buffer.get() + print(f"Read item: {item}") + print(f"After reading 1 item - Content Size: {buffer.size()}") + + # 添加更多項目直到滿 + items_added = 0 + while not buffer.is_full(): + buffer.put(f"Fill-{items_added}") + items_added += 1 + + print(f"Added {items_added} more items until full") + print(f"Buffer full state - Empty: {buffer.is_empty()}, Full: {buffer.is_full()}, Content Size: {buffer.size()}") + + # 嘗試添加到已滿的緩衝區 + result = buffer.put("Overflow") + print(f"Attempt to add to full buffer: {'Succeeded' if result else 'Failed'}") + + # 獲取所有項目 + all_items = buffer.get_all() + print(f"All items in buffer: {all_items}") + print(f"Buffer after get_all() - Empty: {buffer.is_empty()}, Content Size: {buffer.size()}") + + # 印出統計資訊 + buffer.print_stats() + +def demonstrate_producer_consumer(): + """示範生產者-消費者模式""" + print("\n=== Demonstrating Producer-Consumer Pattern ===") + buffer = RingBuffer(capacity=16) + + # 創建生產者和消費者執行緒 + producer_thread = threading.Thread(target=producer, args=(buffer, 20, 0.1)) + consumer_thread = threading.Thread(target=consumer, args=(buffer, 3, 0.2)) + batch_thread = threading.Thread(target=batch_consumer, args=(buffer, 0.5)) + + # 啟動執行緒 + producer_thread.start() + consumer_thread.start() + batch_thread.start() + + # 等待執行緒完成 + producer_thread.join() + consumer_thread.join() + batch_thread.join() + + # 檢查最終狀態 + print(f"Final buffer state - Empty: {buffer.is_empty()}, Size: {buffer.size()}") + + buffer.print_stats() + +if __name__ == "__main__": + # 展示各種使用場景 + # demonstrate_basic_usage() + # demonstrate_producer_consumer() + demonstrate_multi_writer() + + print("\nAll demonstrations completed!") \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_mavlinkObject.py b/src/fc_network_adapter/tests/test_mavlinkObject.py new file mode 100644 index 0000000..9c3ca78 --- /dev/null +++ b/src/fc_network_adapter/tests/test_mavlinkObject.py @@ -0,0 +1,468 @@ +#!/usr/bin/env python +""" +測試腳本，用於測試 mavlinkObject.py 中的 mavlink_object 和 async_io_manager 類別 +""" + +import os +import sys +sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) + + +import unittest +import time +import threading +import socket +import asyncio + +# 導入要測試的模組 +from ..fc_network_adapter.mavlinkObject import ( + mavlink_object, + async_io_manager, + MavlinkObjectState, + stream_bridge_ring, + return_packet_ring +) + +# 預先定義好的真實 MAVLink heartbeat 封包 (MAVLink 1.0 格式) +# Format: STX(0xFE) + LEN + SEQ + SYS + COMP + MSG_ID + PAYLOAD(9 bytes for heartbeat) + CRC(2 bytes) +HEARTBEAT_PACKET_1 = bytes([ + 0xFE, # STX (MAVLink 1.0) + 0x09, # payload length (9 bytes) + 0x00, # sequence + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + # Payload (9 bytes): custom_mode(4), type(1), autopilot(1), base_mode(1), system_status(1), mavlink_version(1) + 0x00, 0x00, 0x00, 0x00, # custom_mode = 0 + 0x02, # type = MAV_TYPE_QUADROTOR (2) + 0x03, # autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA (3) + 0x40, # base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (64) + 0x03, # system_status = MAV_STATE_STANDBY (3) + 0x03, # mavlink_version = 3 + 0x62, 0x8E # CRC (simplified placeholder) +]) + +HEARTBEAT_PACKET_2 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x01, # sequence (增加) + 0x01, # system ID = 1 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x41, 0x03, 0x03, + 0x33, 0xEC +]) + +HEARTBEAT_PACKET_3 = bytes([ + 0xFE, # STX + 0x09, # payload length + 0x02, # sequence + 0x02, # system ID = 2 + 0x01, # component ID = 1 + 0x00, # message ID (HEARTBEAT = 0) + 0x00, 0x00, 0x00, 0x00, + 0x02, 0x03, 0x42, 0x03, 0x03, + 0x37, 0x44 +]) + + +class MockMavlinkSocket: + """模擬 Mavlink Socket 的類別，用於測試 + 使用真實的 MAVLink 封包，而不是模擬的訊息對象 + """ + + def __init__(self, test_packets=None): + """ + Args: + test_packets: list of bytes，每個元素都是完整的 MAVLink 封包 + """ + self.closed = False + self.test_packets = test_packets or [] + self.packet_index = 0 + self.written_data = [] + + # 使用 pymavlink 來解析封包 + from pymavlink import mavutil + self.mav_parser = mavutil.mavlink.MAVLink(self) + + def recv_msg(self): + """返回解析後的 MAVLink 訊息對象""" + if not self.test_packets or self.packet_index >= len(self.test_packets): + return None + + packet = self.test_packets[self.packet_index] + self.packet_index += 1 + + # 使用 pymavlink 解析封包 + try: + for byte in packet: + msg = self.mav_parser.parse_char(bytes([byte])) + if msg: + return msg + except Exception as e: + print(f"Error parsing packet: {e}") + return None + + return None + + def write(self, data): + """寫入數據（用於檢查轉發）""" + self.written_data.append(data) + + def close(self): + """關閉 socket""" + self.closed = True + + +class TestMavlinkObject(unittest.TestCase): + """測試 mavlink_object 類別的獨立功能""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建模擬的 socket，使用真實封包 + self.mock_socket = MockMavlinkSocket([HEARTBEAT_PACKET_1]) + + # 創建測試對象 + self.mavlink_obj = mavlink_object(self.mock_socket) + + def test_initialization(self): + """測試 mavlink_object 初始化是否正確""" + self.assertEqual(self.mavlink_obj.socket_id, 0) + self.assertEqual(self.mavlink_obj.state, MavlinkObjectState.INIT) + self.assertEqual(len(self.mavlink_obj.target_sockets), 0) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0]) + self.assertEqual(self.mavlink_obj.return_msg_types, []) + + def test_add_remove_target_socket(self): + """測試添加和移除目標端口功能""" + # 添加目標端口 + self.assertTrue(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + self.assertEqual(self.mavlink_obj.target_sockets[0], 1) + + self.assertTrue(self.mavlink_obj.add_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + self.assertIn(2, self.mavlink_obj.target_sockets) + + # 嘗試添加已存在的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(1)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 嘗試添加自己的端口 + self.assertFalse(self.mavlink_obj.add_target_socket(0)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 2) + + # 移除端口 + self.assertTrue(self.mavlink_obj.remove_target_socket(2)) + self.assertEqual(len(self.mavlink_obj.target_sockets), 1) + + # 嘗試移除不存在的端口 + self.assertFalse(self.mavlink_obj.remove_target_socket(2)) + + def test_set_message_types(self): + """測試設置訊息類型功能""" + # 設置橋接器訊息類型 + self.assertTrue(self.mavlink_obj.set_bridge_message_types([0, 30])) + self.assertEqual(self.mavlink_obj.bridge_msg_types, [0, 30]) + + # 設置回傳處理器訊息類型 + self.assertTrue(self.mavlink_obj.set_return_message_types([32])) + self.assertEqual(self.mavlink_obj.return_msg_types, [32]) + + # 測試無效的訊息類型 + self.assertFalse(self.mavlink_obj.set_bridge_message_types("invalid")) + self.assertFalse(self.mavlink_obj.set_return_message_types([0, "invalid"])) + + def test_send_message_validation(self): + """測試 send_message 的數據驗證功能（不需要啟動 manager）""" + # 測試非運行狀態下發送消息 + self.assertFalse(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + + # 測試無效的數據類型 + self.mavlink_obj.state = MavlinkObjectState.RUNNING # 臨時設置狀態 + self.assertFalse(self.mavlink_obj.send_message("invalid")) + self.assertFalse(self.mavlink_obj.send_message(123)) + + # 測試太短的封包 + self.assertFalse(self.mavlink_obj.send_message(bytes([0xFE, 0x00]))) + + # 測試無效的起始標記 + invalid_packet = bytes([0xFF, 0x09, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00]) + self.assertFalse(self.mavlink_obj.send_message(invalid_packet)) + + # 測試有效的封包可以加入佇列 + self.assertTrue(self.mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(self.mavlink_obj.outgoing_msgs), 1) + + self.mavlink_obj.state = MavlinkObjectState.INIT # 恢復狀態 + +class TestAsyncIOManager(unittest.TestCase): + """測試 async_io_manager 類別的獨立功能""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + # 創建模擬 mavlink 對象，使用真實封包 + self.mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2]) + self.mock_socket2 = MockMavlinkSocket([HEARTBEAT_PACKET_3]) + + self.mavlink_obj1 = mavlink_object(self.mock_socket1) + self.mavlink_obj2 = mavlink_object(self.mock_socket2) + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.shutdown() + + def test_singleton_pattern(self): + """測試 async_io_manager 的單例模式""" + manager1 = async_io_manager() + manager2 = async_io_manager() + self.assertIs(manager1, manager2) + + def test_start_stop(self): + """測試 async_io_manager 的啟動和停止功能""" + # 啟動管理器 + self.manager.start() + self.assertTrue(self.manager.running) + self.assertIsNotNone(self.manager.thread) + + # 再次啟動應該沒有效果 + old_thread = self.manager.thread + self.manager.start() + self.assertIs(self.manager.thread, old_thread) + + # 停止管理器 + self.manager.shutdown() + self.assertFalse(self.manager.running) + + # 最多等待 5 秒讓線程結束 + start_time = time.time() + while self.manager.thread.is_alive() and time.time() - start_time < 5: + time.sleep(0.1) + self.assertFalse(self.manager.thread.is_alive()) + + def test_add_remove_objects(self): + """測試添加和移除 mavlink_object""" + # 啟動管理器 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj1)) + self.assertEqual(len(self.manager.managed_objects), 1) + self.assertEqual(self.mavlink_obj1.state, MavlinkObjectState.RUNNING) + + # 添加另一個對象 + self.assertTrue(self.manager.add_mavlink_object(self.mavlink_obj2)) + self.assertEqual(len(self.manager.managed_objects), 2) + + # 檢查受管理對象列表 + managed_objects = self.manager.get_managed_objects() + self.assertEqual(len(managed_objects), 2) + self.assertIn(0, managed_objects) + self.assertIn(1, managed_objects) + + # 移除對象 + self.assertTrue(self.manager.remove_mavlink_object(0)) + self.assertEqual(len(self.manager.managed_objects), 1) + + # 嘗試移除不存在的對象 + self.assertFalse(self.manager.remove_mavlink_object(999)) + + # 停止管理器 + self.manager.shutdown() + +class TestIntegration(unittest.TestCase): + """整合測試，測試多個 mavlink_object 之間的互動與資料流""" + + def setUp(self): + """在每個測試方法執行前準備環境""" + # 清空全局變數 + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + + # 清空 ring buffer + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建 async_io_manager 實例 + self.manager = async_io_manager() + + def tearDown(self): + """在每個測試方法執行後清理環境""" + if self.manager.running: + self.manager.shutdown() + + def test_send_message_with_manager(self): + """測試透過 async_io_manager 發送訊息的完整流程""" + # 創建一個新的 mavlink_object 實例 + mock_socket = MockMavlinkSocket() + mavlink_obj = mavlink_object(mock_socket) + + # 測試初始狀態 + self.assertEqual(len(mock_socket.written_data), 0) + + # 測試非運行狀態下發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 0) + + # 啟動 manager + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加對象到 manager + self.manager.add_mavlink_object(mavlink_obj) + time.sleep(0.1) # 等待對象啟動 + + # 確認對象狀態 + self.assertEqual(mavlink_obj.state, MavlinkObjectState.RUNNING) + + # 測試發送消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + time.sleep(0.2) # 等待消息處理 + + # 確認消息已發送 + self.assertEqual(len(mock_socket.written_data), 1) + self.assertEqual(mock_socket.written_data[0], HEARTBEAT_PACKET_1) + + # 測試連續發送多條消息 + self.assertTrue(mavlink_obj.send_message(HEARTBEAT_PACKET_2)) + time.sleep(0.2) # 等待消息處理 + + # 確認兩條消息都已發送 + self.assertEqual(len(mock_socket.written_data), 2) + self.assertEqual(mock_socket.written_data[1], HEARTBEAT_PACKET_2) + + # 停止 manager + self.manager.shutdown() + time.sleep(0.5) # 等待 manager 停止 + + # 測試對象已關閉後發送消息 + self.assertFalse(mavlink_obj.send_message(HEARTBEAT_PACKET_1)) + self.assertEqual(len(mock_socket.written_data), 2) # 消息數量未增加 + + def test_data_processing_and_forwarding(self): + """測試數據處理與轉發流程""" + # 創建用於轉發的 mavlink_objects + mock_socket1 = MockMavlinkSocket([HEARTBEAT_PACKET_1, HEARTBEAT_PACKET_2,]) + mock_socket3 = MockMavlinkSocket() + + mavlink_obj1 = mavlink_object(mock_socket1) + mavlink_obj3 = mavlink_object(mock_socket3) + + # 設置訊息類型 + mavlink_obj1.set_bridge_message_types([0]) # 只處理 HEARTBEAT + + # 設置轉發: obj1 -> obj3 + mavlink_obj1.add_target_socket(mavlink_obj3.socket_id) # socket1 轉發到 socket3 (socket_id=1) + + # 啟動管理器並添加對象 + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + """ + 這邊出現很奇怪的狀況 應該說 設計時沒有考量 但是實測會發現 + mavlink_obj3 是接收端 必需要被優先加入 manager 才能正確接收來自 mavlink_obj1 的轉發封包 + 若先把 mavlink_ojb1 加入 manger 則可能會導致前面幾個封包丟失 + """ + self.manager.add_mavlink_object(mavlink_obj3) + self.manager.add_mavlink_object(mavlink_obj1) + + # 等待處理完成 + time.sleep(0.5) + + # 檢查 Ring buffer 是否有正確的數據 + self.assertGreaterEqual(stream_bridge_ring.size(), 2) # 至少 2 個 HEARTBEAT + + # 檢查是否正確轉發 + self.assertGreaterEqual(len(mock_socket3.written_data), 2) # 至少 2 個 HEARTBEAT + + # 停止管理器 + self.manager.shutdown() + + def test_bidirectional_forwarding(self): + """測試雙向轉發""" + # 清空全局變數和 ring buffer + mavlink_object.mavlinkObjects = {} + mavlink_object.socket_num = 0 + stream_bridge_ring.clear() + return_packet_ring.clear() + + # 創建三個 mavlink 對象，模擬三個通道 + socket1 = MockMavlinkSocket() + socket2 = MockMavlinkSocket() + socket3 = MockMavlinkSocket() + + obj1 = mavlink_object(socket1) + obj2 = mavlink_object(socket2) + obj3 = mavlink_object(socket3) + + # 設置雙向轉發 + # obj1 <-> obj2 <-> obj3 + obj1.add_target_socket(1) # obj1 -> obj2 + obj2.add_target_socket(0) # obj2 -> obj1 + obj2.add_target_socket(2) # obj2 -> obj3 + obj3.add_target_socket(1) # obj3 -> obj2 + + # 啟動 async_io_manager + self.manager.start() + time.sleep(0.5) # 等待事件循環啟動 + + # 添加所有 mavlink_object + self.manager.add_mavlink_object(obj1) + self.manager.add_mavlink_object(obj2) + self.manager.add_mavlink_object(obj3) + + # 對三個對象添加數據 + socket1.test_packets.append(HEARTBEAT_PACKET_1) + socket2.test_packets.append(HEARTBEAT_PACKET_2) + socket3.test_packets.append(HEARTBEAT_PACKET_3) + + # 等待處理所有訊息 + time.sleep(1.0) + + # 檢查轉發結果 + # socket1 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket1.written_data), 1) + + # socket2 應該收到 socket1 和 socket3 的訊息 + self.assertGreaterEqual(len(socket2.written_data), 2) + + # socket3 應該收到 socket2 的訊息 + self.assertGreaterEqual(len(socket3.written_data), 1) + + # 檢查 ring buffer 的數據 + # 所有對象都啟用了橋接器，且預設的 bridge_msg_types = [0] + self.assertGreaterEqual(stream_bridge_ring.size(), 3) # 至少 3 個 HEARTBEAT + + # 停止管理器 + self.manager.shutdown() + + +if __name__ == "__main__": + # 可以指定要運行的測試 + # unittest.main(defaultTest="TestMavlinkObject.test_send_message_validation") + # unittest.main(defaultTest="TestAsyncIOManager.test_add_remove_objects") + unittest.main(defaultTest="TestIntegration.test_bidirectional_forwarding") + unittest.main() + diff --git a/src/fc_network_adapter/tests/test_ringBuffer.py b/src/fc_network_adapter/tests/test_ringBuffer.py new file mode 100644 index 0000000..287a057 --- /dev/null +++ b/src/fc_network_adapter/tests/test_ringBuffer.py @@ -0,0 +1,296 @@ +#!/usr/bin/env python +""" +測試 RingBuffer 類別的功能 +""" + +import unittest +import threading +import time +from concurrent.futures import ThreadPoolExecutor + +from ..fc_network_adapter.utils import RingBuffer + +class TestRingBuffer(unittest.TestCase): + """測試 RingBuffer 基本功能""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=8) + + def test_initialization(self): + """測試初始化""" + self.assertEqual(self.buffer.capacity, 8) + self.assertTrue(self.buffer.is_empty()) + self.assertFalse(self.buffer.is_full()) + self.assertEqual(self.buffer.size(), 0) + + def test_put_get_basic(self): + """測試基本的放入和取出""" + # 測試放入 + self.assertTrue(self.buffer.put("item1")) + self.assertTrue(self.buffer.put("item2")) + self.assertEqual(self.buffer.size(), 2) + self.assertFalse(self.buffer.is_empty()) + + # 測試取出 + item1 = self.buffer.get() + self.assertEqual(item1, "item1") + self.assertEqual(self.buffer.size(), 1) + + item2 = self.buffer.get() + self.assertEqual(item2, "item2") + self.assertTrue(self.buffer.is_empty()) + + # 空緩衝區取出應返回 None + self.assertIsNone(self.buffer.get()) + + def test_capacity_overflow(self): + """測試緩衝區容量限制""" + # 填滿緩衝區 (容量-1，因為需要預留一個位置) + for i in range(7): # 8-1=7 + self.assertTrue(self.buffer.put(f"item{i}")) + + self.assertTrue(self.buffer.is_full()) + + # 嘗試再放入一個應該失敗 + self.assertFalse(self.buffer.put("overflow")) + self.assertEqual(self.buffer.overflow_count.value, 1) + + def test_get_all(self): + """測試取出所有項目""" + items = ["a", "b", "c", "d"] + for item in items: + self.buffer.put(item) + + all_items = self.buffer.get_all() + self.assertEqual(all_items, items) + self.assertTrue(self.buffer.is_empty()) + + def test_clear(self): + """測試清空緩衝區""" + for i in range(5): + self.buffer.put(f"item{i}") + + self.buffer.clear() + self.assertTrue(self.buffer.is_empty()) + self.assertEqual(self.buffer.size(), 0) + + +class TestRingBufferThreadSafety(unittest.TestCase): + """測試 RingBuffer 的線程安全性""" + + def setUp(self): + """每個測試前的準備""" + self.buffer = RingBuffer(capacity=256) + self.results = [] + self.write_count = 1000 + self.read_count = 1000 + + def test_concurrent_producers_consumers(self): + """測試多生產者多消費者場景""" + self.results = [] + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + + def producer(producer_id, count): + """生產者函數""" + for i in range(count): + item = f"producer_{producer_id}_item_{i}" + while not self.buffer.put(item): + time.sleep(0.001) # 緩衝區滿時稍微等待 + + def consumer(consumer_id, count): + """消費者函數""" + items = [] + for _ in range(count): + item = None + while item is None: + item = self.buffer.get() + if item is None: + time.sleep(0.001) # 緩衝區空時稍微等待 + items.append(item) + self.results.extend(items) + + # 創建多個生產者和消費者 + with ThreadPoolExecutor(max_workers=8) as executor: + # 2 個生產者，每個寫入 500 個項目 + producer_futures = [ + executor.submit(producer, 0, 500), + executor.submit(producer, 1, 500) + ] + + # 2 個消費者，每個讀取 500 個項目 + consumer_futures = [ + executor.submit(consumer, 0, 500), + executor.submit(consumer, 1, 500) + ] + + # 等待所有任務完成 + for future in producer_futures + consumer_futures: + future.result() + + # 驗證結果 + self.assertEqual(len(self.results), 1000) + self.assertTrue(self.buffer.is_empty()) + + # 檢查統計數據 + stats = self.buffer.get_stats() + self.assertEqual(stats['total_writes'], 1000) + self.assertGreater(stats['total_reads'], 1000) # 包含失敗的讀取嘗試 + self.assertGreater(stats['write_threads'], 1) + self.assertGreater(stats['read_threads'], 1) + + def test_high_throughput(self): + """測試高吞吐量場景""" + items_per_thread = 10000 + num_threads = 4 + + def writer(): + for i in range(items_per_thread): + while not self.buffer.put(i): + pass # 忙等待 + + def reader(): + items = [] + for _ in range(items_per_thread): + item = None + while item is None: + item = self.buffer.get() + items.append(item) + self.results.extend(items) + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=num_threads * 2) as executor: + # 啟動寫入線程 + writer_futures = [executor.submit(writer) for _ in range(num_threads)] + + # 啟動讀取線程 + reader_futures = [executor.submit(reader) for _ in range(num_threads)] + + # 等待完成 + for future in writer_futures + reader_futures: + future.result() + + end_time = time.time() + + # 驗證結果 + total_items = items_per_thread * num_threads + self.assertEqual(len(self.results), total_items) + + # 性能統計 + duration = end_time - start_time + throughput = total_items / duration + + print(f"\nHigh Throughput Test Results:") + print(f"Total items: {total_items}") + print(f"Duration: {duration:.2f}s") + print(f"Throughput: {throughput:.0f} items/sec") + + # 顯示詳細統計 + self.buffer.print_stats() + + +class TestRingBufferStatistics(unittest.TestCase): + """測試 RingBuffer 的統計功能""" + + def test_statistics_tracking(self): + """測試統計數據追蹤""" + buffer = RingBuffer(capacity=16) + + # 寫入一些數據 + for i in range(10): + buffer.put(f"item{i}") + + # 讀取一些數據 + for _ in range(5): + buffer.get() + + stats = buffer.get_stats() + + # 驗證基本統計 + self.assertEqual(stats['total_writes'], 10) + self.assertEqual(stats['total_reads'], 5) + self.assertEqual(stats['current_size'], 5) + self.assertEqual(stats['write_threads'], 1) + self.assertEqual(stats['read_threads'], 1) + + def test_reset_statistics(self): + """測試重置統計數據""" + buffer = RingBuffer(capacity=16) + + # 產生一些活動 + for i in range(5): + buffer.put(f"item{i}") + for _ in range(3): + buffer.get() + + # 重置統計 + buffer.reset_stats() + + stats = buffer.get_stats() + self.assertEqual(stats['total_writes'], 0) + self.assertEqual(stats['total_reads'], 0) + self.assertEqual(stats['concurrent_writes'], 0) + self.assertEqual(stats['concurrent_reads'], 0) + self.assertEqual(stats['overflow_count'], 0) + + +def benchmark_ringbuffer(): + """RingBuffer 性能基準測試""" + print("\n=== RingBuffer Performance Benchmark ===") + + buffer = RingBuffer(capacity=1024) + num_operations = 100000 + + # 單線程性能測試 + start_time = time.time() + for i in range(num_operations): + buffer.put(i) + for _ in range(num_operations): + buffer.get() + end_time = time.time() + + single_thread_time = end_time - start_time + throughput = (num_operations * 2) / single_thread_time + + print(f"Single Thread: {throughput:.0f} ops/sec") + + # 多線程性能測試 + buffer = RingBuffer(capacity=1024) + + def producer(): + for i in range(num_operations // 2): + while not buffer.put(i): + pass + + def consumer(): + for _ in range(num_operations // 2): + while buffer.get() is None: + pass + + start_time = time.time() + + with ThreadPoolExecutor(max_workers=2) as executor: + future1 = executor.submit(producer) + future2 = executor.submit(consumer) + future1.result() + future2.result() + + end_time = time.time() + + multi_thread_time = end_time - start_time + throughput = num_operations / multi_thread_time + + print(f"Multi Thread: {throughput:.0f} ops/sec") + print(f"Speedup: {single_thread_time/multi_thread_time:.2f}x") + + buffer.print_stats() + + +if __name__ == "__main__": + # 運行單元測試 + unittest.main(argv=[''], exit=False, verbosity=2) + + # 運行性能基準測試 + benchmark_ringbuffer() \ No newline at end of file diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py new file mode 100644 index 0000000..6fd1914 --- /dev/null +++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py @@ -0,0 +1,507 @@ +""" +VehicleStatusPublisher 測試程式 + +測試從 vehicle_registry 讀取資料並發布到 ROS2 topics +""" + +import time +import json +import threading + +# ROS2 imports +import rclpy +from rclpy.node import Node + +# 標準 ROS2 消息類型 +import std_msgs.msg +import sensor_msgs.msg +import geometry_msgs.msg +import mavros_msgs.msg + +# 專案 imports +from ..fc_network_adapter.mavlinkROS2Nodes import ( + VehicleStatusPublisher, + fc_ros_manager, + ros2_manager +) +from ..fc_network_adapter.mavlinkVehicleView import ( + vehicle_registry, + ConnectionType, + ComponentType, +) + + +class TestSubscriber(Node): + """測試用的訂閱者節點 - 接收並記錄收到的消息""" + + def __init__(self, sysid: int = 1): + super().__init__(f'test_subscriber_sys{sysid}') + + self.sysid = sysid + self.received_messages = { + 'position': [], + 'attitude': [], + 'velocity': [], + 'battery': [], + 'vfr_hud': [], + 'mode': [], + 'summary': [], + } + + # 建立所有訂閱者 + self._create_subscriptions() + + print(f"[TestSubscriber] 已建立，監聽 sys{sysid} 的所有 topics") + + def _create_subscriptions(self): + """建立所有 topic 的訂閱者""" + + base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}' + + # Position + self.create_subscription( + sensor_msgs.msg.NavSatFix, + f'{base_topic}/position', + lambda msg: self._on_message('position', msg), + 10 + ) + + # Attitude + self.create_subscription( + sensor_msgs.msg.Imu, + f'{base_topic}/attitude', + lambda msg: self._on_message('attitude', msg), + 10 + ) + + # Velocity + self.create_subscription( + geometry_msgs.msg.TwistStamped, + f'{base_topic}/velocity', + lambda msg: self._on_message('velocity', msg), + 10 + ) + + # Battery + self.create_subscription( + sensor_msgs.msg.BatteryState, + f'{base_topic}/battery', + lambda msg: self._on_message('battery', msg), + 10 + ) + + # VFR HUD + self.create_subscription( + mavros_msgs.msg.VfrHud, + f'{base_topic}/vfr_hud', + lambda msg: self._on_message('vfr_hud', msg), + 10 + ) + + # Mode + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/mode', + lambda msg: self._on_message('mode', msg), + 10 + ) + + # Summary + self.create_subscription( + std_msgs.msg.String, + f'{base_topic}/summary', + lambda msg: self._on_message('summary', msg), + 10 + ) + + def _on_message(self, topic_name: str, msg): + """通用消息接收回調""" + self.received_messages[topic_name].append(msg) + print(f"[TestSubscriber] 收到 {topic_name}: {self._format_msg(topic_name, msg)}") + + def _format_msg(self, topic_name: str, msg) -> str: + """格式化消息以便顯示""" + if topic_name == 'position': + return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m" + elif topic_name == 'attitude': + return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})" + elif topic_name == 'velocity': + return f"linear=({msg.twist.linear.x:.2f}, {msg.twist.linear.y:.2f}, {msg.twist.linear.z:.2f})" + elif topic_name == 'battery': + return f"voltage={msg.voltage:.2f}V, percent={msg.percentage*100:.1f}%" + elif topic_name == 'vfr_hud': + return f"airspeed={msg.airspeed:.2f}, groundspeed={msg.groundspeed:.2f}, heading={msg.heading}" + elif topic_name == 'mode': + return f"mode={msg.data}" + elif topic_name == 'summary': + try: + data = json.loads(msg.data) + return f"sysid={data['sysid']}, socket_id={data['socket_id']}, mode={data['mode_name']}" + except: + return msg.data + return str(msg) + + def get_message_count(self, topic_name: str) -> int: + """獲取收到的消息數量""" + return len(self.received_messages[topic_name]) + + def clear_messages(self): + """清空已收到的消息""" + for key in self.received_messages: + self.received_messages[key].clear() + + +def setup_test_vehicle(sysid: int = 1, socket_id: int = 10): + """ + 建立測試用的載具數據 + + Args: + sysid: 系統 ID + socket_id: Socket ID + """ + print(f"\n=== 建立測試載具 (sysid={sysid}, socket_id={socket_id}) ===") + + # 註冊載具 + vehicle = vehicle_registry.register(sysid) + vehicle.kind = "Copter" + vehicle.vehicle_type = 2 # MAV_TYPE_QUADROTOR + vehicle.connected_via = ConnectionType.UDP + vehicle.custom_meta['socket_id'] = socket_id + + # 新增 autopilot 組件 (component_id=1) + autopilot = vehicle.add_component(1, ComponentType.AUTOPILOT) + autopilot.mav_type = 2 # MAV_TYPE_QUADROTOR + autopilot.mav_autopilot = 3 # MAV_AUTOPILOT_ARDUPILOTMEGA + + # 填充狀態資料 + status = autopilot.status + + # 位置 + status.position.latitude = 25.0330 + status.position.longitude = 121.5654 + status.position.altitude = 100.5 + status.position.relative_altitude = 50.0 + status.position.timestamp = time.time() + + # 姿態 + status.attitude.roll = 0.1 + status.attitude.pitch = -0.05 + status.attitude.yaw = 1.57 + status.attitude.rollspeed = 0.01 + status.attitude.pitchspeed = 0.02 + status.attitude.yawspeed = 0.03 + status.attitude.timestamp = time.time() + + # 飛行模式 + status.mode.base_mode = 89 + status.mode.custom_mode = 4 + status.mode.mode_name = "GUIDED" + status.mode.timestamp = time.time() + + # 電池 + status.battery.voltage = 12.6 + status.battery.current = 15.3 + status.battery.remaining = 75 + status.battery.temperature = 35.2 + status.battery.timestamp = time.time() + + # GPS + status.gps.fix_type = 3 # 3D fix + status.gps.satellites_visible = 12 + status.gps.eph = 100 + status.gps.epv = 150 + status.gps.timestamp = time.time() + + # VFR + status.vfr.airspeed = 5.5 + status.vfr.groundspeed = 6.0 + status.vfr.heading = 90 + status.vfr.throttle = 65 + status.vfr.climb = 1.2 + status.vfr.timestamp = time.time() + + # 系統狀態 + status.armed = True + status.system_status = 4 # MAV_STATE_ACTIVE + + # 更新封包統計 + autopilot.update_packet_stats(seq=10, msg_type=33, timestamp=time.time()) + + print(f"✓ 載具 {sysid} 已建立並填充測試數據") + return vehicle + + +def test_basic_publishing(): + """測試基本的發布功能""" + print("\n" + "="*60) + print("測試 1: 基本發布功能") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化 ROS2 管理器 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 開始發布，等待 5 秒 ---") + + # 運行 5 秒，持續 spin + start_time = time.time() + while time.time() - start_time < 5.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查收到的消息 + print("\n--- 消息統計 ---") + total_messages = 0 + for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + total_messages += count + print(f" {topic:15s}: {count:3d} 條消息") + + print(f"\n總計收到: {total_messages} 條消息") + + # 驗證 + if total_messages > 0: + print("\n✓ 測試通過：成功接收到消息") + else: + print("\n✗ 測試失敗：沒有接收到任何消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_frequency_control(): + """測試頻率控制功能""" + print("\n" + "="*60) + print("測試 2: 頻率控制") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立測試載具 + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + # 初始化（如果還沒初始化） + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 修改頻率設定 + publisher_node = ros2_manager.status_publisher + publisher_node.rate_controller.topic_intervals = { + 'position': 1.5, + 'attitude': 1.0, + 'velocity': 1.0, + 'battery': 1.0, + 'vfr_hud': 0.5, + 'mode': 0.0, + 'summary': 0.0, + } + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試頻率控制，運行 3 秒 ---") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + # 檢查頻率 + print("\n--- 頻率分析 ---") + print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布") + + print("2Hz Topics (預期 ~6 條):") + for topic in ['position', 'attitude', 'velocity', 'vfr_hud']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + for topic in ['battery', 'mode', 'summary']: + count = test_sub.get_message_count(topic) + print(f" {topic:15s}: {count:3d} 條") + + print("\n✓ 頻率控制測試完成") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def test_multi_vehicle(): + """測試多載具發布""" + print("\n" + "="*60) + print("測試 3: 多載具發布") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 建立 3 個測試載具 + v1 = setup_test_vehicle(sysid=1, socket_id=10) + v2 = setup_test_vehicle(sysid=2, socket_id=11) + v3 = setup_test_vehicle(sysid=3, socket_id=12) + + # 修改各載具的位置以便區分 + v2.components[1].status.position.latitude = 26.0 + v3.components[1].status.position.latitude = 27.0 + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立 3 個測試訂閱者 + test_sub1 = TestSubscriber(sysid=1) + test_sub2 = TestSubscriber(sysid=2) + test_sub3 = TestSubscriber(sysid=3) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 測試多載具，運行 3 秒 ---") + + # 運行 3 秒 + start_time = time.time() + while time.time() - start_time < 3.0: + rclpy.spin_once(test_sub1, timeout_sec=0.05) + rclpy.spin_once(test_sub2, timeout_sec=0.05) + rclpy.spin_once(test_sub3, timeout_sec=0.05) + time.sleep(0.1) + + # 檢查每個載具的消息 + print("\n--- 各載具消息統計 ---") + for sysid, test_sub in [(1, test_sub1), (2, test_sub2), (3, test_sub3)]: + total = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"載具 {sysid}: {total:3d} 條消息") + + # 檢查 summary 中的 socket_id + if test_sub.get_message_count('summary') > 0: + last_summary = test_sub.received_messages['summary'][-1] + data = json.loads(last_summary.data) + print(f" └─ socket_id={data['socket_id']}, lat={data['latitude']:.1f}") + + print("\n✓ 多載具測試完成") + + # 停止 + ros2_manager.stop() + test_sub1.destroy_node() + test_sub2.destroy_node() + test_sub3.destroy_node() + + +def test_dynamic_vehicle(): + """測試動態新增/移除載具""" + print("\n" + "="*60) + print("測試 4: 動態載具管理") + print("="*60) + + # 清空 registry + vehicle_registry.clear() + + # 初始化 + if not ros2_manager.initialized: + ros2_manager.initialize() + + # 建立測試訂閱者 + test_sub = TestSubscriber(sysid=1) + + # 啟動 publisher + ros2_manager.start() + + print("\n--- 階段 1: 無載具，運行 1 秒 ---") + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_before = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_before} 條") + + # 新增載具 + print("\n--- 階段 2: 新增載具，運行 2 秒 ---") + vehicle = setup_test_vehicle(sysid=1, socket_id=10) + + start_time = time.time() + while time.time() - start_time < 2.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_after = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_after - count_before} 條") + + # 移除載具 + print("\n--- 階段 3: 移除載具，運行 1 秒 ---") + vehicle_registry.unregister(1) + + start_time = time.time() + while time.time() - start_time < 1.0: + rclpy.spin_once(test_sub, timeout_sec=0.1) + time.sleep(0.1) + + count_final = sum(test_sub.get_message_count(t) for t in test_sub.received_messages.keys()) + print(f"收到消息: {count_final - count_after} 條（應該為 0）") + + if count_final - count_after == 0: + print("\n✓ 動態載具管理測試通過") + else: + print("\n✗ 移除載具後仍收到消息") + + # 停止 + ros2_manager.stop() + test_sub.destroy_node() + + +def main(): + """主測試函數""" + print("\n" + "="*60) + print("VehicleStatusPublisher 測試程式") + print("="*60) + + try: + # 執行各項測試 + test_basic_publishing() + # time.sleep(1) + + # test_frequency_control() + # time.sleep(1) + + # test_multi_vehicle() + # time.sleep(1) + + # test_dynamic_vehicle() + + print("\n" + "="*60) + print("所有測試完成！") + print("="*60) + + except KeyboardInterrupt: + print("\n\n測試被中斷") + except Exception as e: + print(f"\n\n測試出錯: {e}") + import traceback + traceback.print_exc() + finally: + # 清理 + if ros2_manager.initialized: + ros2_manager.shutdown() + vehicle_registry.clear() + print("\n清理完成") + + +if __name__ == '__main__': + main() From 44d63979b52e401485c5334b57e07d917efb0231 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 2 Feb 2026 03:00:49 +0800 Subject: [PATCH 099/146] ROS2 sub --- src/GUI/communication.py | 97 +++++++++++++++++++++------------------- 1 file changed, 50 insertions(+), 47 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 45c960c..74add17 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -11,7 +11,7 @@ import socket from pymavlink import mavutil from geometry_msgs.msg import Point, Vector3 from sensor_msgs.msg import BatteryState, NavSatFix, Imu -from std_msgs.msg import Float64 +from std_msgs.msg import Float64, String from mavros_msgs.msg import State, VfrHud from mavros_msgs.srv import CommandBool, CommandTOL @@ -371,6 +371,7 @@ class WebSocketMavlinkReceiver(threading.Thread): self.running = False class DroneMonitor(Node): + # Subscribe to drone ROS2 topics def __init__(self): super().__init__('drone_monitor') self.signals = DroneSignals() @@ -403,12 +404,15 @@ class DroneMonitor(Node): def scan_topics(self): topics = self.get_topic_names_and_types() - drone_pattern = re.compile(r'/MavLinkBus/(s\d+_\d+)/(\w+)') + drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)') found_drones = set() for topic_name, _ in topics: if match := drone_pattern.match(topic_name): - drone_id, topic_type = match.groups() + sys_id, topic_type = match.groups() + # 將 sys11 轉換為 s0_11 格式以保持兼容性 + drone_num = sys_id.replace('sys', '') + drone_id = f's0_{drone_num}' found_drones.add(drone_id) with self.lock: self.drone_topics.setdefault(drone_id, set()).add(topic_type) @@ -418,9 +422,11 @@ class DroneMonitor(Node): self.setup_drone(drone_id) def setup_drone(self, drone_id): - base_topic = f'/MavLinkBus/{drone_id}' + # 從 s0_11 轉換回 sys11 格式 + sys_id = drone_id.replace('s0_', 'sys') + base_topic = f'/fc_network/vehicle/{sys_id}' - # Add service clients + # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務) self.arm_clients[drone_id] = self.create_client( CommandBool, f'{base_topic}/cmd/arming' @@ -438,58 +444,22 @@ class DroneMonitor(Node): ) 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 ), - 'global': self.create_subscription( + 'position': self.create_subscription( NavSatFix, - f'{base_topic}/global_position/global', + f'{base_topic}/position', 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 - ), - 'loss_rate': self.create_subscription( - Float64, - f'{base_topic}/packet_loss_rate', - lambda msg, did=drone_id: self.loss_rate_callback(did, msg), - 10 - ), - 'state': self.create_subscription( - State, - f'{base_topic}/state', - lambda msg, did=drone_id: self.state_callback(did, msg), - 10 - ), - 'ping': self.create_subscription( - Float64, - f'{base_topic}/ping', - lambda msg, did=drone_id: self.ping_callback(did, msg), + 'summary': self.create_subscription( + String, + f'{base_topic}/summary', + lambda msg, did=drone_id: self.summary_callback(did, msg), 10 ), 'vfr_hud': self.create_subscription( @@ -596,6 +566,39 @@ class DroneMonitor(Node): 'armed': msg.armed } + def summary_callback(self, drone_id, msg): + """處理 summary topic (JSON 格式)""" + try: + data = json.loads(msg.data) + mode = data.get('mode_name', 'UNKNOWN') + if mode in self.filtered_modes: + return + + # 根據 socket_id 更新 drone_id + socket_id = data.get('socket_id') + sysid = data.get('sysid') + if socket_id is not None and sysid is not None: + # 使用 socket_id 作為前綴 + actual_drone_id = f's{socket_id}_{sysid}' + else: + actual_drone_id = drone_id + + self.latest_data[(actual_drone_id, 'state')] = { + 'mode': mode, + 'armed': data.get('armed', False), + 'socket_id': socket_id, + 'sysid': sysid, + 'vehicle_type': data.get('vehicle_type'), + 'autopilot': data.get('autopilot'), + 'gps_fix': data.get('gps_fix'), + 'gps_fix_type': data.get('gps_fix'), + 'connected': data.get('connected') + } + except json.JSONDecodeError as e: + print(f"Error parsing summary JSON for {drone_id}: {e}") + except Exception as e: + print(f"Error in summary_callback for {drone_id}: {e}") + def gps_callback(self, drone_id, msg): self.latest_data[(drone_id, 'gps')] = { 'lat': msg.latitude, From 8fdbbbc5dcf4778330916f400bf65a9e27c2a731 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 4 Feb 2026 03:46:31 +0800 Subject: [PATCH 100/146] update gui --- src/GUI/comm_panel.py | 294 +++++++++++++++++++++++++++++++++++++- src/GUI/communication.py | 41 +++++- src/GUI/gui.py | 136 +++++++++++++++--- src/GUI/map_layout.py | 18 ++- src/GUI/overview_table.py | 33 ++++- 5 files changed, 491 insertions(+), 31 deletions(-) diff --git a/src/GUI/comm_panel.py b/src/GUI/comm_panel.py index 82b539e..bd12794 100644 --- a/src/GUI/comm_panel.py +++ b/src/GUI/comm_panel.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, - QPushButton, QLineEdit) + QPushButton, QLineEdit, QComboBox) from PyQt6.QtCore import pyqtSignal +import glob +import os class CommPanel(QWidget): """通讯设置面板类""" @@ -13,12 +15,16 @@ class CommPanel(QWidget): ws_connection_added = pyqtSignal(str) # url ws_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label ws_connection_removed = pyqtSignal(dict, QWidget) # conn, panel + serial_connection_added = pyqtSignal(str, int) # port, baudrate + serial_connection_toggled = pyqtSignal(dict, QPushButton, QLabel) # conn, btn, status_label + serial_connection_removed = pyqtSignal(dict, QWidget) # conn, panel status_message = pyqtSignal(str, int) # message, timeout def __init__(self, parent=None): super().__init__(parent) self.udp_connections = [] self.ws_connections = [] + self.serial_connections = [] self._init_ui() def _init_ui(self): @@ -105,6 +111,118 @@ class CommPanel(QWidget): separator.setFixedHeight(20) layout.addWidget(separator) + # ========== Serial 區域 ========== + serial_title = QLabel("Serial") + serial_title.setStyleSheet(""" + color: #DDD; + font-size: 14px; + font-weight: bold; + padding: 5px; + background-color: #333; + border-radius: 4px; + """) + layout.addWidget(serial_title) + + # Serial 連接列表容器 + self.serial_list_widget = QWidget() + self.serial_list_layout = QVBoxLayout(self.serial_list_widget) + self.serial_list_layout.setContentsMargins(0, 0, 0, 0) + self.serial_list_layout.setSpacing(5) + layout.addWidget(self.serial_list_widget) + + # Serial 添加新連接區域 + add_serial_widget = QWidget() + add_serial_layout = QHBoxLayout(add_serial_widget) + add_serial_layout.setContentsMargins(0, 0, 0, 0) + + self.serial_port_combo = QComboBox() + self.serial_port_combo.setStyleSheet(""" + QComboBox { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + QComboBox::drop-down { + border: none; + } + QComboBox::down-arrow { + image: none; + border-left: 5px solid transparent; + border-right: 5px solid transparent; + border-top: 5px solid #DDD; + } + """) + self._refresh_serial_ports() + + refresh_ports_btn = QPushButton("↻") + refresh_ports_btn.setFixedWidth(35) + refresh_ports_btn.clicked.connect(self._refresh_serial_ports) + refresh_ports_btn.setToolTip("重新掃描串口") + refresh_ports_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 6px; + border-radius: 4px; + font-size: 16px; + } + QPushButton:hover { background-color: #555; } + """) + + self.serial_baudrate_combo = QComboBox() + self.serial_baudrate_combo.addItems(['9600', '19200', '38400', '57600', '115200']) + self.serial_baudrate_combo.setCurrentText('57600') + self.serial_baudrate_combo.setFixedWidth(100) + self.serial_baudrate_combo.setStyleSheet(""" + QComboBox { + background-color: #333; + color: #DDD; + border: 1px solid #555; + border-radius: 4px; + padding: 5px; + } + QComboBox::drop-down { + border: none; + } + QComboBox::down-arrow { + image: none; + border-left: 5px solid transparent; + border-right: 5px solid transparent; + border-top: 5px solid #DDD; + } + """) + + add_serial_btn = QPushButton("添加") + add_serial_btn.clicked.connect(self._handle_add_serial) + add_serial_btn.setStyleSheet(""" + QPushButton { + background-color: #4CAF50; + color: white; + border: none; + padding: 6px 12px; + border-radius: 4px; + min-width: 30px; + } + QPushButton:hover { background-color: #45a049; } + """) + + add_serial_layout.addWidget(QLabel("Port:", styleSheet="color: #DDD;")) + add_serial_layout.addWidget(self.serial_port_combo) + add_serial_layout.addWidget(refresh_ports_btn) + add_serial_layout.addWidget(QLabel("Baud:", styleSheet="color: #DDD;")) + add_serial_layout.addWidget(self.serial_baudrate_combo) + add_serial_layout.addWidget(add_serial_btn) + + layout.addWidget(add_serial_widget) + + # 分隔線 + separator = QWidget() + separator.setFixedHeight(20) + layout.addWidget(separator) + # ========== WebSocket 區域 ========== ws_title = QLabel("WebSocket") ws_title.setStyleSheet(""" @@ -229,6 +347,94 @@ class CommPanel(QWidget): # 清空輸入框 self.ws_url_input.clear() + def _refresh_serial_ports(self): + """重新掃描可用的串口""" + self.serial_port_combo.clear() + + # 掃描 Linux 下的串口設備 + ports = [] + + # 掃描 USB 串口 + usb_ports = glob.glob('/dev/ttyUSB*') + ports.extend(usb_ports) + + # 掃描 ACM 串口 + acm_ports = glob.glob('/dev/ttyACM*') + ports.extend(acm_ports) + + # 排序 + ports.sort() + + if ports: + self.serial_port_combo.addItems(ports) + else: + self.serial_port_combo.addItem("沒有找到串口") + self.serial_port_combo.setEnabled(False) + return + + self.serial_port_combo.setEnabled(True) + + def _handle_add_serial(self): + """處理添加 Serial 連接""" + port = self.serial_port_combo.currentText() + baudrate_text = self.serial_baudrate_combo.currentText() + + if not port or port == "沒有找到串口": + self.status_message.emit("請選擇有效的串口", 3000) + return + + try: + baudrate = int(baudrate_text) + except ValueError: + self.status_message.emit("波特率格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.serial_connections: + if conn['port'] == port: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.serial_connection_added.emit(port, baudrate) + + def _handle_add_ws(self): + """處理添加 WebSocket 連接""" + input_url = self.ws_url_input.text().strip() + + if not input_url: + self.status_message.emit("請輸入 WebSocket URL", 3000) + return + + # 自動添加 ws:// 前綴 + if not input_url.startswith('ws://') and not input_url.startswith('wss://'): + url = f'ws://{input_url}' + else: + url = input_url + + # 基本 URL 格式驗證 + try: + if '://' in url: + parts = url.split('://', 1) + if len(parts) == 2 and ':' not in parts[1]: + self.status_message.emit("URL 格式錯誤，需要包含端口號 (例如: 127.0.0.1:8756)", 3000) + return + except: + self.status_message.emit("URL 格式錯誤", 3000) + return + + # 檢查是否已存在相同連接 + for conn in self.ws_connections: + if conn['url'] == url: + self.status_message.emit("連接已存在", 3000) + return + + # 發送信號通知主窗口 + self.ws_connection_added.emit(url) + + # 清空輸入框 + self.ws_url_input.clear() + def create_udp_connection_panel(self, conn): """創建 UDP 連接面板""" panel = QWidget() @@ -395,4 +601,88 @@ class CommPanel(QWidget): def remove_ws_connection_from_list(self, conn): """從列表中移除 WebSocket 連接""" if conn in self.ws_connections: - self.ws_connections.remove(conn) \ No newline at end of file + self.ws_connections.remove(conn) + + def create_serial_connection_panel(self, conn): + """創建 Serial 連接面板""" + panel = QWidget() + panel.setStyleSheet(""" + QWidget { + background-color: #2A2A2A; + border-radius: 6px; + padding: 8px; + border: 1px solid #444; + } + """) + + layout = QHBoxLayout(panel) + layout.setContentsMargins(8, 8, 8, 8) + + # 連接資訊 + info_label = QLabel(f"{conn['name']} - {conn['port']} @ {conn['baudrate']}") + info_label.setStyleSheet("color: #DDD; font-size: 12px;") + + # 狀態指示器 + status_label = QLabel("●") + if conn.get('enabled', False): + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + else: + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + + # 控制按鈕 + toggle_btn = QPushButton("停止" if conn.get('enabled', False) else "啟動") + toggle_btn.setFixedWidth(60) + toggle_btn.clicked.connect(lambda: self.serial_connection_toggled.emit(conn, toggle_btn, status_label)) + toggle_btn.setStyleSheet(""" + QPushButton { + background-color: #444; + color: #DDD; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #555; } + """) + + remove_btn = QPushButton("移除") + remove_btn.setFixedWidth(60) + remove_btn.clicked.connect(lambda: self.serial_connection_removed.emit(conn, panel)) + remove_btn.setStyleSheet(""" + QPushButton { + background-color: #d32f2f; + color: white; + border: none; + padding: 4px 8px; + border-radius: 3px; + font-size: 11px; + } + QPushButton:hover { background-color: #b71c1c; } + """) + + layout.addWidget(status_label) + layout.addWidget(info_label) + layout.addStretch() + layout.addWidget(toggle_btn) + layout.addWidget(remove_btn) + + # 儲存引用 + panel.connection = conn + panel.toggle_btn = toggle_btn + panel.status_label = status_label + + return panel + + def add_serial_panel(self, conn): + """添加 Serial 連接面板到列表""" + panel = self.create_serial_connection_panel(conn) + self.serial_list_layout.addWidget(panel) + self.serial_connections.append(conn) + return panel + + def remove_serial_connection_from_list(self, conn): + """從列表中移除 Serial 連接""" + if conn in self.serial_connections: + self.serial_connections.remove(conn) \ No newline at end of file diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 74add17..c95e74e 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -397,6 +397,12 @@ class DroneMonitor(Node): # ================================================================================ self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} # ================================================================================ + + # ================================================================================ + # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取) + # ================================================================================ + self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'} + # ================================================================================ self.serial_receivers = [] # 主题检测定时器 @@ -553,7 +559,13 @@ class DroneMonitor(Node): } def battery_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'battery')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'battery')] = { 'voltage': msg.voltage } @@ -580,6 +592,15 @@ class DroneMonitor(Node): if socket_id is not None and sysid is not None: # 使用 socket_id 作為前綴 actual_drone_id = f's{socket_id}_{sysid}' + + # ================================================================================ + # 【關鍵】保存 sys_id 到 actual_drone_id 的映射 + # ================================================================================ + sys_key = f'sys{sysid}' + self.sys_to_actual_id[sys_key] = actual_drone_id + # 也保存原始的 s0_ 格式到實際 ID 的映射 + self.sys_to_actual_id[drone_id] = actual_drone_id + # ================================================================================ else: actual_drone_id = drone_id @@ -600,7 +621,13 @@ class DroneMonitor(Node): print(f"Error in summary_callback for {drone_id}: {e}") def gps_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'gps')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'gps')] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude @@ -609,7 +636,7 @@ class DroneMonitor(Node): # ================================================================================ # 【新增】儲存 GPS 資料到 drone_gps 字典 # ================================================================================ - self.drone_gps[drone_id] = { + self.drone_gps[actual_drone_id] = { 'lat': msg.latitude, 'lon': msg.longitude, 'alt': msg.altitude @@ -636,7 +663,13 @@ class DroneMonitor(Node): } def hud_callback(self, drone_id, msg): - self.latest_data[(drone_id, 'hud')] = { + # 使用映射獲取實際的 drone_id + actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + # 如果還沒有從 summary 獲取到映射，則不處理 + if actual_drone_id is None: + return + + self.latest_data[(actual_drone_id, 'hud')] = { 'airspeed': msg.airspeed, 'groundspeed': msg.groundspeed, 'heading': msg.heading, diff --git a/src/GUI/gui.py b/src/GUI/gui.py index cefc9c9..1e0c5ab 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -44,25 +44,6 @@ class ControlStationUI(QMainWindow): # 初始化UI self.drones = {} self.socket_groups = {} - self.info_types = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", - "Roll", "Pitch", "Yaw", "丟包", "延遲"] - self.info_type_map = { - "mode": 0, - "armed": 1, - "battery": 2, - "longitude": 3, - "latitude": 4, - "altitude": 5, - "local": 6, - "velocity": 7, - "groundspeed": 8, - "heading": 9, - "roll": 10, - "pitch": 11, - "yaw": 12, - "loss_rate": 13, - "ping": 14 - } self.socket_colors = { '0': '#00BFFF', # 天藍色 (DeepSkyBlue) @@ -86,7 +67,8 @@ class ControlStationUI(QMainWindow): self.udp_receivers = [] self.udp_connections = [] self.ws_connections = [] - self.monitor.start_serial_connection('/dev/ttyUSB0', 57600) + self.serial_receivers = [] + self.serial_connections = [] # ================================================================================ # 【新增】初始化任務規劃器 @@ -122,17 +104,20 @@ class ControlStationUI(QMainWindow): self.left_tab.addTab(scroll, "無人載具") # — 分頁 2：Overview Table - self.overview_table = OverviewTable(self.info_types, self.info_type_map) + self.overview_table = OverviewTable() self.left_tab.addTab(self.overview_table, "總覽") # — 分頁 3：通訊設定 self.comm_panel = CommPanel() self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) + self.comm_panel.serial_connection_added.connect(self.handle_serial_connection_added) self.comm_panel.udp_connection_toggled.connect(self.toggle_udp_connection) self.comm_panel.ws_connection_toggled.connect(self.toggle_ws_connection) + self.comm_panel.serial_connection_toggled.connect(self.toggle_serial_connection) self.comm_panel.udp_connection_removed.connect(self.remove_udp_connection) self.comm_panel.ws_connection_removed.connect(self.remove_ws_connection) + self.comm_panel.serial_connection_removed.connect(self.remove_serial_connection) self.comm_panel.status_message.connect(lambda msg, timeout: self.statusBar().showMessage(msg, timeout)) self.left_tab.addTab(self.comm_panel, "通訊") @@ -269,6 +254,7 @@ class ControlStationUI(QMainWindow): # 添加地圖 right_layout.addWidget(self.drone_map.get_widget()) self.drone_map.get_gps_signal().connect(self.handle_map_click) + self.drone_map.get_drone_clicked_signal().connect(self.handle_drone_clicked) # Add widgets to splitter @@ -427,6 +413,69 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage(f"已移除 UDP 连接: {conn['ip']}:{conn['port']}", 3000) + def handle_serial_connection_added(self, port, baudrate): + """處理添加 Serial 連接""" + conn = { + 'name': f'Serial', + 'port': port, + 'baudrate': baudrate, + 'enabled': False, + 'receiver': None + } + + # 添加到列表 + self.serial_connections.append(conn) + + # 在 UI 中顯示 + panel = self.comm_panel.add_serial_panel(conn) + + self.statusBar().showMessage(f"已添加 Serial 连接: {port} @ {baudrate}", 3000) + + def toggle_serial_connection(self, conn, btn, status_label): + """切換 Serial 連接狀態""" + if conn.get('enabled', False): + # 停止連接 + if conn.get('receiver'): + conn['receiver'].stop() + conn['receiver'] = None + + conn['enabled'] = False + btn.setText("啟動") + status_label.setStyleSheet("color: #888; font-size: 16px;") + status_label.setToolTip("已停止") + self.statusBar().showMessage(f"已停止 Serial 連接: {conn['port']}", 3000) + else: + # 啟動連接 + try: + receiver = self.monitor.start_serial_connection(conn['port'], conn['baudrate']) + conn['receiver'] = receiver + conn['enabled'] = True + btn.setText("停止") + status_label.setStyleSheet("color: #4CAF50; font-size: 16px;") + status_label.setToolTip("運行中") + self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000) + except Exception as e: + self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000) + + def remove_serial_connection(self, conn, panel): + """移除 Serial 連接""" + # 停止連接 + if conn.get('enabled', False) and conn.get('receiver'): + conn['receiver'].stop() + + # 从列表移除 + if conn in self.serial_connections: + self.serial_connections.remove(conn) + + # 从 comm_panel 列表移除 + self.comm_panel.remove_serial_connection_from_list(conn) + + # 从 UI 移除 + panel.setParent(None) + panel.deleteLater() + + self.statusBar().showMessage(f"已移除 Serial 连接: {conn['port']}", 3000) + def create_socket_group_panel(self, socket_id): """創建 socket 分組面板""" color = self.socket_colors.get(socket_id, self.socket_colors['default']) @@ -592,15 +641,45 @@ class ControlStationUI(QMainWindow): heading = data.get('heading') self.drone_headings[drone_id] = heading groundspeed = data.get('groundspeed') + airspeed = data.get('airspeed') + throttle = data.get('throttle') + hud_alt = data.get('alt') + climb = data.get('climb') + heading_text = f"{heading:.1f}°" if isinstance(groundspeed, (int, float)): groundspeed_text = f"{groundspeed:.1f} m/s" else: groundspeed_text = "--" + + if isinstance(airspeed, (int, float)): + airspeed_text = f"{airspeed:.1f} m/s" + else: + airspeed_text = "--" + + if isinstance(throttle, (int, float)): + throttle_text = f"{throttle:.0f}%" + else: + throttle_text = "--" + + if isinstance(hud_alt, (int, float)): + hud_alt_text = f"{hud_alt:.1f} m" + else: + hud_alt_text = "--" + + if isinstance(climb, (int, float)): + climb_text = f"{climb:.1f} m/s" + else: + climb_text = "--" + self.update_field(panel, drone_id, 'heading', heading_text) self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) self.update_overview_table(drone_id, 'heading', heading_text) self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) + self.update_overview_table(drone_id, 'airspeed', airspeed_text) + self.update_overview_table(drone_id, 'throttle', throttle_text) + self.update_overview_table(drone_id, 'hud_alt', hud_alt_text) + self.update_overview_table(drone_id, 'climb', climb_text) # 如果有位置資訊，也更新地圖上的航向 if drone_id in self.drone_positions: @@ -948,6 +1027,21 @@ class ControlStationUI(QMainWindow): import traceback traceback.print_exc() + def handle_drone_clicked(self, drone_id): + """ + 處理地圖上無人機被點擊事件，切換該無人機的選取狀態 + + Args: + drone_id: 無人機 ID + """ + print(f"地圖上點擊無人機: {drone_id}") + + if drone_id in self.drones: + panel = self.drones[drone_id] + checkbox = panel.get_checkbox() + # 切換勾選狀態 + checkbox.setChecked(not checkbox.isChecked()) + def show_planned_waypoints(self): """ 顯示規劃的航點（在終端輸出） diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 24bd2d2..5b1f6ef 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -214,6 +214,9 @@ class DroneMap: rotationOrigin: 'center' }) .on('click', function () { + if (bridge) { + bridge.emitDroneClicked(id); + } focusOn(id); }) .addTo(map); @@ -223,6 +226,9 @@ class DroneMap: zIndexOffset: 1000 }) .on('click', function() { + if (bridge) { + bridge.emitDroneClicked(id); + } focusOn(id); }) .addTo(map); @@ -423,9 +429,14 @@ class DroneMap: """獲取 GPS 信號""" return self.bridge.gps_signal + def get_drone_clicked_signal(self): + """獲取無人機點擊信號""" + return self.bridge.drone_clicked + class MapBridge(QObject): """JavaScript 和 Python 之間的橋接類""" gps_signal = pyqtSignal(float, float) # lat, lon + drone_clicked = pyqtSignal(str) # drone_id def __init__(self): super().__init__() @@ -433,4 +444,9 @@ class MapBridge(QObject): @pyqtSlot(float, float) def emitGpsSignal(self, lat, lon): """供 JavaScript 調用的方法""" - self.gps_signal.emit(lat, lon) \ No newline at end of file + self.gps_signal.emit(lat, lon) + + @pyqtSlot(str) + def emitDroneClicked(self, drone_id): + """供 JavaScript 調用的方法 - 當無人機被點擊時""" + self.drone_clicked.emit(drone_id) \ No newline at end of file diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py index 11953f0..452eb06 100644 --- a/src/GUI/overview_table.py +++ b/src/GUI/overview_table.py @@ -5,11 +5,38 @@ from PyQt6.QtCore import Qt class OverviewTable(QTableWidget): """總覽表格，顯示所有無人機的狀態資訊""" - def __init__(self, info_types, info_type_map, parent=None): + # 默認的資訊類型和映射 + DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "Local", "Velocity", "地速", "航向", + "空速", "油門", "HUD高", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"] + + DEFAULT_INFO_TYPE_MAP = { + "mode": 0, + "armed": 1, + "battery": 2, + "longitude": 3, + "latitude": 4, + "altitude": 5, + "local": 6, + "velocity": 7, + "groundspeed": 8, + "heading": 9, + "airspeed": 10, + "throttle": 11, + "hud_alt": 12, + "climb": 13, + "roll": 14, + "pitch": 15, + "yaw": 16, + "loss_rate": 17, + "ping": 18 + } + + def __init__(self, info_types=None, info_type_map=None, parent=None): super().__init__(parent) - self.info_types = info_types - self.info_type_map = info_type_map + # 使用提供的或默認的資訊類型 + self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES + self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP self.drones = {} # 存儲無人機面板的引用 # 初始化表格 From 36c881b22940533b2b76c7eb042ee6821b7e23e4 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 4 Feb 2026 12:43:57 +0800 Subject: [PATCH 101/146] Upload files to 'src/GUI' --- src/GUI/gui.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1e0c5ab..f110256 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -580,7 +580,10 @@ class ControlStationUI(QMainWindow): cells = round(voltage / 3.95) # 計算電量百分比 - percentage = (voltage / cells - 3.7) / 0.5 * 100 + if cells == 0: + percentage = 0 + else: + percentage = (voltage / cells - 3.7) / 0.5 * 100 # 根據百分比設置顏色 if percentage < 20: From f7f90c31685684db9530280b90516318bfb130c1 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Sat, 7 Feb 2026 18:31:22 +0800 Subject: [PATCH 103/146] Upload files to 'src/GUI' --- src/GUI/communication.py | 156 ++++++++----- src/GUI/gui.py | 13 +- src/GUI/map_layout.py | 483 ++++++++++++++++++++++++++++++++++++++- 3 files changed, 582 insertions(+), 70 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index c95e74e..b97828b 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -20,12 +20,15 @@ class DroneSignals(QObject): class UDPMavlinkReceiver(threading.Thread): """UDP MAVLink 接收器""" - def __init__(self, ip, port, signals, connection_name): + def __init__(self, ip, port, signals, connection_name, monitor=None): super().__init__(daemon=True) self.ip = ip self.port = port self.signals = signals self.connection_name = connection_name + self.monitor = monitor # 保存 monitor 引用 + # UDP 使用原始 socket_id = 8 + self.socket_id = monitor.get_or_assign_socket_id('8') if monitor else 0 self.running = False self.sock = None @@ -61,7 +64,7 @@ class UDPMavlinkReceiver(threading.Thread): try: msg_type = msg.get_type() system_id = msg.get_srcSystem() - drone_id = f"s8_{system_id}" # 使用 s8_ 前綴表示 UDP 來源 + drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) @@ -116,11 +119,13 @@ class UDPMavlinkReceiver(threading.Thread): }) elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading self.signals.update_signal.emit('hud', drone_id, { - 'heading': heading, - 'groundspeed': groundspeed + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.alt, + 'climb': msg.climb }) except Exception as e: @@ -132,12 +137,15 @@ class UDPMavlinkReceiver(threading.Thread): class SerialMavlinkReceiver(threading.Thread): """串口 MAVLink 接收器""" - def __init__(self, port, baudrate, signals, connection_name): + def __init__(self, port, baudrate, signals, connection_name, monitor=None): super().__init__(daemon=True) self.port = port self.baudrate = baudrate self.signals = signals self.connection_name = connection_name + self.monitor = monitor # 保存 monitor 引用 + # Serial 使用原始 socket_id = 5 + self.socket_id = monitor.get_or_assign_socket_id('5') if monitor else 0 self.running = False self.mav = None @@ -184,7 +192,7 @@ class SerialMavlinkReceiver(threading.Thread): try: msg_type = msg.get_type() system_id = msg.get_srcSystem() - drone_id = f"s5_{system_id}" # 使用 serial_ 前綴表示串口來源 + drone_id = f"s{self.socket_id}_{system_id}" if msg_type == "HEARTBEAT": mode = mavutil.mode_string_v10(msg) @@ -239,11 +247,13 @@ class SerialMavlinkReceiver(threading.Thread): }) elif msg_type == "VFR_HUD": - groundspeed = msg.groundspeed - heading = msg.heading self.signals.update_signal.emit('hud', drone_id, { - 'heading': heading, - 'groundspeed': groundspeed + 'airspeed': msg.airspeed, + 'groundspeed': msg.groundspeed, + 'heading': msg.heading, + 'throttle': msg.throttle, + 'alt': msg.alt, + 'climb': msg.climb }) except Exception as e: @@ -255,12 +265,12 @@ class SerialMavlinkReceiver(threading.Thread): class WebSocketMavlinkReceiver(threading.Thread): """WebSocket MAVLink 接收器""" - def __init__(self, url, signals, connection_name): + def __init__(self, url, signals, connection_name, monitor=None): super().__init__(daemon=True) self.url = url self.signals = signals self.connection_name = connection_name - self.running = False + self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False self.max_retries = 5 self.base_delay = 1.0 @@ -319,10 +329,10 @@ class WebSocketMavlinkReceiver(threading.Thread): def process_websocket_message(self, data): """處理 WebSocket 訊息""" try: - drone_id = data.get('system_id') - drone_id = f"s9_{drone_id}" if drone_id else None - if not drone_id: + system_id = data.get('system_id') + if not system_id: return + drone_id = f"s{self.socket_id}_{system_id}" # 模式 if 'mode' in data: @@ -398,15 +408,39 @@ class DroneMonitor(Node): self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}} # ================================================================================ + # ================================================================================ + # 【新增】Socket ID 重新分配機制 (從 0 開始) + # ================================================================================ + self.socket_id_mapping = {} # {原始socket_id: 重新分配的socket_id} + self.socket_id_counter = 0 # 當前分配到的最大socket_id + self.socket_id_lock = Lock() # 線程安全鎖 + # ================================================================================ + # ================================================================================ # 【新增】儲存 sys_id 到 actual_drone_id 的映射 (從 summary 獲取) # ================================================================================ self.sys_to_actual_id = {} # {sys_id: actual_drone_id} e.g. {'sys11': 's0_11'} + self.sys_to_socket_id = {} # {sys_id: assigned_socket_id} e.g. {'sys11': 0} # ================================================================================ self.serial_receivers = [] # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + + def get_or_assign_socket_id(self, original_socket_id): + """根據原始 socket_id 分配或獲取對應的 socket_id（從 0 開始連續分配） + 同一個原始 socket_id 會得到同一個分配的 ID + """ + original_socket_id = str(original_socket_id) + + with self.socket_id_lock: + if original_socket_id not in self.socket_id_mapping: + # 分配新的 socket_id + self.socket_id_mapping[original_socket_id] = self.socket_id_counter + print(f"🆕 Socket ID 映射: 原始 {original_socket_id} -> 分配 {self.socket_id_counter}") + self.socket_id_counter += 1 + + return self.socket_id_mapping[original_socket_id] def scan_topics(self): topics = self.get_topic_names_and_types() @@ -416,34 +450,36 @@ class DroneMonitor(Node): for topic_name, _ in topics: if match := drone_pattern.match(topic_name): sys_id, topic_type = match.groups() - # 將 sys11 轉換為 s0_11 格式以保持兼容性 - drone_num = sys_id.replace('sys', '') - drone_id = f's0_{drone_num}' - found_drones.add(drone_id) + found_drones.add(sys_id) with self.lock: - self.drone_topics.setdefault(drone_id, set()).add(topic_type) + self.drone_topics.setdefault(sys_id, set()).add(topic_type) - for drone_id in found_drones: - if not hasattr(self, f'drone_{drone_id}_subs'): - self.setup_drone(drone_id) + for sys_id in found_drones: + # 為每個 sys_id 分配 socket_id（如果還沒有分配） + # 注意：如果後續 summary 提供了 socket_id，會使用 summary 的映射覆蓋 + if sys_id not in self.sys_to_socket_id: + # 暫時所有 ROS2 topic 共享同一個原始 socket_id，等 summary 提供實際的 socket_id + self.sys_to_socket_id[sys_id] = self.get_or_assign_socket_id('0') + + if not hasattr(self, f'drone_{sys_id}_subs'): + self.setup_drone(sys_id) - def setup_drone(self, drone_id): - # 從 s0_11 轉換回 sys11 格式 - sys_id = drone_id.replace('s0_', 'sys') + def setup_drone(self, sys_id): + # sys_id 格式： sys11, sys12, ... base_topic = f'/fc_network/vehicle/{sys_id}' # Add service clients (保留但可能無法使用，因為新 topic 可能沒有這些服務) - self.arm_clients[drone_id] = self.create_client( + self.arm_clients[sys_id] = self.create_client( CommandBool, f'{base_topic}/cmd/arming' ) - self.takeoff_clients[drone_id] = self.create_client( + self.takeoff_clients[sys_id] = self.create_client( CommandTOL, f'{base_topic}/cmd/takeoff' ) # Add setpoint publisher - self.setpoint_pubs[drone_id] = self.create_publisher( + self.setpoint_pubs[sys_id] = self.create_publisher( Point, f'{base_topic}/setpoint_position/local', 10 @@ -453,30 +489,30 @@ class DroneMonitor(Node): 'battery': self.create_subscription( BatteryState, f'{base_topic}/battery', - lambda msg, did=drone_id: self.battery_callback(did, msg), + lambda msg, sid=sys_id: self.battery_callback(sid, msg), 10 ), 'position': self.create_subscription( NavSatFix, f'{base_topic}/position', - lambda msg, did=drone_id: self.gps_callback(did, msg), + lambda msg, sid=sys_id: self.gps_callback(sid, msg), 10 ), 'summary': self.create_subscription( String, f'{base_topic}/summary', - lambda msg, did=drone_id: self.summary_callback(did, msg), + lambda msg, sid=sys_id: self.summary_callback(sid, msg), 10 ), 'vfr_hud': self.create_subscription( VfrHud, f'{base_topic}/vfr_hud', - lambda msg, did=drone_id: self.hud_callback(did, msg), + lambda msg, sid=sys_id: self.hud_callback(sid, msg), 10 ) } - setattr(self, f'drone_{drone_id}_subs', subs) + setattr(self, f'drone_{sys_id}_subs', subs) async def arm_drone(self, drone_id, arm): if drone_id not in self.arm_clients: @@ -558,9 +594,9 @@ class DroneMonitor(Node): msg.angular_velocity.z) } - def battery_callback(self, drone_id, msg): + def battery_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -578,7 +614,7 @@ class DroneMonitor(Node): 'armed': msg.armed } - def summary_callback(self, drone_id, msg): + def summary_callback(self, sys_id, msg): """處理 summary topic (JSON 格式)""" try: data = json.loads(msg.data) @@ -586,28 +622,34 @@ class DroneMonitor(Node): if mode in self.filtered_modes: return - # 根據 socket_id 更新 drone_id - socket_id = data.get('socket_id') + # 從 summary 獲取原始 socket_id，並映射到分配的 socket_id + original_socket_id = data.get('socket_id') + if original_socket_id is not None: + # 使用原始 socket_id 獲取或分配統一的 socket_id + assigned_socket_id = self.get_or_assign_socket_id(original_socket_id) + else: + # 如果沒有 socket_id，使用 sys_to_socket_id 映射 + assigned_socket_id = self.sys_to_socket_id.get(sys_id, 0) + sysid = data.get('sysid') - if socket_id is not None and sysid is not None: - # 使用 socket_id 作為前綴 - actual_drone_id = f's{socket_id}_{sysid}' + if sysid is not None: + actual_drone_id = f's{assigned_socket_id}_{sysid}' # ================================================================================ # 【關鍵】保存 sys_id 到 actual_drone_id 的映射 # ================================================================================ - sys_key = f'sys{sysid}' - self.sys_to_actual_id[sys_key] = actual_drone_id - # 也保存原始的 s0_ 格式到實際 ID 的映射 - self.sys_to_actual_id[drone_id] = actual_drone_id + self.sys_to_actual_id[sys_id] = actual_drone_id # ================================================================================ else: - actual_drone_id = drone_id + # 如果沒有 sysid，使用 sys_id 中的數字 + sys_num = sys_id.replace('sys', '') + actual_drone_id = f's{assigned_socket_id}_{sys_num}' + self.sys_to_actual_id[sys_id] = actual_drone_id self.latest_data[(actual_drone_id, 'state')] = { 'mode': mode, 'armed': data.get('armed', False), - 'socket_id': socket_id, + 'socket_id': original_socket_id, 'sysid': sysid, 'vehicle_type': data.get('vehicle_type'), 'autopilot': data.get('autopilot'), @@ -616,13 +658,13 @@ class DroneMonitor(Node): 'connected': data.get('connected') } except json.JSONDecodeError as e: - print(f"Error parsing summary JSON for {drone_id}: {e}") + print(f"Error parsing summary JSON for {sys_id}: {e}") except Exception as e: - print(f"Error in summary_callback for {drone_id}: {e}") + print(f"Error in summary_callback for {sys_id}: {e}") - def gps_callback(self, drone_id, msg): + def gps_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -662,9 +704,9 @@ class DroneMonitor(Node): 'z': msg.z } - def hud_callback(self, drone_id, msg): + def hud_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id - actual_drone_id = self.sys_to_actual_id.get(drone_id, None) + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) # 如果還沒有從 summary 獲取到映射，則不處理 if actual_drone_id is None: return @@ -691,7 +733,7 @@ class DroneMonitor(Node): def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): """啟動串口 MAVLink 連接""" connection_name = f"Serial_{port.replace('/', '_')}" - receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name) + receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver.start() self.serial_receivers.append(receiver) print(f"Started serial connection on {port} at {baudrate} baud") diff --git a/src/GUI/gui.py b/src/GUI/gui.py index f110256..d73b08d 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -276,7 +276,7 @@ class ControlStationUI(QMainWindow): } # 启动接收器 - receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name']) + receiver = UDPMavlinkReceiver(ip, port, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.udp_receivers.append(receiver) new_conn['receiver'] = receiver @@ -299,7 +299,7 @@ class ControlStationUI(QMainWindow): } # 启动接收器 - receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name']) + receiver = WebSocketMavlinkReceiver(url, self.monitor.signals, new_conn['name'], self.monitor) receiver.start() self.monitor.ws_receivers.append(receiver) new_conn['receiver'] = receiver @@ -845,10 +845,13 @@ class ControlStationUI(QMainWindow): self.overview_table.update_table(drone_id, field, value) def get_socket_id(self, drone_id): - """從 drone_id 提取 socket_id (例如 's9_1' -> '9')""" + """從 drone_id 提取 socket_id (例如 's0_1' -> '0')""" import re - match = re.match(r's(\d+)_\d+', drone_id) - return match.group(1) if match else 'unknown' + match = re.match(r's(\d+)_(\d+)', drone_id) + if not match: + return 'unknown' + + return match.group(1) def add_drone(self, drone_id): if drone_id in self.drones: diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 5b1f6ef..e0a8951 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -32,15 +32,27 @@ class DroneMap: @@ -58,6 +151,25 @@ class DroneMap:
+
+ +
+
+
+ + + +
+
+ 中心點: + 未設定 +
+
+ 目標點: + 未設定 +
+ +
@@ -57,6 +181,24 @@ class DroneMap:
+ + +
+
+
+ + +
+
+ 中心點: + 未設定 +
+
+ 目標點: + 未設定 +
+ +
+ + + + + +
+
+ +
+ + +
+ + +
+
+
+ + +
+
+ 中心點: + 未設定 +
+
+ 目標點: + 未設定 +
+ + +
+ + + + + ''' + + self.map_view.setHtml(inline_html) + self.map_view.loadFinished.connect(self._on_map_loaded) + + # 設置地圖更新計時器 + self.map_update_timer = QTimer() + self.map_update_timer.timeout.connect(self.update_map_positions) + self.map_update_timer.start(200) # 每 200ms 更新一次 + + def _on_map_loaded(self, ok: bool): + """地圖加載完成回調""" + if ok: + self.map_loaded = True + else: + print("⚠️ 地圖加載失敗") + + def update_drone_position(self, drone_id, lat, lon, heading): + """更新無人機位置（加入待處理隊列）""" + self.pending_map_updates[drone_id] = (lat, lon, heading) + + def update_map_positions(self): + """批量更新地圖上的無人機位置""" + if not self.map_loaded or not self.pending_map_updates: + return + + js_commands = [] + for drone_id, (lat, lon, heading) in self.pending_map_updates.items(): + js_commands.append(f"updateDrone({lat:.6f}, {lon:.6f}, '{drone_id}', {heading:.1f});") + + if js_commands: + combined_js = "\n".join(js_commands) + self.map_view.page().runJavaScript(combined_js) + + self.pending_map_updates.clear() + + def clear_trajectories(self): + """清除所有軌跡""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearAllTrajectories();") + + def focus_on_drone(self, drone_id): + """聚焦到指定無人機""" + if self.map_loaded: + self.map_view.page().runJavaScript(f"focusOn('{drone_id}');") + + # ================================================================================ + # 任務規劃視覺化方法 + # ================================================================================ + def draw_mission_plan(self, center_lat, center_lon, target_lat, target_lon): + """在地圖上繪製任務規劃""" + if self.map_loaded: + js_code = f"drawMissionPlan({center_lat:.6f}, {center_lon:.6f}, {target_lat:.6f}, {target_lon:.6f});" + self.map_view.page().runJavaScript(js_code) + print(f"📍 地圖已繪製任務規劃: C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") + + def clear_mission_plan(self): + """清除地圖上的任務規劃標記""" + if self.map_loaded: + self.map_view.page().runJavaScript("clearMissionPlan();") + print("🗑️ 地圖已清除任務規劃") + # ================================================================================ + + def get_widget(self): + """獲取地圖 widget""" + return self.map_view + + def get_gps_signal(self): + """獲取 GPS 信號""" + return self.bridge.gps_signal + + def get_drone_clicked_signal(self): + """獲取無人機點擊信號""" + return self.bridge.drone_clicked + + def get_clear_all_drone_selection_signal(self): + """獲取清除所有無人機選擇信號""" + return self.bridge.clear_all_drone_selection + + def get_toggle_select_all_drones_signal(self): + """獲取切換全選所有無人機信號""" + return self.bridge.select_all_drones + + def get_select_all_drones_signal(self): + """獲取全選所有無人機信號""" + return self.bridge.select_all_drones + + def get_start_mission_signal(self): + """獲取開始任務信號""" + return self.bridge.start_mission_signal + + def get_pause_mission_signal(self): + """獲取暫停任務信號""" + return self.bridge.pause_mission_signal + + def get_rectangle_selected_signal(self): + """獲取矩形選擇信號""" + return self.bridge.rectangle_selected + + def get_polygon_selected_signal(self): + """獲取多邊形選擇信號""" + return self.bridge.polygon_selected + + def get_mission_mode_changed_signal(self): + """獲取任務模式切換信號""" + return self.bridge.mission_mode_changed + + def get_route_confirmed_signal(self): + """獲取路徑確認信號""" + return self.bridge.route_confirmed + +class MapBridge(QObject): + """JavaScript 和 Python 之間的橋接類""" + gps_signal = pyqtSignal(float, float) + drone_clicked = pyqtSignal(str) + clear_all_drone_selection = pyqtSignal() + select_all_drones = pyqtSignal() + start_mission_signal = pyqtSignal(float, float, float, float) + pause_mission_signal = pyqtSignal() + rectangle_selected = pyqtSignal(str) + polygon_selected = pyqtSignal(str) + mission_mode_changed = pyqtSignal(str) + route_confirmed = pyqtSignal(str) # 路徑確認 (JSON 字串) + + def __init__(self): + super().__init__() + + @pyqtSlot(float, float) + def emitGpsSignal(self, lat, lon): + """供 JavaScript 調用的方法""" + self.gps_signal.emit(lat, lon) + + @pyqtSlot(str) + def emitDroneClicked(self, drone_id): + """供 JavaScript 調用的方法 - 當無人機被點擊時""" + self.drone_clicked.emit(drone_id) + + @pyqtSlot() + def clearAllDroneSelection(self): + """供 JavaScript 調用的方法 - 清除所有無人機選擇""" + self.clear_all_drone_selection.emit() + print("🗑️ 清除所有無人機選擇") + + @pyqtSlot() + def toggleSelectAllDrones(self): + """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機""" + self.select_all_drones.emit() + print("🔄 切換全選無人機") + + @pyqtSlot(float, float, float, float) + def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon): + """供 JavaScript 調用的方法 - 開始任務""" + self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon) + print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})") + + @pyqtSlot() + def pauseMissionSignal(self): + """供 JavaScript 調用的方法 - 暫停任務""" + self.pause_mission_signal.emit() + print("⏸️ 暫停任務信號已發出") + + @pyqtSlot(str) + def rectangleSelected(self, points_json): + """供 JavaScript 調用的方法 - 矩形選擇完成""" + self.rectangle_selected.emit(points_json) + print(f"📦 矩形區域已選擇: {points_json}") + + @pyqtSlot(str) + def polygonSelected(self, points_json): + """供 JavaScript 調用的方法 - 多邊形選擇完成""" + self.polygon_selected.emit(points_json) + print(f"🔷 多邊形區域已選擇: {points_json}") + + @pyqtSlot(str) + def missionModeChanged(self, mode): + """供 JavaScript 調用的方法 - 任務模式切換""" + self.mission_mode_changed.emit(mode) + print(f"🔄 任務模式切換: {mode}") + + @pyqtSlot(str) + def routeConfirmed(self, points_json): + """供 JavaScript 調用的方法 - 路徑確認""" + self.route_confirmed.emit(points_json) + print(f"📍 路徑已確認: {points_json}") \ No newline at end of file diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py new file mode 100644 index 0000000..b6afdff --- /dev/null +++ b/src/GUI/mission_executor.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python3 +""" +任務執行模組 +管理多架無人機的 GUIDED 模式飛行控制迴圈 + +設計: + - 每架無人機持有一個航點序列，逐點推進 + - 各自到達就各自切換到下一個航點 + - 用 QTimer 驅動，在 Qt 主線程執行 + - 暫停 = 停止發送 setpoint，飛控自動懸停 + - 相容舊的 2 階段任務與新的多航點任務 (Grid Sweep) +""" +import math +from enum import Enum +from PyQt6.QtCore import QObject, QTimer, pyqtSignal + + +class MissionState(Enum): + """整體任務狀態""" + IDLE = "idle" + RUNNING = "running" + PAUSED = "paused" + + +class DroneTask: + """單架無人機的任務資料""" + __slots__ = ('drone_id', 'sysid', 'waypoints', 'wp_index', 'done') + + def __init__(self, drone_id, sysid, waypoints): + """ + Args: + drone_id: GUI 用的 ID (如 's0_11') + sysid: MAVLink system ID (如 11) + waypoints: 航點序列 [(lat, lon, alt), ...] + """ + self.drone_id = drone_id + self.sysid = sysid + self.waypoints = waypoints + self.wp_index = 0 + self.done = len(waypoints) == 0 + + @property + def current_target(self): + if self.done or self.wp_index >= len(self.waypoints): + return None + return self.waypoints[self.wp_index] + + @property + def total_waypoints(self): + return len(self.waypoints) + + +class MissionExecutor(QObject): + """ + 任務執行器 + + planned_waypoints 格式: + { + 'drone_ids': ['s0_1', 's0_2', ...], + 'waypoints': [ + [(lat,lon,alt), ...], # drone 0 + [(lat,lon,alt), ...], # drone 1 + ] + } + """ + + # 信號 + drone_waypoint_reached = pyqtSignal(str, int, int) # (drone_id, wp_index, total) + mission_completed = pyqtSignal() + + def __init__(self, sender, drone_gps, + arrival_radius=2.0, send_rate_hz=2.0): + super().__init__() + self.sender = sender + self.drone_gps = drone_gps + self.arrival_radius = arrival_radius + self.state = MissionState.IDLE + self.tasks = {} + + self._interval_ms = int(1000 / send_rate_hz) + self._timer = QTimer() + self._timer.timeout.connect(self._tick) + + # ------------------------------------------------------------------ 公開方法 + + def start(self, planned_waypoints): + """啟動任務""" + if self.state == MissionState.RUNNING: + print("任務已在執行中") + return + + self.tasks.clear() + + drone_ids = planned_waypoints['drone_ids'] + waypoints_list = planned_waypoints['waypoints'] + + for i, drone_id in enumerate(drone_ids): + sysid = int(drone_id.split('_')[1]) + self.tasks[drone_id] = DroneTask(drone_id, sysid, waypoints_list[i]) + + self.state = MissionState.RUNNING + self._timer.start(self._interval_ms) + + total_wps = sum(t.total_waypoints for t in self.tasks.values()) + print(f"任務啟動: {len(self.tasks)} 架無人機, " + f"共 {total_wps} 個航點, " + f"到達半徑={self.arrival_radius}m, " + f"發送週期={self._interval_ms}ms") + + def pause(self): + """暫停任務""" + if self.state == MissionState.RUNNING: + self._timer.stop() + self.state = MissionState.PAUSED + print("任務暫停") + + def resume(self): + """恢復任務""" + if self.state == MissionState.PAUSED: + self._timer.start(self._interval_ms) + self.state = MissionState.RUNNING + print("任務恢復") + + def stop(self): + """停止任務""" + self._timer.stop() + self.tasks.clear() + self.state = MissionState.IDLE + print("任務停止") + + # ------------------------------------------------------------------ 控制迴圈 + + def _tick(self): + """控制迴圈""" + all_done = True + + for task in self.tasks.values(): + if task.done: + continue + + all_done = False + target = task.current_target + if target is None: + continue + + # 讀取當前 GPS + gps = self.drone_gps.get(task.drone_id) + if gps is None: + continue + + tgt_lat, tgt_lon, tgt_alt = target + distance = _haversine(gps['lat'], gps['lon'], tgt_lat, tgt_lon) + + # 到達判定 + if distance < self.arrival_radius: + task.wp_index += 1 + if task.wp_index >= task.total_waypoints: + task.done = True + self.drone_waypoint_reached.emit( + task.drone_id, task.wp_index, task.total_waypoints + ) + print(f" {task.drone_id} → DONE " + f"({task.total_waypoints}/{task.total_waypoints})") + continue + else: + self.drone_waypoint_reached.emit( + task.drone_id, task.wp_index, task.total_waypoints + ) + print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} " + f"(距離: {distance:.1f}m)") + # 更新目標 + tgt_lat, tgt_lon, tgt_alt = task.current_target + + # 發送 setpoint + self.sender.send_position_global( + task.sysid, tgt_lat, tgt_lon, tgt_alt + ) + + # 全部完成檢查 + if all_done and self.tasks: + self._timer.stop() + self.state = MissionState.IDLE + self.mission_completed.emit() + print("===== 任務全部完成 =====") + + +# ------------------------------------------------------------------ 工具函式 + +def _haversine(lat1, lon1, lat2, lon2): + """計算兩個 GPS 座標間的水平距離 (公尺)""" + R = 6371000.0 + lat1_r = math.radians(lat1) + lat2_r = math.radians(lat2) + dlat = math.radians(lat2 - lat1) + dlon = math.radians(lon2 - lon1) + + a = (math.sin(dlat / 2) ** 2 + + math.cos(lat1_r) * math.cos(lat2_r) * math.sin(dlon / 2) ** 2) + return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) \ No newline at end of file diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py new file mode 100644 index 0000000..239529e --- /dev/null +++ b/src/GUI/mission_planner.py @@ -0,0 +1,538 @@ +#!/usr/bin/env python3 +""" +飛行任務規劃模組 +支援: M-Formation, Circle, Leader-Follower (Bezier 轉彎), Grid Sweep +""" +import math +from typing import List, Tuple, Optional, Dict, Any +from enum import Enum + + +class MissionType(Enum): + """任務類型""" + M_FORMATION = "m_formation" + CIRCLE_FORMATION = "circle_formation" + LEADER_FOLLOWER = "leader_follower" + GRID_SWEEP = "grid_sweep" + + +class CoordinateConverter: + """GPS 座標與 Local 座標的轉換器""" + + def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0): + self.origin_lat = origin_lat + self.origin_lon = origin_lon + self.origin_alt = origin_alt + self.R = 6371000.0 + + def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]: + lat_rad = math.radians(lat) + lon_rad = math.radians(lon) + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + dlat = lat_rad - origin_lat_rad + dlon = lon_rad - origin_lon_rad + + x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2) + y = self.R * dlat + z = alt - self.origin_alt + + return x, y, z + + def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]: + origin_lat_rad = math.radians(self.origin_lat) + origin_lon_rad = math.radians(self.origin_lon) + + lat_rad = origin_lat_rad + (y / self.R) + lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2))) + + lat = math.degrees(lat_rad) + lon = math.degrees(lon_rad) + alt = self.origin_alt + z + + return lat, lon, alt + + +class FormationPlanner: + """隊形規劃器""" + + def __init__(self, spacing: float = 5.0, + base_altitude: float = 10.0, + altitude_diff: float = 2.0): + self.spacing = spacing + self.base_altitude = base_altitude + self.altitude_diff = altitude_diff + self.current_origin = None + self.converter = None + + def plan_formation_mission(self, + drone_gps_positions: List[Tuple[float, float, float]], + target_gps: Tuple[float, float, float], + mission_type: MissionType = MissionType.M_FORMATION, + params: Optional[Dict[str, Any]] = None + ) -> Tuple[List[List[Tuple[float, float, float]]], + Tuple[float, float, float]]: + if len(drone_gps_positions) == 0: + raise ValueError("無人機位置列表不能為空") + + center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions) + center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions) + center_alt = sum(pos[2] for pos in drone_gps_positions) / len(drone_gps_positions) + + self.current_origin = (center_lat, center_lon, center_alt) + self.converter = CoordinateConverter(center_lat, center_lon, center_alt) + + drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions] + target_local = self.converter.gps_to_local(*target_gps) + + if mission_type == MissionType.M_FORMATION: + s1, s2 = self._calculate_m_formation(drone_local, target_local, params) + waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))] + + elif mission_type == MissionType.CIRCLE_FORMATION: + s1, s2 = self._calculate_circle_formation(drone_local, target_local, params) + waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))] + + elif mission_type == MissionType.LEADER_FOLLOWER: + params = params or {} + route_wps_gps = params.get('route_waypoints') + if route_wps_gps is None or len(route_wps_gps) < 2: + raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點") + route_wps_local = [ + self.converter.gps_to_local(wp[0], wp[1], 0)[:2] + for wp in route_wps_gps + ] + waypoints_local = self._calculate_leader_follower(drone_local, route_wps_local, params) + + elif mission_type == MissionType.GRID_SWEEP: + params = params or {} + rect_corners_gps = params.get('rect_corners') + if rect_corners_gps is None or len(rect_corners_gps) != 4: + raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點") + rect_corners_local = [ + self.converter.gps_to_local(c[0], c[1], 0)[:2] + for c in rect_corners_gps + ] + waypoints_local = self._calculate_grid_sweep(drone_local, rect_corners_local, params) + else: + raise ValueError(f"不支援的任務類型: {mission_type}") + + waypoints_gps = [] + for drone_wps in waypoints_local: + gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps] + waypoints_gps.append(gps_wps) + + return waypoints_gps, self.current_origin + + # ------------------------------------------------------------------ M-Formation + + def _calculate_m_formation(self, drone_positions, target_point, params): + params = params or {} + N = len(drone_positions) + spacing = params.get('spacing', self.spacing) + base_altitude = params.get('base_altitude', self.base_altitude) + altitude_diff = params.get('altitude_diff', self.altitude_diff) + + C_x = sum(pos[0] for pos in drone_positions) / N + C_y = sum(pos[1] for pos in drone_positions) / N + + T_x, T_y, T_z = target_point + V_x, V_y = T_x - C_x, T_y - C_y + P_x, P_y = -V_y, V_x + + length = math.sqrt(P_x ** 2 + P_y ** 2) + P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0) + + projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i) + for i, pos in enumerate(drone_positions)] + projections.sort() + + stage1_positions = [None] * N + stage2_positions = [None] * N + + for rank, (_, original_idx) in enumerate(projections): + offset = (rank - (N - 1) / 2) * spacing + altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff) + stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude) + stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude) + + return stage1_positions, stage2_positions + + # ------------------------------------------------------------------ Circle + + def _calculate_circle_formation(self, drone_positions, target_point, params): + params = params or {} + N = len(drone_positions) + radius = params.get('radius', 10.0) + altitude = params.get('altitude', 10.0) + start_angle = params.get('start_angle', 0.0) + + center_x, center_y, center_z = target_point + stage1_positions = [] + stage2_positions = [] + angle_step = 360.0 / N + + for i in range(N): + angle_rad = math.radians(start_angle + angle_step * i) + final_x = center_x + radius * math.cos(angle_rad) + final_y = center_y + radius * math.sin(angle_rad) + final_z = altitude + + current_x, current_y, current_z = drone_positions[i] + stage1_positions.append(( + current_x + (final_x - current_x) * 0.5, + current_y + (final_y - current_y) * 0.5, + current_z + (final_z - current_z) * 0.5 + )) + stage2_positions.append((final_x, final_y, final_z)) + + return stage1_positions, stage2_positions + + # ------------------------------------------------------------------ 路徑跟隨 (Bezier 轉彎) + + def _calculate_leader_follower(self, drone_positions, route_wps_local, params): + """ + 路徑跟隨編隊 — Bezier 曲線轉彎版 + + 步驟: + 1. _build_center_path: 在轉折 WP 處用二次 Bezier 曲線平滑轉彎 + 2. 固定排序，每架無人機沿中心路徑套用橫向+縱向偏移 + + 隊形 (俯視): + | (前進方向) | + | D1 | ← 左偏移, 後 0m + | D2 | ← 右偏移, 後 5m + | D3 | ← 左偏移, 後 10m + | D4 | ← 右偏移, 後 15m + """ + N = len(drone_positions) + lateral_offset = params.get('lateral_offset', 3.0) + longitudinal_spacing = params.get('longitudinal_spacing', 5.0) + altitude = params.get('altitude', self.base_altitude) + turn_margin = params.get('turn_margin', 0.35) # 轉彎切入距離佔段長比例 + curve_resolution = params.get('curve_resolution', 8) # 每個彎道的插值點數 + + # Step 1: 建立帶 Bezier 轉彎的中心路徑 + center_path = self._build_center_path( + route_wps_local, turn_margin, curve_resolution + ) + + # Step 2: 固定排序 + first_dir = (center_path[0][2], center_path[0][3]) + first_perp = (-first_dir[1], first_dir[0]) + C_x = sum(p[0] for p in drone_positions) / N + C_y = sum(p[1] for p in drone_positions) / N + + projections = [ + ((pos[0] - C_x) * first_perp[0] + (pos[1] - C_y) * first_perp[1], i) + for i, pos in enumerate(drone_positions) + ] + projections.sort() + + # Step 3: 偏移 + all_waypoints = [None] * N + + for rank, (_, original_idx) in enumerate(projections): + lat_sign = -1 if rank % 2 == 0 else 1 + lat = lat_sign * lateral_offset + lon = rank * longitudinal_spacing + + waypoints = [] + for (cx, cy, dx, dy) in center_path: + perp_x, perp_y = -dy, dx + ox = cx + lat * perp_x - lon * dx + oy = cy + lat * perp_y - lon * dy + waypoints.append((ox, oy, altitude)) + + all_waypoints[original_idx] = waypoints + + return all_waypoints + + def _build_center_path(self, waypoints, turn_margin, curve_resolution): + """ + 建立帶 Bezier 曲線轉彎的中心路徑 + + 在每個轉折 WP 處: + 1. 計算 pre_turn = WP - d_in × T + 2. 計算 post_turn = WP + d_out × T + 3. 用二次 Bezier 曲線: B(t) = (1-t)²·pre + 2t(1-t)·WP + t²·post + 4. 方向從導數得到: B'(t) = 2(1-t)(WP-pre) + 2t(post-WP) + + Args: + waypoints: 路徑航點 [(x, y), ...] + turn_margin: 轉彎切入距離佔相鄰段長的比例 (0~0.5) + curve_resolution: 每個彎道的 Bezier 插值點數 + + Returns: + [(x, y, dir_x, dir_y), ...] 中心路徑點 + 單位方向 + """ + num_wps = len(waypoints) + + if num_wps < 2: + return [(waypoints[0][0], waypoints[0][1], 0.0, 1.0)] + + # 計算每段方向和長度 + segments = [] + for i in range(num_wps - 1): + dx = waypoints[i + 1][0] - waypoints[i][0] + dy = waypoints[i + 1][1] - waypoints[i][1] + length = math.sqrt(dx ** 2 + dy ** 2) + if length < 0.01: + segments.append((0.0, 1.0, length)) + else: + segments.append((dx / length, dy / length, length)) + + path = [] + + # 第一個航點 + path.append((waypoints[0][0], waypoints[0][1], + segments[0][0], segments[0][1])) + + # 中間航點 (轉彎處) + for i in range(1, num_wps - 1): + d_in_x, d_in_y, len_in = segments[i - 1] + d_out_x, d_out_y, len_out = segments[i] + + # 切入距離 T: 取相鄰兩段中較短的 × turn_margin + T = min(len_in, len_out) * turn_margin + + if T < 0.5: + # 段太短，不插彎，直接加一個平均方向點 + avg_dx = d_in_x + d_out_x + avg_dy = d_in_y + d_out_y + avg_len = math.sqrt(avg_dx ** 2 + avg_dy ** 2) + if avg_len > 0.01: + avg_dx /= avg_len + avg_dy /= avg_len + else: + avg_dx, avg_dy = d_in_x, d_in_y + path.append((waypoints[i][0], waypoints[i][1], avg_dx, avg_dy)) + continue + + # P0 = pre_turn (弧線起始) + p0_x = waypoints[i][0] - d_in_x * T + p0_y = waypoints[i][1] - d_in_y * T + + # P1 = WP 本身 (控制點) + p1_x = waypoints[i][0] + p1_y = waypoints[i][1] + + # P2 = post_turn (弧線結束) + p2_x = waypoints[i][0] + d_out_x * T + p2_y = waypoints[i][1] + d_out_y * T + + # 加入 pre_turn 點 (方向 = incoming) + path.append((p0_x, p0_y, d_in_x, d_in_y)) + + # 加入 Bezier 插值點 + for j in range(1, curve_resolution): + t = j / curve_resolution + + # B(t) = (1-t)²·P0 + 2t(1-t)·P1 + t²·P2 + one_minus_t = 1.0 - t + bx = one_minus_t * one_minus_t * p0_x + \ + 2 * t * one_minus_t * p1_x + \ + t * t * p2_x + by = one_minus_t * one_minus_t * p0_y + \ + 2 * t * one_minus_t * p1_y + \ + t * t * p2_y + + # B'(t) = 2(1-t)(P1-P0) + 2t(P2-P1) → 切線方向 + tdx = 2 * one_minus_t * (p1_x - p0_x) + 2 * t * (p2_x - p1_x) + tdy = 2 * one_minus_t * (p1_y - p0_y) + 2 * t * (p2_y - p1_y) + + # 正規化 + t_len = math.sqrt(tdx ** 2 + tdy ** 2) + if t_len > 0.01: + tdx /= t_len + tdy /= t_len + else: + tdx, tdy = d_in_x, d_in_y + + path.append((bx, by, tdx, tdy)) + + # 加入 post_turn 點 (方向 = outgoing) + path.append((p2_x, p2_y, d_out_x, d_out_y)) + + # 最後一個航點 + path.append((waypoints[-1][0], waypoints[-1][1], + segments[-1][0], segments[-1][1])) + + return path + + # ------------------------------------------------------------------ 柵狀掃描 + + def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params): + """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊""" + N = len(drone_positions) + line_spacing = params.get('line_spacing', 5.0) + altitude = params.get('altitude', self.base_altitude) + + c0, c1, c2, c3 = rect_corners_local + + edge_a = (c1[0] - c0[0], c1[1] - c0[1]) + len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2) + edge_b = (c3[0] - c0[0], c3[1] - c0[1]) + len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2) + + if len_a >= len_b: + sweep_vec = edge_a + sweep_len = len_a + cut_vec = edge_b + cut_len = len_b + sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2) + sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2) + cut_start_corner = c0 + else: + sweep_vec = edge_b + sweep_len = len_b + cut_vec = edge_a + cut_len = len_a + sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2) + sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2) + cut_start_corner = c0 + + sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len) + cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len) + + C_x = sum(p[0] for p in drone_positions) / N + C_y = sum(p[1] for p in drone_positions) / N + + dist_to_start = math.sqrt( + (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2 + ) + dist_to_end = math.sqrt( + (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2 + ) + + if dist_to_start <= dist_to_end: + near_corner_a = cut_start_corner + else: + sweep_dir = (-sweep_dir[0], -sweep_dir[1]) + near_corner_a = (cut_start_corner[0] + sweep_vec[0], + cut_start_corner[1] + sweep_vec[1]) + + projections = [ + ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i) + for i, pos in enumerate(drone_positions) + ] + projections.sort() + + strip_width = cut_len / N + drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1] + near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1] + gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2 + + all_waypoints = [None] * N + + for rank, (_, original_idx) in enumerate(projections): + strip_center_offset = (rank + 0.5) * strip_width + base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset + base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset + + waypoints_list = [] + + gather_offset = gather_sweep_proj - near_sweep_proj + gx = base_x + sweep_dir[0] * gather_offset + gy = base_y + sweep_dir[1] * gather_offset + waypoints_list.append((gx, gy, altitude)) + + num_lines = max(1, int(strip_width / line_spacing)) + actual_spacing = strip_width / num_lines + first_line_offset = (rank * strip_width) + actual_spacing / 2 + + entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset + entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset + waypoints_list.append((entry_x, entry_y, altitude)) + + current_cut_offset = first_line_offset + + for line_idx in range(num_lines): + line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset + line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset + line_far_x = line_near_x + sweep_dir[0] * sweep_len + line_far_y = line_near_y + sweep_dir[1] * sweep_len + + if line_idx % 2 == 0: + waypoints_list.append((line_far_x, line_far_y, altitude)) + else: + waypoints_list.append((line_near_x, line_near_y, altitude)) + + if line_idx < num_lines - 1: + next_cut_offset = current_cut_offset + actual_spacing + next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset + next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset + next_far_x = next_near_x + sweep_dir[0] * sweep_len + next_far_y = next_near_y + sweep_dir[1] * sweep_len + + if line_idx % 2 == 0: + waypoints_list.append((next_far_x, next_far_y, altitude)) + else: + waypoints_list.append((next_near_x, next_near_y, altitude)) + current_cut_offset = next_cut_offset + + all_waypoints[original_idx] = waypoints_list + + return all_waypoints + + +# ================================================================================ +# 測試 +# ================================================================================ +if __name__ == "__main__": + planner = FormationPlanner() + + drones = [ + (24.123450, 120.567800, 0.0), + (24.123470, 120.567820, 0.0), + (24.123440, 120.567810, 0.0), + (24.123460, 120.567830, 0.0), + ] + target = (24.12400, 120.56795, 10.0) + + # M-Formation + wps, origin = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) + print("M-Formation:") + for i, wp_list in enumerate(wps): + print(f" Drone {i}: {len(wp_list)} waypoints") + + # Leader-Follower (Bezier 轉彎) + route = [ + (24.12345, 120.56780), + (24.12370, 120.56800), + (24.12390, 120.56820), + (24.12400, 120.56800), + (24.12420, 120.56790), + ] + wps_lf, origin_lf = planner.plan_formation_mission( + drones, target, MissionType.LEADER_FOLLOWER, + params={ + 'route_waypoints': route, + 'lateral_offset': 3.0, + 'longitudinal_spacing': 5.0, + 'turn_margin': 0.35, + 'curve_resolution': 8, + 'altitude': 10.0 + } + ) + print(f"\nLeader-Follower (Bezier turns):") + for i, wp_list in enumerate(wps_lf): + print(f" Drone {i}: {len(wp_list)} waypoints") + for j, wp in enumerate(wp_list): + print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})") + + # Grid Sweep + rect = [ + (24.1237, 120.5679), + (24.1237, 120.5683), + (24.1240, 120.5683), + (24.1240, 120.5679) + ] + wps2, origin2 = planner.plan_formation_mission( + drones, target, MissionType.GRID_SWEEP, + params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0} + ) + print(f"\nGrid Sweep:") + for i, wp_list in enumerate(wps2): + print(f" Drone {i}: {len(wp_list)} waypoints") \ No newline at end of file diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py new file mode 100644 index 0000000..2715539 --- /dev/null +++ b/src/GUI/overview_table.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +from PyQt6.QtWidgets import QTableWidget, QTableWidgetItem, QHeaderView, QLabel +from PyQt6.QtCore import Qt + +class OverviewTable(QTableWidget): + """總覽表格，顯示所有無人機的狀態資訊""" + + # 默認的資訊類型和映射 + DEFAULT_INFO_TYPES = ["模式", "ARM", "電壓", "經度", "緯度", "高度", "位置", "速度", "地速", "航向", + "空速", "油門", "HUD ALT", "爬升率", "Roll", "Pitch", "Yaw", "丟包", "延遲"] + + DEFAULT_INFO_TYPE_MAP = { + "mode": 0, + "armed": 1, + "battery": 2, + "longitude": 3, + "latitude": 4, + "altitude": 5, + "local": 6, + "velocity": 7, + "groundspeed": 8, + "heading": 9, + "airspeed": 10, + "throttle": 11, + "hud_alt": 12, + "climb": 13, + "roll": 14, + "pitch": 15, + "yaw": 16, + "loss_rate": 17, + "ping": 18 + } + + def __init__(self, info_types=None, info_type_map=None, parent=None): + super().__init__(parent) + + # 使用提供的或默認的資訊類型 + self.info_types = info_types if info_types is not None else self.DEFAULT_INFO_TYPES + self.info_type_map = info_type_map if info_type_map is not None else self.DEFAULT_INFO_TYPE_MAP + self.drones = {} # 存儲無人機面板的引用 + + # 初始化表格 + self.setColumnCount(1) + self.setRowCount(len(self.info_types)) + self.setHorizontalHeaderLabels(["資訊"]) + header = self.horizontalHeader() + header.setSectionResizeMode(0, QHeaderView.ResizeMode.ResizeToContents) + self.verticalHeader().setVisible(False) + + # 設置第一列的資訊類型 + for i, txt in enumerate(self.info_types): + item = QTableWidgetItem(txt) + item.setFlags(Qt.ItemFlag.ItemIsEnabled) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(i, 0, item) + + def set_drones(self, drones): + """設置無人機面板字典的引用""" + self.drones = drones + + def update_table(self, drone_id=None, field=None, value=None): + """更新總覽表格 + + Args: + drone_id: 無人機 ID + field: 欄位名稱 (如 'mode', 'altitude' 等) + value: 要更新的值 + """ + # 更新特定儲存格 + if drone_id and field and value: + if drone_id not in self.drones: + return + + col = 1 + list(self.drones.keys()).index(drone_id) + row = self.info_type_map.get(field, -1) + + if row == -1: + return # 無效的欄位 + + item = self.item(row, col) + if not item: + item = QTableWidgetItem() + self.setItem(row, col, item) + + item.setText(value) + item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + + # 如果沒有指定更新，刷新整個表格 + if drone_id is None: + self.refresh_all() + + def refresh_all(self): + """刷新整個表格""" + cols = 1 + len(self.drones) + self.setColumnCount(cols) + headers = ["資訊"] + list(self.drones.keys()) + self.setHorizontalHeaderLabels(headers) + + for col, did in enumerate(self.drones, start=1): + panel = self.drones[did] + for field, row in self.info_type_map.items(): + lbl = panel.findChild(QLabel, f"{did}_{field}") + val = lbl.text() if lbl else "--" + val_item = QTableWidgetItem(val) + val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) + self.setItem(row, col, val_item) + + def add_drone_column(self, drone_id): + """當新增無人機時，添加一列""" + if drone_id in self.drones: + self.refresh_all() + + def remove_drone_column(self, drone_id): + """當移除無人機時，刷新表格""" + if drone_id in self.drones: + self.refresh_all() diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py new file mode 100644 index 0000000..22b6e22 --- /dev/null +++ b/src/GUI/validation/test_mission.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python3 +""" +獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作 + +使用方式: + 1. 啟動 SITL + 2. 修改下方 CONFIG 區塊 + 3. python3 test_mission.py +""" +import sys, os +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) + +import time +from pymavlink import mavutil +from PyQt6.QtWidgets import QApplication +from PyQt6.QtCore import QTimer + +from mission_planner import FormationPlanner, MissionType +from command_sender import MavlinkSender +from mission_executor import MissionExecutor, MissionState + + +# ================================================================================ +# CONFIG +# ================================================================================ + +# 接收用連線 (讀取無人機狀態) +RECV_CONNECTION = "udp:127.0.0.1:14550" + +# 發送用連線 (發送 setpoint 指令) +SEND_CONNECTION = "udpout:127.0.0.1:14550" + +# 要控制的無人機 sysid 列表 +DRONE_SYSIDS = [1] + +# 起飛高度 (公尺) +TAKEOFF_ALT = 10.0 + +# 任務規劃參數 +FORMATION_SPACING = 5.0 +BASE_ALTITUDE = 10.0 +ALTITUDE_DIFF = 2.0 +ARRIVAL_RADIUS = 2.0 + +# 測試模式: "formation" 或 "grid_sweep" +TEST_MODE = "formation" + +# Grid Sweep 專用設定 +GRID_LINE_SPACING = 5.0 + +# ================================================================================ + + +class SITLDroneManager: + """管理 SITL 無人機的連線、起飛前置作業""" + + def __init__(self, connection_string, sysids): + self.connection_string = connection_string + self.sysids = sysids + self.mav = None + self.drone_gps = {} + + def connect(self): + """建立 MAVLink 連線並等待心跳""" + print(f"連線到 {self.connection_string} ...") + self.mav = mavutil.mavlink_connection(self.connection_string) + self.mav.wait_heartbeat() + print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}") + + def set_guided_and_arm(self, sysid): + """切換到 GUIDED 模式並解鎖""" + print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---") + + # 切換 GUIDED 模式 + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_DO_SET_MODE, + 0, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 4, # GUIDED = 4 + 0, 0, 0, 0, 0 + ) + time.sleep(1) + + # 解鎖 + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1, 0, 0, 0, 0, 0, 0 + ) + time.sleep(1) + print(f" sysid={sysid}: GUIDED + ARMED") + + def takeoff(self, sysid, altitude): + """起飛到指定高度""" + print(f" sysid={sysid}: 起飛到 {altitude}m ...") + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, 0, 0, 0, 0, 0, 0, altitude + ) + + def wait_for_altitude(self, sysid, target_alt, timeout=30): + """等待無人機到達指定高度""" + print(f" sysid={sysid}: 等待到達 {target_alt}m ...") + start = time.time() + while time.time() - start < timeout: + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if msg and msg.get_srcSystem() == sysid: + alt = msg.relative_alt / 1000.0 + if alt >= target_alt * 0.9: + print(f" sysid={sysid}: 已到達 {alt:.1f}m") + return True + print(f" sysid={sysid}: 等待超時!") + return False + + def update_gps_once(self): + """讀取一輪 GPS 資料更新 drone_gps""" + deadline = time.time() + 3 + received = set() + while time.time() < deadline and len(received) < len(self.sysids): + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if msg is None: + continue + sid = msg.get_srcSystem() + if sid in self.sysids: + drone_id = f"s0_{sid}" + self.drone_gps[drone_id] = { + 'lat': msg.lat / 1e7, + 'lon': msg.lon / 1e7, + 'alt': msg.relative_alt / 1000.0 + } + received.add(sid) + + for sid in self.sysids: + drone_id = f"s0_{sid}" + if drone_id in self.drone_gps: + gps = self.drone_gps[drone_id] + print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)") + else: + print(f" {drone_id}: 尚未收到 GPS") + + def start_gps_polling(self, interval_ms=200): + """啟動定時 GPS 輪詢 (用 QTimer)""" + self._gps_timer = QTimer() + self._gps_timer.timeout.connect(self._poll_gps) + self._gps_timer.start(interval_ms) + + def _poll_gps(self): + """非阻塞方式讀取最新 GPS""" + while True: + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False) + if msg is None: + break + sid = msg.get_srcSystem() + if sid in self.sysids: + drone_id = f"s0_{sid}" + self.drone_gps[drone_id] = { + 'lat': msg.lat / 1e7, + 'lon': msg.lon / 1e7, + 'alt': msg.relative_alt / 1000.0 + } + + +def main(): + # 建立 Qt 應用 (MissionExecutor 需要 QTimer) + app = QApplication(sys.argv) + + # 連線 + 起飛前置作業 + manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS) + manager.connect() + + for sysid in DRONE_SYSIDS: + manager.set_guided_and_arm(sysid) + manager.takeoff(sysid, TAKEOFF_ALT) + + # 等待所有無人機到達起飛高度 + for sysid in DRONE_SYSIDS: + manager.wait_for_altitude(sysid, TAKEOFF_ALT) + + time.sleep(2) + + # 讀取當前 GPS 位置 + print("\n讀取當前 GPS 位置 ...") + manager.update_gps_once() + + drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS] + drone_gps_positions = [] + for drone_id in drone_ids: + gps = manager.drone_gps.get(drone_id) + if gps is None: + print(f"錯誤: 讀不到 {drone_id} 的 GPS") + return + drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt'])) + + # 規劃任務 + print(f"\n規劃任務 (模式: {TEST_MODE}) ...") + planner = FormationPlanner( + spacing=FORMATION_SPACING, + base_altitude=BASE_ALTITUDE, + altitude_diff=ALTITUDE_DIFF + ) + + center_lat = drone_gps_positions[0][0] + center_lon = drone_gps_positions[0][1] + + if TEST_MODE == "formation": + target_lat = center_lat + 30.0 / 111000.0 + target_lon = center_lon + target_gps = (target_lat, target_lon, BASE_ALTITUDE) + print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})") + + waypoints_per_drone, origin = planner.plan_formation_mission( + drone_gps_positions, target_gps, MissionType.M_FORMATION + ) + + elif TEST_MODE == "grid_sweep": + # 在無人機前方 30m 處建立 40m x 30m 的矩形 + offset_lat = 30.0 / 111000.0 + half_w = 20.0 / (111000.0 * 0.9) + half_h = 15.0 / 111000.0 + + rect_center_lat = center_lat + offset_lat + rect_center_lon = center_lon + + rect_corners = [ + (rect_center_lat - half_h, rect_center_lon - half_w), + (rect_center_lat - half_h, rect_center_lon + half_w), + (rect_center_lat + half_h, rect_center_lon + half_w), + (rect_center_lat + half_h, rect_center_lon - half_w), + ] + target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE) + print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})") + + waypoints_per_drone, origin = planner.plan_formation_mission( + drone_gps_positions, target_gps, MissionType.GRID_SWEEP, + params={ + 'rect_corners': rect_corners, + 'line_spacing': GRID_LINE_SPACING, + 'altitude': BASE_ALTITUDE + } + ) + else: + print(f"未知測試模式: {TEST_MODE}") + return + + planned_waypoints = { + 'drone_ids': drone_ids, + 'waypoints': waypoints_per_drone + } + + # 印出規劃結果 + for i, did in enumerate(drone_ids): + wps = waypoints_per_drone[i] + print(f" {did}: {len(wps)} 個航點") + for j, wp in enumerate(wps): + print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)") + + # 啟動任務 + print("\n啟動任務 ...") + manager.start_gps_polling(interval_ms=200) + + sender = MavlinkSender(SEND_CONNECTION) + executor = MissionExecutor( + sender=sender, + drone_gps=manager.drone_gps, + arrival_radius=ARRIVAL_RADIUS, + send_rate_hz=2.0 + ) + + executor.drone_waypoint_reached.connect( + lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}") + ) + executor.mission_completed.connect( + lambda: (print("\n===== 任務全部完成 ====="), app.quit()) + ) + + # 設定超時自動退出 + timeout_timer = QTimer() + timeout_timer.setSingleShot(True) + timeout_timer.timeout.connect(lambda: ( + print("\n⚠ 測試超時，強制退出"), + executor.stop(), + app.quit() + )) + timeout_timer.start(180_000) # 180 秒超時 + + executor.start(planned_waypoints) + + print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n") + app.exec() + + executor.stop() + sender.close() + print("測試結束") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/GUI/validation/verify_waypoints.py b/src/GUI/validation/verify_waypoints.py new file mode 100644 index 0000000..fe2d855 --- /dev/null +++ b/src/GUI/validation/verify_waypoints.py @@ -0,0 +1,482 @@ +#!/usr/bin/env python3 +""" +任務規劃視覺化驗證工具（含動畫模擬） + +使用方式: + 1. 由 GUI 自動觸發: python3 verify_waypoints.py --file /tmp/mission_plan.json + 2. 獨立手動執行: python3 verify_waypoints.py + +操作: + - 啟動後先顯示靜態航點圖 + - 點擊圖下方的按鈕控制動畫 +""" +import sys, os +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) + +import json +import argparse +import math +import matplotlib +matplotlib.use('TkAgg') +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from matplotlib.widgets import Button +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +from mission_planner import FormationPlanner, MissionType, CoordinateConverter + + +# ================================================================================ +# 色彩定義 +# ================================================================================ +COLORS = ['#378ADD', '#1D9E75', '#BA7517', '#D85A30', '#7F77DD', '#D4537E', + '#E24B4A', '#639922', '#00BFFF', '#FF69B4'] + +# 動畫參數 +FRAMES_PER_SEGMENT = 40 # 每段航點之間的畫面數 +TRAIL_LENGTH = 60 # 拖尾長度（畫面數） +INTERVAL_MS = 50 # 每幀間隔 (ms) + + +# ================================================================================ +# 靜態繪圖 +# ================================================================================ + +def plot_grid_sweep(ax, data, conv): + """畫 Grid Sweep 俯視圖""" + if 'rect_corners' in data: + rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']] + xs = [p[0] for p in rect_local] + [rect_local[0][0]] + ys = [p[1] for p in rect_local] + [rect_local[0][1]] + ax.plot(xs, ys, 'k--', linewidth=1.5, label='Task area') + ax.fill(xs, ys, alpha=0.05, color='gray') + + _draw_waypoint_paths(ax, data, conv, show_sweep_labels=True) + + total_wps = sum(len(wps) for wps in data['waypoints']) + ax.set_title(f'Grid Sweep - {len(data["drone_ids"])} drones, {total_wps} waypoints') + ax.set_xlabel('East (m)') + ax.set_ylabel('North (m)') + ax.set_aspect('equal') + ax.grid(True, alpha=0.3) + + +def plot_formation(ax, data, conv): + """畫 M-Formation / Circle / Leader-Follower 俯視圖""" + _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False) + + if 'target' in data: + t = data['target'] + tx, ty, _ = conv.gps_to_local(t[0], t[1], t[2] if len(t) > 2 else 0) + ax.plot(tx, ty, 'r*', markersize=18, zorder=5) + ax.annotate('Target', (tx, ty + 1), fontsize=9, color='red', ha='center', va='bottom') + + if 'route_waypoints' in data: + route_local = [conv.gps_to_local(wp[0], wp[1], 0)[:2] for wp in data['route_waypoints']] + rxs = [p[0] for p in route_local] + rys = [p[1] for p in route_local] + ax.plot(rxs, rys, 'r--', linewidth=1.5, alpha=0.5, label='Route center') + for i, (rx, ry) in enumerate(route_local): + ax.plot(rx, ry, 'ro', markersize=6, alpha=0.5) + ax.annotate(f'R{i+1}', (rx, ry + 0.8), fontsize=7, color='red', + ha='center', va='bottom', alpha=0.7) + + mission_type = data.get('mission_type', 'formation') + total_wps = sum(len(wps) for wps in data['waypoints']) + ax.set_title(f'{mission_type} - {len(data["drone_ids"])} drones, {total_wps} waypoints') + ax.set_xlabel('East (m)') + ax.set_ylabel('North (m)') + ax.set_aspect('equal') + ax.grid(True, alpha=0.3) + + +def _draw_waypoint_paths(ax, data, conv, show_sweep_labels=False): + """共用的航點路徑繪圖""" + drone_ids = data['drone_ids'] + waypoints = data['waypoints'] + drones_gps = data.get('drones_gps', []) + + for i, pos in enumerate(drones_gps): + x, y, _ = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0) + c = COLORS[i % len(COLORS)] + ax.plot(x, y, 'o', color=c, markersize=10, zorder=5) + ax.annotate(f'{drone_ids[i]}', (x, y + 1), fontsize=8, fontweight='bold', + ha='center', va='bottom', color=c) + + for i, wps in enumerate(waypoints): + c = COLORS[i % len(COLORS)] + local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps] + xs = [p[0] for p in local_wps] + ys = [p[1] for p in local_wps] + ax.plot(xs, ys, '-', color=c, linewidth=1.5, alpha=0.7) + + for j, (x, y, z) in enumerate(local_wps): + if show_sweep_labels: + if j == 0: + ax.plot(x, y, 's', color=c, markersize=8, zorder=4) + ax.annotate('gather', (x, y), fontsize=6, ha='right', va='top') + elif j == 1: + ax.plot(x, y, '^', color=c, markersize=8, zorder=4) + ax.annotate('entry', (x, y), fontsize=6, ha='right', va='top') + else: + ax.plot(x, y, '.', color=c, markersize=4) + else: + marker = 's' if j == 0 else '*' + ax.plot(x, y, marker, color=c, markersize=10, zorder=4) + ax.annotate(f'WP{j}\n({z:.0f}m)', (x, y), fontsize=6, ha='center', va='bottom') + + if local_wps: + lx, ly, _ = local_wps[-1] + ax.plot(lx, ly, 'X', color=c, markersize=10, markeredgewidth=2, zorder=4) + + +# ================================================================================ +# 動畫模擬 +# ================================================================================ + +class MissionAnimator: + """任務動畫控制器""" + + def __init__(self, fig, ax, data, conv): + self.fig = fig + self.ax = ax + self.data = data + self.conv = conv + self.is_playing = False + self.anim = None + self.current_frame = 0 + + drone_ids = data['drone_ids'] + waypoints = data['waypoints'] + drones_gps = data.get('drones_gps', []) + self.num_drones = len(drone_ids) + + # 建立完整航點序列：初始位置 → WP0 → WP1 → ... + self.all_local_wps = [] + for i, wps in enumerate(waypoints): + local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps] + + # 把初始位置插入最前面 + if i < len(drones_gps): + pos = drones_gps[i] + start = conv.gps_to_local(pos[0], pos[1], pos[2] if len(pos) > 2 else 0) + local_wps.insert(0, start) + + self.all_local_wps.append(local_wps) + + # 計算最大航點段數 + self.max_segments = max(len(wps) - 1 for wps in self.all_local_wps) if self.all_local_wps else 0 + self.total_frames = self.max_segments * FRAMES_PER_SEGMENT + + # 預計算位置 + self.positions = self._precompute_positions() + + # 動畫元素 + self.drone_dots = [] + self.trail_lines = [] + self.trail_data = [[] for _ in range(self.num_drones)] + self.status_text = None + + def _precompute_positions(self): + """預計算所有幀的位置 — 等時間步進""" + positions = [] + + for frame in range(self.total_frames + 1): + seg_idx = frame // FRAMES_PER_SEGMENT + seg_progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT + + frame_positions = [] + for drone_idx in range(self.num_drones): + wps = self.all_local_wps[drone_idx] + num_segs = len(wps) - 1 + + if seg_idx >= num_segs: + frame_positions.append((wps[-1][0], wps[-1][1])) + else: + x0, y0, _ = wps[seg_idx] + x1, y1, _ = wps[seg_idx + 1] + x = x0 + (x1 - x0) * seg_progress + y = y0 + (y1 - y0) * seg_progress + frame_positions.append((x, y)) + + positions.append(frame_positions) + + return positions + + def setup(self): + """建立動畫元素和按鈕""" + for i in range(self.num_drones): + c = COLORS[i % len(COLORS)] + dot, = self.ax.plot([], [], 'o', color=c, markersize=12, + markeredgecolor='white', markeredgewidth=1.5, zorder=10) + self.drone_dots.append(dot) + trail, = self.ax.plot([], [], '-', color=c, linewidth=2.5, alpha=0.4, zorder=9) + self.trail_lines.append(trail) + + self.status_text = self.ax.text( + 0.02, 0.98, 'Ready', + transform=self.ax.transAxes, fontsize=10, + verticalalignment='top', + bbox=dict(boxstyle='round,pad=0.3', facecolor='white', alpha=0.8) + ) + + # 按鈕放在右上角圖內，避免擋到軸標籤 + ax_play = self.fig.add_axes([0.72, 0.91, 0.08, 0.035]) + self.btn_play = Button(ax_play, 'Play', color='#4CAF50', hovercolor='#66BB6A') + self.btn_play.label.set_color('white') + self.btn_play.label.set_fontweight('bold') + self.btn_play.on_clicked(self._on_play) + + ax_pause = self.fig.add_axes([0.81, 0.91, 0.08, 0.035]) + self.btn_pause = Button(ax_pause, 'Pause', color='#FF9800', hovercolor='#FFB74D') + self.btn_pause.label.set_color('white') + self.btn_pause.label.set_fontweight('bold') + self.btn_pause.on_clicked(self._on_pause) + + ax_reset = self.fig.add_axes([0.90, 0.91, 0.08, 0.035]) + self.btn_reset = Button(ax_reset, 'Reset', color='#F44336', hovercolor='#EF5350') + self.btn_reset.label.set_color('white') + self.btn_reset.label.set_fontweight('bold') + self.btn_reset.on_clicked(self._on_reset) + + def _on_play(self, event): + if self.is_playing: + return + if self.anim is None: + self.anim = animation.FuncAnimation( + self.fig, self._update_frame, + frames=range(self.current_frame, self.total_frames + 1), + interval=INTERVAL_MS, + blit=False, + repeat=False + ) + else: + self.anim.resume() + self.is_playing = True + self.fig.canvas.draw_idle() + + def _on_pause(self, event): + if not self.is_playing or self.anim is None: + return + self.anim.pause() + self.is_playing = False + self.status_text.set_text(f'Paused (frame {self.current_frame}/{self.total_frames})') + self.fig.canvas.draw_idle() + + def _on_reset(self, event): + if self.anim is not None: + self.anim.event_source.stop() + self.anim = None + self.is_playing = False + self.current_frame = 0 + self.trail_data = [[] for _ in range(self.num_drones)] + for dot in self.drone_dots: + dot.set_data([], []) + for trail in self.trail_lines: + trail.set_data([], []) + self.status_text.set_text('Ready') + self.fig.canvas.draw_idle() + + def _update_frame(self, frame): + self.current_frame = frame + + if frame >= len(self.positions): + self.is_playing = False + self.status_text.set_text('Done') + return self.drone_dots + self.trail_lines + [self.status_text] + + # seg_idx - 1 是因為第 0 段是 start→WP0 + seg_idx = frame // FRAMES_PER_SEGMENT + progress = (frame % FRAMES_PER_SEGMENT) / FRAMES_PER_SEGMENT + + # 顯示時把第 0 段標為 "Start -> WP0" + if seg_idx == 0: + label = f'Start -> WP0 Progress {progress:.0%}' + else: + label = f'WP{seg_idx-1} -> WP{seg_idx} Progress {progress:.0%}' + self.status_text.set_text( + f'{label} Frame {frame}/{self.total_frames}' + ) + + for i in range(self.num_drones): + x, y = self.positions[frame][i] + self.drone_dots[i].set_data([x], [y]) + + self.trail_data[i].append((x, y)) + if len(self.trail_data[i]) > TRAIL_LENGTH: + self.trail_data[i] = self.trail_data[i][-TRAIL_LENGTH:] + trail_x = [p[0] for p in self.trail_data[i]] + trail_y = [p[1] for p in self.trail_data[i]] + self.trail_lines[i].set_data(trail_x, trail_y) + + return self.drone_dots + self.trail_lines + [self.status_text] + + +# ================================================================================ +# 主流程 +# ================================================================================ + +def visualize_from_file(filepath): + """從 JSON 檔案讀取並視覺化""" + with open(filepath, 'r') as f: + data = json.load(f) + + origin = data['origin'] + conv = CoordinateConverter(origin[0], origin[1], 0) + mission_type = data.get('mission_type', 'formation') + is_sweep = mission_type == 'grid_sweep' + + fig, ax = plt.subplots(1, 1, figsize=(10, 8)) + fig.suptitle(f'Mission Verification - {mission_type}', fontsize=13, fontweight='bold') + + if is_sweep: + plot_grid_sweep(ax, data, conv) + else: + plot_formation(ax, data, conv) + + _print_summary(data) + + animator = MissionAnimator(fig, ax, data, conv) + animator.setup() + + plt.tight_layout(rect=[0, 0, 1, 0.95]) + plt.show() + + +def visualize_standalone(): + """獨立執行：使用內建測試資料""" + drones = [ + (24.123450, 120.567800, 0.0), + (24.123470, 120.567820, 0.0), + (24.123440, 120.567810, 0.0), + (24.123460, 120.567830, 0.0), + ] + rect_corners = [ + (24.12380, 120.56775), + (24.12380, 120.56810), + (24.12420, 120.56810), + (24.12420, 120.56775), + ] + target = (24.12400, 120.56795, 10.0) + + planner = FormationPlanner(spacing=5.0, base_altitude=10.0, altitude_diff=2.0) + + fig = plt.figure(figsize=(16, 10)) + fig.suptitle('Mission Planner Verification (standalone)', fontsize=14, fontweight='bold') + + # M-Formation + wps_m, origin_m = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION) + conv_m = CoordinateConverter(origin_m[0], origin_m[1], 0) + data_m = { + 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))], + 'waypoints': wps_m, + 'drones_gps': drones, + 'target': target, + 'mission_type': 'M_FORMATION' + } + ax1 = fig.add_subplot(2, 2, 1) + plot_formation(ax1, data_m, conv_m) + + # Grid Sweep 5m + target_gs = (sum(c[0] for c in rect_corners) / 4, + sum(c[1] for c in rect_corners) / 4, 10.0) + wps_g, origin_g = planner.plan_formation_mission( + drones, target_gs, MissionType.GRID_SWEEP, + params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': 10.0} + ) + conv_g = CoordinateConverter(origin_g[0], origin_g[1], 0) + data_g = { + 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))], + 'waypoints': wps_g, + 'drones_gps': drones, + 'rect_corners': rect_corners, + 'mission_type': 'grid_sweep' + } + ax2 = fig.add_subplot(2, 2, 2) + plot_grid_sweep(ax2, data_g, conv_g) + + # Leader-Follower + route = [ + (24.12360, 120.56780), + (24.12380, 120.56800), + (24.12400, 120.56820), + (24.12410, 120.56800), + (24.12420, 120.56790), + ] + wps_lf, origin_lf = planner.plan_formation_mission( + drones, target, MissionType.LEADER_FOLLOWER, + params={'route_waypoints': route, 'lateral_offset': 3.0, + 'longitudinal_spacing': 5.0, 'altitude': 10.0} + ) + conv_lf = CoordinateConverter(origin_lf[0], origin_lf[1], 0) + data_lf = { + 'drone_ids': [f's0_{i + 1}' for i in range(len(drones))], + 'waypoints': wps_lf, + 'drones_gps': drones, + 'route_waypoints': route, + 'target': target, + 'mission_type': 'LEADER_FOLLOWER' + } + ax3 = fig.add_subplot(2, 2, 3) + plot_formation(ax3, data_lf, conv_lf) + + # 3D + ax4 = fig.add_subplot(2, 2, 4, projection='3d') + _plot_3d(ax4, data_g, conv_g) + + plt.tight_layout() + plt.show() + + +def _plot_3d(ax, data, conv): + """3D 視角""" + if 'rect_corners' in data: + rect_local = [conv.gps_to_local(c[0], c[1], 0)[:2] for c in data['rect_corners']] + xs = [p[0] for p in rect_local] + [rect_local[0][0]] + ys = [p[1] for p in rect_local] + [rect_local[0][1]] + ax.plot(xs, ys, [0] * len(xs), 'k--', linewidth=1, alpha=0.4) + + for i, wps in enumerate(data['waypoints']): + c = COLORS[i % len(COLORS)] + local_wps = [conv.gps_to_local(wp[0], wp[1], wp[2]) for wp in wps] + xs = [p[0] for p in local_wps] + ys = [p[1] for p in local_wps] + zs = [p[2] for p in local_wps] + ax.plot(xs, ys, zs, '-', color=c, linewidth=1.5) + if local_wps: + ax.scatter(xs[0], ys[0], zs[0], color=c, s=50, marker='s') + ax.scatter(xs[-1], ys[-1], zs[-1], color=c, s=50, marker='X') + + ax.set_title('3D view') + ax.set_xlabel('East (m)') + ax.set_ylabel('North (m)') + ax.set_zlabel('Alt (m)') + + +def _print_summary(data): + """終端印出摘要""" + drone_ids = data['drone_ids'] + waypoints = data['waypoints'] + mission_type = data.get('mission_type', 'unknown') + print(f"\n{'=' * 50}") + print(f"Mission: {mission_type}") + print(f"Drones: {len(drone_ids)}") + print(f"{'=' * 50}") + for i, did in enumerate(drone_ids): + wps = waypoints[i] + print(f" {did}: {len(wps)} waypoints") + for j, wp in enumerate(wps): + print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)") + print(f"{'=' * 50}\n") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Mission waypoint visualizer') + parser.add_argument('--file', '-f', type=str, default=None, + help='JSON file from GUI (auto-generated)') + args = parser.parse_args() + + if args.file: + visualize_from_file(args.file) + else: + visualize_standalone() \ No newline at end of file From ad87eda1b4b3ada93142d7592c8e5cc76a017726 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 1 Apr 2026 14:48:46 +0800 Subject: [PATCH 120/146] Merge GUI 1.0.1 from ken --- src/GUI/BEFORE_AFTER_COMPARISON.md | 218 ----------------------------- src/GUI/IMPLEMENTATION_SUMMARY.md | 119 ---------------- src/GUI/PANEL_MAP_UPDATE.md | 189 ------------------------- src/GUI/THREAD_SAFETY.md | 218 ----------------------------- src/GUI/VERIFICATION_CHECKLIST.md | 171 ---------------------- 5 files changed, 915 deletions(-) delete mode 100644 src/GUI/BEFORE_AFTER_COMPARISON.md delete mode 100644 src/GUI/IMPLEMENTATION_SUMMARY.md delete mode 100644 src/GUI/PANEL_MAP_UPDATE.md delete mode 100644 src/GUI/THREAD_SAFETY.md delete mode 100644 src/GUI/VERIFICATION_CHECKLIST.md diff --git a/src/GUI/BEFORE_AFTER_COMPARISON.md b/src/GUI/BEFORE_AFTER_COMPARISON.md deleted file mode 100644 index c9657de..0000000 --- a/src/GUI/BEFORE_AFTER_COMPARISON.md +++ /dev/null @@ -1,218 +0,0 @@ -# 實現前後對比 - -## 系統架構變化 - -### 舊架構（直接更新） -``` -接收執行緒 - ↓ -monitor.latest_data - ↓ -spin_ros() [10ms] - ↓ -update_ui() [100% 直接更新 UI] - ├─ GPS → 立即更新地圖和表格 - ├─ HUD → 立即更新 Panel 和地圖 - ├─ State → 立即更新 State - ├─ Battery → 立即更新 Battery - └─ ... 等等 - -❌ 問題： - - Map 更新頻率太高，可能導致 CPU 過度使用 - - 高頻率的連續更新可能造成視覺閃爍 - - 沒有批次更新機制 -``` - -### 新架構（分層更新） -``` -接收執行緒 - ↓ -monitor.latest_data - ↓ -spin_ros() [10ms] - ↓ -update_ui() - ├─ GPS/HUD → 快取到 _message_cache - ├─ State → 立即更新 - ├─ Battery → 立即更新 - └─ ... 等等 - ↓ -_update_panel_and_map() [100ms / 10Hz] - ├─ 讀取 _message_cache GPS 資料 - │ └─ 更新地圖位置和表格 - ├─ 讀取 _message_cache HUD 資料 - │ └─ 更新 Panel 和地圖方向 - └─ 使用上次快取值（如無新消息） - -✅ 優勢： - - Map 和 Panel 只在 10Hz 更新，降低 CPU 負荷 - - 批次更新確保原子性 - - 消息未更新時使用上次值，資訊連續性好 - - 分層設計允許不同組件不同更新率 -``` - -## 代碼實現對比 - -### 舊代碼片段（GPS 更新） -```python -def update_ui(self, msg_type, drone_id, data): - ... - elif msg_type == 'gps': - lat, lon = data.get('lat', 0), data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - alt = data.get('alt', 0) - if not hasattr(self.monitor, 'drone_gps'): - self.monitor.drone_gps = {} - self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt} - self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") - self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") - heading = self.drone_headings.get(drone_id, 0) - self.drone_map.update_drone_position(drone_id, lat, lon, heading) # ← 直接更新 Map -``` - -### 新代碼片段（GPS 快取） -```python -def update_ui(self, msg_type, drone_id, data): - ... - # 快取 GPS 和 HUD 資料用於 panel/map 的 10Hz 更新 - if msg_type in ('gps', 'hud'): - if drone_id not in self._message_cache: - self._message_cache[drone_id] = {} - self._message_cache[drone_id][msg_type] = data - # 不在這裡更新，等待 _update_panel_and_map 在 10Hz 執行 - return -``` - -### 新代碼片段（10Hz 批次更新） -```python -def _update_panel_and_map(self): - """10Hz 定時更新 panel 和 map，使用快取的 GPS 和 HUD 消息""" - for drone_id in list(self._message_cache.keys()): - cache = self._message_cache[drone_id] - - # 使用快取的 GPS 資料 - if 'gps' in cache: - gps_data = cache['gps'] - lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - # ... 更新表格 ... - - # 使用快取的 HUD 資料 - if 'hud' in cache: - # ... 更新 panel 和 map ... - self.drone_map.update_drone_position(drone_id, lat, lon, heading) # ← 10Hz 更新 -``` - -## 性能影響分析 - -### Map 更新頻率 - -| 場景 | 舊架構 | 新架構 | 改進 | -|------|--------|--------|------| -| 單架無人機 | ~50Hz | 10Hz | ↓ 80% | -| 5 架無人機 | ~250Hz | 10Hz | ↓ 96% | -| 10 架無人機 | ~500Hz | 10Hz | ↓ 98% | - -### 消息快取大小 - -``` -最大快取大小 = num_drones × 2 messages (gps + hud) - -例如：10 架無人機 - 最大快取: 10 × 2 = 20 個消息 - 內存使用: ~10KB (非常小) -``` - -## 延遲分析 - -### 位置更新延遲 - -``` -GPS 消息到達 - ↓ -快取到 _message_cache - ↓ -等待至多 100ms（下一個 10Hz 週期） - ↓ -_update_panel_and_map() 讀取並更新地圖 - ↓ -總延遲: 0-100ms - -用戶體驗：無可見延遲（100ms 對人眼不可察） -``` - -## 初始化檢查清單 - -在啟動 GUI 時確保： - -- [ ] `panel_map_timer` 已初始化並啟動 - ```python - self.panel_map_timer = QTimer() - self.panel_map_timer.timeout.connect(self._update_panel_and_map) - self.panel_map_timer.start(100) - ``` - -- [ ] `_message_cache` 已初始化為空字典 - ```python - self._message_cache = {} - ``` - -- [ ] `update_ui()` 正確快取 GPS/HUD 消息 - ```python - if msg_type in ('gps', 'hud'): - # 快取邏輯 - ``` - -- [ ] `_update_panel_and_map()` 方法存在且被連接 - ```python - self.panel_map_timer.timeout.connect(self._update_panel_and_map) - ``` - -## 監控 UI - -### 添加調試輸出（可選） - -在 `_update_panel_and_map()` 開始添加： - -```python -import time - -if not hasattr(self, '_debug_map_time'): - self._debug_map_time = time.time() - self._debug_map_count = 0 - -self._debug_map_count += 1 -if time.time() - self._debug_map_time >= 1.0: - cached_drones = len(self._message_cache) - updated_drones = sum(1 for d in self._message_cache.values() if d) - print(f"[10Hz] Cycle {self._debug_map_count} | " - f"Cached: {cached_drones} | " - f"Updated: {updated_drones}") - self._debug_map_time = time.time() - self._debug_map_count = 0 -``` - -## 故障診斷 - -如果地圖或 Panel 沒有更新： - -1. **檢查定時器是否運行** - ```python - print(f"Timer active: {self.panel_map_timer.isActive()}") - ``` - -2. **檢查快取是否有數據** - ```python - print(f"Cache: {self._message_cache}") - ``` - -3. **檢查方法是否被調用** - - 在 `_update_panel_and_map()` 開始添加 `print("_update_panel_and_map called")` - -4. **檢查 update_drone_position 是否有錯誤** - - 查看控制台輸出，是否有異常拋出 - ---- - -**版本**: 2025-03-25 -**狀態**: ✅ 生產就緒 diff --git a/src/GUI/IMPLEMENTATION_SUMMARY.md b/src/GUI/IMPLEMENTATION_SUMMARY.md deleted file mode 100644 index 4e32a63..0000000 --- a/src/GUI/IMPLEMENTATION_SUMMARY.md +++ /dev/null @@ -1,119 +0,0 @@ -# Panel 和 Map 10Hz 更新實現 - 完成總結 - -## 實現完成 ✅ - -已成功實現 **Panel（DronePanel）和 Map（DroneMap）的 10Hz 更新機制**，同時其他 UI 元素保持更快的更新速率。 - -## 關鍵改動 - -### 1. **初始化 10Hz 定時器** (`__init__`) -```python -# 初始化 panel 和 map 更新（10Hz） -self.panel_map_timer = QTimer() -self.panel_map_timer.timeout.connect(self._update_panel_and_map) -self.panel_map_timer.start(100) # 10Hz - -# 快取消息數據，以便在沒有新消息時使用上一次的值 -self._message_cache = {} -``` - -### 2. **快取 GPS 和 HUD 消息** (`update_ui`) -```python -# 快取 GPS 和 HUD 資料用於 panel/map 的 10Hz 更新 -if msg_type in ('gps', 'hud'): - if drone_id not in self._message_cache: - self._message_cache[drone_id] = {} - self._message_cache[drone_id][msg_type] = data - # 不在這裡更新，等待 _update_panel_and_map 在 10Hz 執行 - return -``` - -### 3. **實現 10Hz 批次更新方法** (`_update_panel_and_map`) -- 每 100ms 執行一次 -- 讀取快取的 GPS 資料更新位置和表格 -- 讀取快取的 HUD 資料更新標題、速度、高度等 -- 更新 Panel 顯示 -- 更新地圖位置和無人機方向 -- 如果消息未更新，使用上一次快取的值 - -### 4. **移除舊的直接 GPS/HUD 更新** -- 從 `update_ui()` 中移除了 `msg_type == 'gps'` 的直接更新邏輯 -- 從 `update_ui()` 中移除了 `msg_type == 'hud'` 的直接更新邏輯 -- 這些操作現在由 `_update_panel_and_map()` 在 10Hz 執行 - -## 更新頻率 - -| 組件 | 頻率 | 說明 | -|------|------|------| -| GPS 位置 | 10Hz | 快取並批次更新 | -| HUD (標題、速度、高度) | 10Hz | 快取並批次更新 | -| Map 更新 | 10Hz | 隨 HUD/GPS 更新 | -| Panel 顯示 | 10Hz | 隨 HUD 更新 | -| State/Battery/Altitude | 即時 | 保持快速響應 | -| Loss Rate/Ping | 即時 | 保持快速響應 | - -## 數據持久性 - -當消息未被更新時，系統使用上一次的值： -- `_message_cache` 保留最後接收的 GPS 和 HUD 數據 -- 即使沒有新消息，`_update_panel_and_map()` 仍然會使用快取值執行更新 -- 確保 Panel 和 Map 始終顯示最新已知的無人機位置和姿態 - -## 檔案修改 - -### `/home/dodo/Downloads/AirTrapMine/src/GUI/gui.py` -- ✅ 添加 `panel_map_timer` 初始化 -- ✅ 添加 `_message_cache` 初始化 -- ✅ 修改 `update_ui()` 快取 GPS/HUD 消息 -- ✅ 添加 `_update_panel_and_map()` 方法 -- ✅ 移除舊的 GPS/HUD 直接更新邏輯 -- ✅ 所有語法檢查通過 ✓ - -## 文檔 - -### 新建文檔 -- `PANEL_MAP_UPDATE.md` - 詳細的 10Hz 更新機制說明、故障排除和監控指南 - -## 驗證 - -### 語法驗證 ✅ -```bash -$ python -m py_compile gui.py -✓ Syntax check passed -``` - -### 錯誤檢查 ✅ -``` -No errors found -``` - -## 下一步驗證 - -如果需要進一步驗證，可以在代碼中添加： - -```python -# 在 _update_panel_and_map() 中添加頻率監控 -import time - -if not hasattr(self, '_map_update_time'): - self._map_update_time = time.time() - self._map_update_count = 0 - -self._map_update_count += 1 -now = time.time() -if now - self._map_update_time >= 1.0: - print(f"[Panel/Map] Update frequency: {self._map_update_count} Hz") - self._map_update_time = now - self._map_update_count = 0 -``` - -## 性能預期 - -- **Map 和 Panel 的 CPU 使用**: 降低（從 ~100Hz 降至 10Hz） -- **用戶體驗**: 流暢，無可見延遲（100ms 最大延遲） -- **數據新鮮度**: 優秀（100ms 更新週期內最新值） - ---- - -**完成日期**: 2025-03-25 -**狀態**: ✅ 實現完成，語法驗證通過 diff --git a/src/GUI/PANEL_MAP_UPDATE.md b/src/GUI/PANEL_MAP_UPDATE.md deleted file mode 100644 index cad76c4..0000000 --- a/src/GUI/PANEL_MAP_UPDATE.md +++ /dev/null @@ -1,189 +0,0 @@ -# Panel 和 Map 10Hz 更新機制 - -## 概述 - -Panel（DronePanel）和 Map（DroneMap）的更新率已優化為 **10Hz（每 100ms 更新一次）**，同時其他 UI 元素保持更快的更新速率。這確保了地圖和面板在資訊流量大時不會過度刷新，同時保持流暢的用戶體驗。 - -## 架構 - -### 資料流 - -``` -接收執行緒 (高頻) - ↓ -monitor.latest_data - ↓ -spin_ros() [10ms] - 發送信號 - ↓ -update_ui() [快速更新] - ├─ State, Battery, Altitude, etc. → 直接更新 (快速) - └─ GPS, HUD → 快取到 _message_cache - ↓ -_update_panel_and_map() [100ms / 10Hz] - ├─ 讀取 _message_cache 的 GPS 資料 - │ └─ 更新經緯度表格 - ├─ 讀取 _message_cache 的 HUD 資料 - │ ├─ 更新標題、速度、高度等 - │ └─ 更新 Panel 顯示 - └─ 更新地圖位置和無人機方向 -``` - -### 關鍵特性 - -1. **消息快取 (`_message_cache`)** - - GPS 和 HUD 消息被快取而不是立即處理 - - 如果在 10Hz 更新週期內沒有收到新消息，使用上一次的值 - - 避免因快速連續的消息導致過度刷新 - -2. **分層更新** - - **快速更新** (on-demand): State, Battery, Altitude, Loss Rate, Ping 等 - - **10Hz 更新**: GPS 位置, HUD（標題、速度、高度、爬升率）, Panel 和 Map - -3. **定時器機制** - ```python - # 10Hz 定時器 - self.panel_map_timer = QTimer() - self.panel_map_timer.timeout.connect(self._update_panel_and_map) - self.panel_map_timer.start(100) # 100ms = 10Hz - ``` - -## 實現細節 - -### 步驟 1: 快取消息 - -在 `update_ui()` 中，GPS 和 HUD 消息被快取： - -```python -# 快取 GPS 和 HUD 資料用於 panel/map 的 10Hz 更新 -if msg_type in ('gps', 'hud'): - if drone_id not in self._message_cache: - self._message_cache[drone_id] = {} - self._message_cache[drone_id][msg_type] = data - # 不在這裡更新，等待 _update_panel_and_map 在 10Hz 執行 - return -``` - -### 步驟 2: 10Hz 批次更新 - -每 100ms，`_update_panel_and_map()` 被調用： - -```python -def _update_panel_and_map(self): - """10Hz 定時更新 panel 和 map，使用快取的 GPS 和 HUD 消息""" - for drone_id in list(self._message_cache.keys()): - cache = self._message_cache[drone_id] - - # 使用快取的 GPS 資料 - if 'gps' in cache: - gps_data = cache['gps'] - lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - # ... 更新表格 - - # 使用快取的 HUD 資料 - if 'hud' in cache: - hud_data = cache['hud'] - heading = hud_data.get('heading', 0) - # ... 更新 panel 和 map - self.drone_map.update_drone_position(drone_id, lat, lon, heading) -``` - -### 步驟 3: 持久化數據 - -即使沒有新消息，先前快取的值仍然存在於 `_message_cache` 中，所以 `_update_panel_and_map()` 將在下一個 10Hz 周期使用它： - -```python -# 第一個週期：GPS 消息到達 -_message_cache = { - 'drone_0': {'gps': {'lat': 23.123, 'lon': 120.456, ...}} -} - -# 第二個週期：沒有新 GPS 消息，但仍使用前一個值 -_update_panel_and_map() 會使用 'drone_0' 的上一個 GPS 位置 -``` - -## 性能影響 - -### 優點 -- **降低 Map 更新頻率**: 避免過度繪製導致 CPU 負荷 -- **更流暢的 UI**: 批次更新減少了視覺閃爍 -- **減少同步開銷**: 地圖位置和面板資訊一起批次更新 - -### 更新頻率對比 - -| 組件 | 舊速率 | 新速率 | 說明 | -|------|--------|--------|------| -| State/Battery/Altitude | 即時 | 即時 | 保持快速響應 | -| GPS/HUD 消息 | 即時 | 10Hz | 快取並批次更新 | -| Map 更新 | 即時 | 10Hz | 隨 HUD 更新 | -| Panel 顯示 | 即時 | 10Hz | 隨 HUD 更新 | - -## 故障排除 - -### Panel 或 Map 沒有更新 -1. 檢查 `panel_map_timer` 是否啟動 - ```python - print(self.panel_map_timer.isActive()) # 應該是 True - ``` - -2. 驗證 `_update_panel_and_map()` 是否被調用 - - 在方法開始添加 `print(f"Panel/Map update: {len(self._message_cache)} drones")` - -3. 檢查快取是否有數據 - ```python - print(self._message_cache) # 應該看到 drone_id 和消息 - ``` - -### 數據延遲或重複 - -如果看到數據延遲（最多 100ms），這是正常的。這就是為什麼我們使用快取 - 確保最新值始終可用。 - -如果看到重複更新，檢查是否有多個地方在調用 `update_drone_position()`。 - -## 初始化 - -在 `__init__` 中添加： - -```python -# 初始化 panel 和 map 更新（10Hz） -self.panel_map_timer = QTimer() -self.panel_map_timer.timeout.connect(self._update_panel_and_map) -self.panel_map_timer.start(100) # 10Hz - -# 快取消息數據，以便在沒有新消息時使用上一次的值 -self._message_cache = {} -``` - -## 監控與調試 - -### 列印更新頻率 - -添加到 `_update_panel_and_map()`： - -```python -def _update_panel_and_map(self): - if not hasattr(self, '_map_update_count'): - self._map_update_count = 0 - self._map_update_time = time.time() - - self._map_update_count += 1 - if time.time() - self._map_update_time >= 1.0: - print(f"Panel/Map update frequency: {self._map_update_count} Hz") - self._map_update_count = 0 - self._map_update_time = time.time() - - # ... 其餘代碼 -``` - -### 快取大小監控 - -```python -print(f"Cache size: {len(self._message_cache)} drones") -for drone_id, cache in self._message_cache.items(): - print(f" {drone_id}: {list(cache.keys())}") -``` - ---- - -**更新日期**: 2025-03-25 -**版本**: Panel/Map 10Hz 優化 v1.0 diff --git a/src/GUI/THREAD_SAFETY.md b/src/GUI/THREAD_SAFETY.md deleted file mode 100644 index 0b68514..0000000 --- a/src/GUI/THREAD_SAFETY.md +++ /dev/null @@ -1,218 +0,0 @@ -# 執行緒安全性實現 - GCS GUI - -## 架構概述 - -GCS GUI 採用 **拉取式 UI 更新架構** 以確保執行緒安全和高效的UI渲染，避免UI執行緒被資料收集堵塞。 - -## 執行緒模型 - -### 1. **主 GUI 執行緒** (Qt Event Loop) -- 負責所有 UI 操作（更新標籤、表格、地圖、ADI 等） -- 執行 `_process_ui_updates()` 每 33ms（30Hz） - -### 2. **ROS 執行緒** (Single-threaded Executor) -- 執行 `spin_ros()` 定時器每 10ms -- 只負責收集資料並快取，**不進行任何 UI 操作** - -### 3. **背景接收執行緒** (Receiver Threads) -- UDP/WebSocket/Serial 接收器在獨立執行緒上運行 -- 寫入 `monitor.latest_data` 字典（快取層） - -### 4. **任務執行執行緒** (Mission Executor) -- 運行在獨立執行緒上 -- 不直接訪問 UI 元素 - -## 資料流 - -``` -接收執行緒 - ↓ -monitor.latest_data (共享字典) - ↓ -spin_ros() [UI 執行緒, 10ms] - ↓ (快取資料，不更新 UI) -_ui_update_cache (字典) - ↓ -_process_ui_updates() [UI 執行緒, 30Hz] - ↓ (批次處理，更新 UI) -DronePanel, OverviewTable, DroneMap -``` - -## 執行緒安全機制 - -### 1. **_ui_cache_lock (布林鎖)** -```python -# 在 __init__ 中初始化 -self._ui_cache_lock = False -self._ui_update_cache = {} - -# 在 spin_ros() 中快取資料 -self._ui_update_cache[drone_id][msg_type] = data - -# 在 _process_ui_updates() 中保護快取訪問 -if self._ui_cache_lock or not self._ui_update_cache: - return -self._ui_cache_lock = True -try: - # 處理快取資料 - for drone_id in list(self._ui_update_cache.keys()): - ... -finally: - self._ui_cache_lock = False -``` - -### 2. **drone_positions 與 drone_headings 訪問** -- **寫入**: 在 `_update_gps_ui()` 和 `_update_hud_ui()` 中進行 - - 這些方法**只在 UI 執行緒**的 `_process_ui_updates()` 中調用 - -- **讀取**: 在 `_update_attitude_ui()` 和 `_update_hud_ui()` 中讀取 - - 這些方法**只在 UI 執行緒**的 `_process_ui_updates()` 中調用 - -```python -# 只在 UI 執行緒上讀寫 -heading = self.drone_headings.get(drone_id, 0) # 讀取 -self.drone_headings[drone_id] = heading # 寫入 -``` - -### 3. **資料同步屬性** - -| 屬性 | 寫入執行緒 | 讀取執行緒 | 保護機制 | -|------|----------|----------|--------| -| `_ui_update_cache` | spin_ros (UI) | _process_ui_updates (UI) | _ui_cache_lock | -| `monitor.latest_data` | 接收執行緒 | spin_ros (UI) | 直接讀取後清空 | -| `drone_positions` | _update_gps_ui (UI) | _update_hud_ui (UI) | 同一執行緒 | -| `drone_headings` | _update_hud_ui (UI) | _update_attitude_ui (UI) | 同一執行緒 | -| `self.drones[*]` | add_drone (UI) | 各 _update_*_ui (UI) | 同一執行緒 | - -## UI 更新流程 - -### 步驟 1: 資料收集 (spin_ros, 10ms) -```python -def spin_ros(self): - # 執行 ROS 收集資料 - self.executor.spin_once(timeout_sec=0.01) - - # 只快取資料，不更新 UI - for (drone_id, msg_type), data in self.monitor.latest_data.items(): - if drone_id not in self._ui_update_cache: - self._ui_update_cache[drone_id] = {} - self._ui_update_cache[drone_id][msg_type] = data - self.monitor.latest_data.clear() -``` - -### 步驟 2: 批次 UI 更新 (_process_ui_updates, 30Hz / 33ms) -```python -def _process_ui_updates(self): - # 檢查是否有資料且無鎖定 - if self._ui_cache_lock or not self._ui_update_cache: - return - - self._ui_cache_lock = True # 上鎖 - try: - # 批次處理各無人機的快取資料 - for drone_id in list(self._ui_update_cache.keys()): - data_dict = self._ui_update_cache.get(drone_id, {}) - - # 依訊息類型調用相應的 UI 更新方法 - if 'attitude' in data_dict: - self._update_attitude_ui(drone_id, data_dict['attitude']) - if 'hud' in data_dict: - self._update_hud_ui(drone_id, data_dict['hud']) - # ... 其他訊息類型 - - # 清空快取 - self._ui_update_cache.clear() - finally: - self._ui_cache_lock = False # 解鎖 -``` - -### 步驟 3: 各資料類型的 UI 更新 -```python -# 所有 _update_*_ui 方法都在 UI 執行緒上運行 -def _update_gps_ui(self, drone_id, data): - # 安全地寫入共享結構（同一執行緒） - self.drone_positions[drone_id] = (lat, lon) - - # 更新 UI 元素 - self.update_overview_table(drone_id, 'latitude', ...) - self.drone_map.update_drone_position(drone_id, lat, lon, heading) - -def _update_hud_ui(self, drone_id, data): - # 安全地寫入共享結構（同一執行緒） - self.drone_headings[drone_id] = heading - - # 讀取之前寫入的資料（同一執行緒，無競賽條件） - if drone_id in self.drone_positions: - lat, lon = self.drone_positions[drone_id] - self.drone_map.update_drone_position(drone_id, lat, lon, heading) -``` - -## 關鍵設計原則 - -### 1. **執行緒隔離** -- 所有 UI 操作都在主 GUI 執行緒上進行 -- ROS 資料收集隔離在單獨的定時器回調中 -- 背景執行緒只寫入快取，不觸及 UI - -### 2. **資料流向一致** -``` -背景執行緒 → monitor.latest_data - ↓ - 主 UI 執行緒 (spin_ros) - ↓ - _ui_update_cache - ↓ - 主 UI 執行緒 (_process_ui_updates) - ↓ - UI 元素 -``` - -### 3. **批次更新減少開銷** -- 不是每次收到訊息就更新 UI（造成主執行緒壓力） -- 而是批次收集 33ms 內的所有更新，一次性渲染 -- 結果：UI 流暢度提高，執行緒爭奪減少 - -### 4. **簡單的同步策略** -- 使用簡單的布林標誌而不是複雜的鎖 -- 避免死鎖，因為只有一個重要的臨界區 (`_ui_update_cache`) - -## 監控與驗證 - -### 態度指示器（ADI）更新頻率 -在 [drone_panel.py](drone_panel.py) 中的 `update_attitude()` 方法自動列印頻率： -``` -[drone_0] Attitude update frequency: 30.00 Hz -[drone_1] Attitude update frequency: 29.98 Hz -``` - -### 預期頻率 -- **資料收集**: 10ms = 100Hz（但不更新 UI） -- **UI 渲染**: 30Hz（批次模式） -- **ADI 更新**: 約 30Hz（受限於 `_process_ui_updates()` 頻率） - -## 故障排除 - -### 1. ADI 仍然更新頻率低 -- 檢查 `ui_update_timer` 是否啟動 -- 確認 `_process_ui_updates()` 正在被調用 -- 檢查是否有其他執行緒在嘗試直接更新 UI - -### 2. 地圖或表格更新時卡頓 -- 檢查 `drone_map.update_drone_position()` 和 `update_overview_table()` 是否有阻塞操作 -- 確認這些操作在 `_process_ui_updates()` 中被調用（主執行緒） - -### 3. 資料不同步 -- 驗證 `_ui_cache_lock` 是否正確保護快取訪問 -- 檢查是否有代碼在 spin_ros 之外寫入 `_ui_update_cache` - -## 測試建議 - -1. **頻率監控**: 執行應用程序並查看列印的 ADI 更新頻率 -2. **視覺檢查**: 觀察 ADI 指標的平滑性和應答性 -3. **壓力測試**: 同時連接多個無人機並監控 CPU 使用率 -4. **QDebug**: 在時間敏感的部分添加 `print()` 以測量執行時間 - ---- - -**更新日期**: 2025-01-20 -**版本**: 執行緒安全優化 v1.0 diff --git a/src/GUI/VERIFICATION_CHECKLIST.md b/src/GUI/VERIFICATION_CHECKLIST.md deleted file mode 100644 index 7c113c5..0000000 --- a/src/GUI/VERIFICATION_CHECKLIST.md +++ /dev/null @@ -1,171 +0,0 @@ -# 實現驗證清單 ✅ - -## 需求清單 - -- [x] **Panel 更新率設為 10Hz** - - 已在 `__init__` 初始化 `panel_map_timer` (100ms = 10Hz) - - 位置: [gui.py#L50-L52](gui.py#L50-L52) - -- [x] **Map 更新率設為 10Hz** - - 已在 `__init__` 初始化 `panel_map_timer` (100ms = 10Hz) - - 已移除直接更新地圖的舊代碼 - - 位置: [gui.py#L50-L52](gui.py#L50-L52) - -- [x] **消息未更新時讀取上一次的值** - - 已實現消息快取機制 (`_message_cache`) - - GPS 和 HUD 消息保留在快取中 - - 即使沒有新消息，舊值仍被使用 - - 位置: [gui.py#L56](gui.py#L56) - -## 代碼實現驗證 - -### 1. 初始化部分 ✅ -```python -# 初始化 panel 和 map 更新（10Hz） -self.panel_map_timer = QTimer() # ✓ 已添加 -self.panel_map_timer.timeout.connect(self._update_panel_and_map) # ✓ 已連接 -self.panel_map_timer.start(100) # 10Hz # ✓ 已設置 - -# 快取消息數據，以便在沒有新消息時使用上一次的值 -self._message_cache = {} # ✓ 已初始化 -``` - -### 2. 快取機制 ✅ -```python -# 快取 GPS 和 HUD 資料用於 panel/map 的 10Hz 更新 -if msg_type in ('gps', 'hud'): - if drone_id not in self._message_cache: - self._message_cache[drone_id] = {} - self._message_cache[drone_id][msg_type] = data - # 不在這裡更新，等待 _update_panel_and_map 在 10Hz 執行 - return -``` -- ✓ 檢查消息類型是否為 GPS 或 HUD -- ✓ 創建無人機快取字典 -- ✓ 保存消息數據 -- ✓ 返回而不直接更新 UI - -### 3. 10Hz 批次更新 ✅ -```python -def _update_panel_and_map(self): - """10Hz 定時更新 panel 和 map，使用快取的 GPS 和 HUD 消息""" - for drone_id in list(self._message_cache.keys()): - cache = self._message_cache[drone_id] - - # GPS 更新 - if 'gps' in cache: - gps_data = cache['gps'] - lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - # ... 更新表格 - - # HUD 更新 - if 'hud' in cache: - hud_data = cache['hud'] - heading = hud_data.get('heading', 0) - # ... 更新 panel 和 map - self.drone_map.update_drone_position(drone_id, lat, lon, heading) -``` -- ✓ 遍歷快取中的所有無人機 -- ✓ 檢查 GPS 消息並更新 -- ✓ 檢查 HUD 消息並更新 -- ✓ 使用快取值（即使未更新） - -### 4. 移除舊的直接更新 ✅ -- ✓ 移除了 `msg_type == 'gps'` 的舊代碼 -- ✓ 移除了 `msg_type == 'hud'` 的舊代碼 -- ✓ GPS 和 HUD 更新現在只通過 `_update_panel_and_map()` 進行 - -## 文件驗證 - -### gui.py -- [x] 語法檢查通過 ✅ -- [x] 無編譯錯誤 ✅ -- [x] 所有方法定義完整 ✅ -- [x] 所有引用方法存在 ✅ - -### 文檔 -- [x] PANEL_MAP_UPDATE.md - 10Hz 更新機制詳細說明 -- [x] IMPLEMENTATION_SUMMARY.md - 實現完成總結 -- [x] BEFORE_AFTER_COMPARISON.md - 架構對比 - -## 運行時驗證清單 - -當應用啟動時，檢查： - -### 應該看到的行為 - -1. **Panel 和 Map 更新平滑** - - 無人機位置在地圖上平滑移動（10Hz） - - Panel 顯示的標題、速度等流暢更新 - - 無視覺閃爍或抖動 - -2. **快取工作正常** - - 即使停止 GPS 消息，地圖上的位置仍保持最後已知位置 - - Panel 顯示的值保留最後已知值 - - 當消息恢復時，立即反映新值 - -3. **其他 UI 保持快速** - - State（ARMED/DISARMED）即時更新 - - Battery 電壓即時更新 - - Loss Rate 即時更新 - - Ping 即時更新 - -4. **無性能問題** - - CPU 使用率合理（相比之前應該更低） - - 無內存洩漏 - - GUI 響應靈敏 - -### 故障排除 - -| 症狀 | 原因 | 檢查項目 | -|------|------|--------| -| Panel/Map 不更新 | 定時器未啟動 | `self.panel_map_timer.isActive()` | -| 快取總是空 | GPS/HUD 消息未被快取 | 檢查 `update_ui()` 是否被調用 | -| 高 CPU 使用 | `update_drone_position()` 性能問題 | 檢查 Map 繪製邏輯 | -| 數據延遲 | 正常現象（0-100ms） | 這是預期行為 | - -## 性能預期 - -### 資源使用 ✅ - -| 指標 | 舊值 | 新值 | 改進 | -|------|------|------|------| -| Map 更新頻率 | ~100Hz+ | 10Hz | ↓ 90%+ | -| Panel 更新頻率 | ~100Hz+ | 10Hz | ↓ 90%+ | -| CPU 用於渲染 | 高 | 低-中 | ✓ 顯著 | -| 內存快取 | 無 | ~10KB | 可接受 | - -### 延遲 ✅ - -| 操作 | 延遲 | 說明 | -|------|------|------| -| GPS 消息 → 地圖更新 | 0-100ms | 可接受 | -| HUD 消息 → Panel 更新 | 0-100ms | 可接受 | -| 其他消息 → UI 更新 | 0-10ms | 保持快速 | - -## 最終確認 - -- [x] 需求已實現 -- [x] 代碼語法正確 -- [x] 文檔完整 -- [x] 無編譯錯誤 -- [x] 邏輯驗證通過 -- [x] 性能預期達成 - -## 部署檢查表 - -在部署到生產環境前： - -- [ ] 在測試環境驗證 GUI 啟動無誤 -- [ ] 驗證與多架無人機的連接 -- [ ] 檢查地圖在移動時的流暢性 -- [ ] 驗證 Panel 數據顯示正確 -- [ ] 監控 CPU 和內存使用 -- [ ] 檢查是否有任何控制台錯誤或警告 - ---- - -**驗證日期**: 2025-03-25 -**驗證狀態**: ✅ 所有項目通過 -**準備狀態**: ✅ 準備就緒 From 337ca8ce24fa5159eb6e4d811590e62b0d6f8a57 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 1 Apr 2026 15:00:20 +0800 Subject: [PATCH 121/146] =?UTF-8?q?chore:=20=E6=9B=B4=E6=96=B0=20.gitignor?= =?UTF-8?q?e=EF=BC=8C=E7=A7=BB=E9=99=A4=E4=B8=8D=E5=86=8D=E4=BD=BF?= =?UTF-8?q?=E7=94=A8=E7=9A=84=20test=5Fmission.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.6 --- .gitignore | 7 + src/GUI/validation/test_mission.py | 299 ----------------------------- 2 files changed, 7 insertions(+), 299 deletions(-) delete mode 100644 src/GUI/validation/test_mission.py diff --git a/.gitignore b/.gitignore index ab42e47..f247176 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,10 @@ Makefile **/*.pyo logs/ src/logs/ + +# 個人環境筆記 +my_env_notes.md + +# Claude Code 本地設定 +CLAUDE.md +.claude/ diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py deleted file mode 100644 index 22b6e22..0000000 --- a/src/GUI/validation/test_mission.py +++ /dev/null @@ -1,299 +0,0 @@ -#!/usr/bin/env python3 -""" -獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作 - -使用方式: - 1. 啟動 SITL - 2. 修改下方 CONFIG 區塊 - 3. python3 test_mission.py -""" -import sys, os -sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) - -import time -from pymavlink import mavutil -from PyQt6.QtWidgets import QApplication -from PyQt6.QtCore import QTimer - -from mission_planner import FormationPlanner, MissionType -from command_sender import MavlinkSender -from mission_executor import MissionExecutor, MissionState - - -# ================================================================================ -# CONFIG -# ================================================================================ - -# 接收用連線 (讀取無人機狀態) -RECV_CONNECTION = "udp:127.0.0.1:14550" - -# 發送用連線 (發送 setpoint 指令) -SEND_CONNECTION = "udpout:127.0.0.1:14550" - -# 要控制的無人機 sysid 列表 -DRONE_SYSIDS = [1] - -# 起飛高度 (公尺) -TAKEOFF_ALT = 10.0 - -# 任務規劃參數 -FORMATION_SPACING = 5.0 -BASE_ALTITUDE = 10.0 -ALTITUDE_DIFF = 2.0 -ARRIVAL_RADIUS = 2.0 - -# 測試模式: "formation" 或 "grid_sweep" -TEST_MODE = "formation" - -# Grid Sweep 專用設定 -GRID_LINE_SPACING = 5.0 - -# ================================================================================ - - -class SITLDroneManager: - """管理 SITL 無人機的連線、起飛前置作業""" - - def __init__(self, connection_string, sysids): - self.connection_string = connection_string - self.sysids = sysids - self.mav = None - self.drone_gps = {} - - def connect(self): - """建立 MAVLink 連線並等待心跳""" - print(f"連線到 {self.connection_string} ...") - self.mav = mavutil.mavlink_connection(self.connection_string) - self.mav.wait_heartbeat() - print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}") - - def set_guided_and_arm(self, sysid): - """切換到 GUIDED 模式並解鎖""" - print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---") - - # 切換 GUIDED 模式 - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_DO_SET_MODE, - 0, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - 4, # GUIDED = 4 - 0, 0, 0, 0, 0 - ) - time.sleep(1) - - # 解鎖 - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, 1, 0, 0, 0, 0, 0, 0 - ) - time.sleep(1) - print(f" sysid={sysid}: GUIDED + ARMED") - - def takeoff(self, sysid, altitude): - """起飛到指定高度""" - print(f" sysid={sysid}: 起飛到 {altitude}m ...") - self.mav.mav.command_long_send( - sysid, 1, - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, 0, 0, 0, 0, 0, 0, altitude - ) - - def wait_for_altitude(self, sysid, target_alt, timeout=30): - """等待無人機到達指定高度""" - print(f" sysid={sysid}: 等待到達 {target_alt}m ...") - start = time.time() - while time.time() - start < timeout: - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if msg and msg.get_srcSystem() == sysid: - alt = msg.relative_alt / 1000.0 - if alt >= target_alt * 0.9: - print(f" sysid={sysid}: 已到達 {alt:.1f}m") - return True - print(f" sysid={sysid}: 等待超時!") - return False - - def update_gps_once(self): - """讀取一輪 GPS 資料更新 drone_gps""" - deadline = time.time() + 3 - received = set() - while time.time() < deadline and len(received) < len(self.sysids): - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if msg is None: - continue - sid = msg.get_srcSystem() - if sid in self.sysids: - drone_id = f"s0_{sid}" - self.drone_gps[drone_id] = { - 'lat': msg.lat / 1e7, - 'lon': msg.lon / 1e7, - 'alt': msg.relative_alt / 1000.0 - } - received.add(sid) - - for sid in self.sysids: - drone_id = f"s0_{sid}" - if drone_id in self.drone_gps: - gps = self.drone_gps[drone_id] - print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)") - else: - print(f" {drone_id}: 尚未收到 GPS") - - def start_gps_polling(self, interval_ms=200): - """啟動定時 GPS 輪詢 (用 QTimer)""" - self._gps_timer = QTimer() - self._gps_timer.timeout.connect(self._poll_gps) - self._gps_timer.start(interval_ms) - - def _poll_gps(self): - """非阻塞方式讀取最新 GPS""" - while True: - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False) - if msg is None: - break - sid = msg.get_srcSystem() - if sid in self.sysids: - drone_id = f"s0_{sid}" - self.drone_gps[drone_id] = { - 'lat': msg.lat / 1e7, - 'lon': msg.lon / 1e7, - 'alt': msg.relative_alt / 1000.0 - } - - -def main(): - # 建立 Qt 應用 (MissionExecutor 需要 QTimer) - app = QApplication(sys.argv) - - # 連線 + 起飛前置作業 - manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS) - manager.connect() - - for sysid in DRONE_SYSIDS: - manager.set_guided_and_arm(sysid) - manager.takeoff(sysid, TAKEOFF_ALT) - - # 等待所有無人機到達起飛高度 - for sysid in DRONE_SYSIDS: - manager.wait_for_altitude(sysid, TAKEOFF_ALT) - - time.sleep(2) - - # 讀取當前 GPS 位置 - print("\n讀取當前 GPS 位置 ...") - manager.update_gps_once() - - drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS] - drone_gps_positions = [] - for drone_id in drone_ids: - gps = manager.drone_gps.get(drone_id) - if gps is None: - print(f"錯誤: 讀不到 {drone_id} 的 GPS") - return - drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt'])) - - # 規劃任務 - print(f"\n規劃任務 (模式: {TEST_MODE}) ...") - planner = FormationPlanner( - spacing=FORMATION_SPACING, - base_altitude=BASE_ALTITUDE, - altitude_diff=ALTITUDE_DIFF - ) - - center_lat = drone_gps_positions[0][0] - center_lon = drone_gps_positions[0][1] - - if TEST_MODE == "formation": - target_lat = center_lat + 30.0 / 111000.0 - target_lon = center_lon - target_gps = (target_lat, target_lon, BASE_ALTITUDE) - print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})") - - waypoints_per_drone, origin = planner.plan_formation_mission( - drone_gps_positions, target_gps, MissionType.M_FORMATION - ) - - elif TEST_MODE == "grid_sweep": - # 在無人機前方 30m 處建立 40m x 30m 的矩形 - offset_lat = 30.0 / 111000.0 - half_w = 20.0 / (111000.0 * 0.9) - half_h = 15.0 / 111000.0 - - rect_center_lat = center_lat + offset_lat - rect_center_lon = center_lon - - rect_corners = [ - (rect_center_lat - half_h, rect_center_lon - half_w), - (rect_center_lat - half_h, rect_center_lon + half_w), - (rect_center_lat + half_h, rect_center_lon + half_w), - (rect_center_lat + half_h, rect_center_lon - half_w), - ] - target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE) - print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})") - - waypoints_per_drone, origin = planner.plan_formation_mission( - drone_gps_positions, target_gps, MissionType.GRID_SWEEP, - params={ - 'rect_corners': rect_corners, - 'line_spacing': GRID_LINE_SPACING, - 'altitude': BASE_ALTITUDE - } - ) - else: - print(f"未知測試模式: {TEST_MODE}") - return - - planned_waypoints = { - 'drone_ids': drone_ids, - 'waypoints': waypoints_per_drone - } - - # 印出規劃結果 - for i, did in enumerate(drone_ids): - wps = waypoints_per_drone[i] - print(f" {did}: {len(wps)} 個航點") - for j, wp in enumerate(wps): - print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)") - - # 啟動任務 - print("\n啟動任務 ...") - manager.start_gps_polling(interval_ms=200) - - sender = MavlinkSender(SEND_CONNECTION) - executor = MissionExecutor( - sender=sender, - drone_gps=manager.drone_gps, - arrival_radius=ARRIVAL_RADIUS, - send_rate_hz=2.0 - ) - - executor.drone_waypoint_reached.connect( - lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}") - ) - executor.mission_completed.connect( - lambda: (print("\n===== 任務全部完成 ====="), app.quit()) - ) - - # 設定超時自動退出 - timeout_timer = QTimer() - timeout_timer.setSingleShot(True) - timeout_timer.timeout.connect(lambda: ( - print("\n⚠ 測試超時，強制退出"), - executor.stop(), - app.quit() - )) - timeout_timer.start(180_000) # 180 秒超時 - - executor.start(planned_waypoints) - - print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n") - app.exec() - - executor.stop() - sender.close() - print("測試結束") - - -if __name__ == "__main__": - main() \ No newline at end of file From c2196153ffd1860f6f3f1444e654252e94724898 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 1 Apr 2026 18:15:49 +0800 Subject: [PATCH 122/146] =?UTF-8?q?feat:=20=E5=A4=9A=E4=BB=BB=E5=8B=99?= =?UTF-8?q?=E7=BE=A4=E7=B5=84=E7=B3=BB=E7=B5=B1=20=E2=80=94=20=E5=9B=9B?= =?UTF-8?q?=E6=AC=84=E5=BC=8F=E9=9D=A2=E6=9D=BF=E3=80=81=E5=9C=B0=E5=9C=96?= =?UTF-8?q?=E6=A1=86=E9=81=B8=E5=88=86=E9=85=8D=E3=80=81=E5=8F=AF=E8=AA=BF?= =?UTF-8?q?=E5=8F=83=E6=95=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 新增 mission_group.py：MissionGroup 資料結構、DroneAssignDialog、GroupPanel - GroupPanel 四欄式佈局：控制指令 / 任務規劃 / 任務參數 / 選取與分組 - 任務參數欄依任務類型動態切換（間距、高度、偏移量等），規劃時從 UI 讀值 - 框選/全選可直接分配無人機到群組，清除分組一鍵重置 - 地圖清理：移除右下角 overlay，功能整合至 GroupPanel - 修復 JS clearSelectionMode 引用已移除元素導致框選/Grid Sweep 失效 - 每群組獨立 MissionExecutor，共用 MavlinkSender Co-Authored-By: Claude Opus 4.6 --- CLAUDE.md | 17 + src/GUI/gui.py | 695 ++++++++++++++++++++++++++------------- src/GUI/map_layout.py | 277 +++++++--------- src/GUI/mission_group.py | 495 ++++++++++++++++++++++++++++ 4 files changed, 1103 insertions(+), 381 deletions(-) create mode 100644 CLAUDE.md create mode 100644 src/GUI/mission_group.py diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..a5c0c5d --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,17 @@ +# 專案說明 + +## 專案背景 +這是計畫相關的研究，負責 Airtrapmine 無人機系統開發。 +使用 SITL、Gazebo 進行模擬，搭配 ArduPilot、MAVLink。 + +## 技術環境 +- Ubuntu Linux +- Python, ROS/Gazebo, SITL +- MAVLink, PyQt5, cmavnode + +## 注意事項 +- 這個專案跟在 Windows 上做的 MADDPG/MATLAB 異質載具對抗模擬是完全不同的工作，不要把兩者的內容混在一起 + +## 工作流程（Model 使用偏好） +- **規劃、討論、設計階段**：使用 Opus model（啟動時加 `--model opusplan` 或手動 `/model opus`） +- **實作、寫程式階段**：使用 Sonnet model（預設，或 `/model sonnet` 切換回來） diff --git a/src/GUI/gui.py b/src/GUI/gui.py index a850097..1886305 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -1,10 +1,12 @@ #!/usr/bin/env python3 import rclpy from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, - QWidget, QLabel, QSplitter, QScrollArea, + QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, - QHeaderView, QPushButton, QCheckBox, QLineEdit) + QHeaderView, QPushButton, QCheckBox, QLineEdit, + QComboBox, QDialog) from PyQt6.QtCore import Qt, QTimer +from PyQt6.QtGui import QColor import sys import asyncio import json @@ -19,11 +21,14 @@ from comm_panel import CommPanel from overview_table import OverviewTable # ================================================================================ -# 導入任務規劃器、執行器、發送器 +# 導入任務規劃器、執行器、發送器、群組 # ================================================================================ from mission_planner import FormationPlanner, MissionType from command_sender import MavlinkSender -from mission_executor import MissionExecutor +from mission_executor import MissionExecutor, MissionState +from mission_group import ( + MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS +) # ================================================================================ class ControlStationUI(QMainWindow): @@ -94,28 +99,22 @@ class ControlStationUI(QMainWindow): base_altitude=10.0, # 基準高度 10 公尺 altitude_diff=2.0 # 高低差 2 公尺 ) - self.planned_waypoints = None # 儲存規劃結果 # ================================================================================ # ================================================================================ - # 當前任務模式 (由地圖右上角下拉選單控制) + # 初始化指令發送器（所有群組共用） # ================================================================================ - self.current_mission_mode = 'M_FORMATION' + self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 # ================================================================================ # ================================================================================ - # 初始化指令發送器與任務執行器 + # 多任務群組管理 # ================================================================================ - self.command_sender = MavlinkSender("udpout:127.0.0.1:14550") # 驗證階段寫死 - - self.mission_executor = MissionExecutor( - sender=self.command_sender, - drone_gps=self.monitor.drone_gps, # 直接傳引用，即時讀取 - arrival_radius=2.0, - send_rate_hz=2.0 - ) - self.mission_executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) - self.mission_executor.mission_completed.connect(self.on_mission_completed) + self.mission_groups = {} # group_id → MissionGroup + self.group_panels = {} # group_id → GroupPanel + self.active_group_id = None # 當前 active 的 group + self._group_counter = 0 # 用來產生 group_id + self._pending_box_assign = None # 框選後直接分配到的 group_id # ================================================================================ self.init_ui() @@ -163,97 +162,51 @@ class ControlStationUI(QMainWindow): right_layout.setContentsMargins(10, 10, 10, 10) right_layout.setSpacing(10) - # ========== 批次控制區域 ========== - batch_control_layout = QHBoxLayout() - - batch_title = QLabel("批次操作") - batch_title.setStyleSheet(""" - color: #DDD; font-size: 16px; font-weight: bold; - padding: 5px; background-color: #333; border-radius: 4px; - """) - batch_control_layout.addWidget(batch_title) - - first_row = QHBoxLayout() - select_all_btn = QPushButton("全選") - select_all_btn.clicked.connect(self.handle_select_all) - select_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + # ========== 任務群組 Tab ========== + group_header = QHBoxLayout() + group_title = QLabel("任務群組") + group_title.setStyleSheet( + "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;") + group_header.addWidget(group_title) + group_header.addStretch() + + add_group_btn = QPushButton("+ 新增群組") + add_group_btn.setStyleSheet(""" + QPushButton { background-color: #4A9EFF; color: white; border: none; + padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; } + QPushButton:hover { background-color: #3A8EEF; } """) - first_row.addWidget(select_all_btn) - first_row.addStretch() - - mode_layout = QHBoxLayout() - mode_label = QLabel("模式：") - mode_label.setStyleSheet("color: #DDD; min-width: 40px;") - - from PyQt6.QtWidgets import QComboBox - self.mode_combo = QComboBox() - self.mode_combo.addItems([ - "GUIDED", "AUTO", "LAND", "LOITER", - "STABILIZE", "ACRO", "ALT_HOLD", "RTL", - "CIRCLE", "DRIFT", "SPORT", "FLIP", - "AUTOTUNE", "POSHOLD", "BRAKE", "THROW", - "AVOID_ADSB", "GUIDED_NOGPS", "SMART_RTL", - "FLOWHOLD", "FOLLOW", "ZIGZAG", "SYSTEMID", - "AUTOROTATE", "AUTO_RTL" - ]) - self.mode_combo.setCurrentIndex(1) - self.mode_combo.setStyleSheet(""" - QComboBox { background-color: #333; color: #DDD; border-radius: 3px; padding: 2px 10px;} - """) - - batch_mode_btn = QPushButton("切換") - batch_mode_btn.clicked.connect(self.handle_batch_mode_change) - batch_mode_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + add_group_btn.clicked.connect(self._add_mission_group) + group_header.addWidget(add_group_btn) + + clear_traj_btn = QPushButton("清除軌跡") + clear_traj_btn.setStyleSheet(""" + QPushButton { background-color: #EF5350; color: white; border: none; + padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; } + QPushButton:hover { background-color: #E53935; } """) - mode_layout.addWidget(mode_label) - mode_layout.addWidget(self.mode_combo) - mode_layout.addWidget(batch_mode_btn) - mode_layout.addStretch() - - third_row = QHBoxLayout() - arm_all_btn = QPushButton("解鎖") - arm_all_btn.clicked.connect(self.handle_arm_selected) - arm_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } - """) - third_row.addWidget(arm_all_btn) - third_row.addStretch() - - fourth_row = QHBoxLayout() - self.z_input = QLineEdit() - self.z_input.setFixedWidth(60) - self.z_input.setStyleSheet(""" - QLineEdit { background-color: #333; color: #DDD; - border: 1px solid #555; border-radius: 4px; padding: 3px; } - """) - - takeoff_all_btn = QPushButton("起飛") - takeoff_all_btn.clicked.connect(self.handle_takeoff_selected) - takeoff_all_btn.setStyleSheet(""" - QPushButton { background-color: #444; color: #DDD; border: none; - padding: 8px 12px; border-radius: 4px; min-width: 80px; } - QPushButton:hover { background-color: #555; } + clear_traj_btn.clicked.connect(self.drone_map.clear_trajectories) + group_header.addWidget(clear_traj_btn) + + right_layout.addLayout(group_header) + + self.group_tab_widget = QTabWidget() + self.group_tab_widget.setStyleSheet(""" + QTabWidget::pane { border: 1px solid #444; background-color: #2B2B2B; } + QTabBar::tab { + background-color: #333; color: #AAA; border: 1px solid #444; + padding: 5px 12px; margin-right: 2px; border-top-left-radius: 4px; + border-top-right-radius: 4px; font-size: 12px; + } + QTabBar::tab:selected { background-color: #2B2B2B; color: #FFF; border-bottom-color: #2B2B2B; } + QTabBar::tab:hover { background-color: #3A3A3A; } """) - - fourth_row.addWidget(QLabel("高度:", styleSheet="color: #DDD;")) - fourth_row.addWidget(self.z_input) - fourth_row.addWidget(takeoff_all_btn) - fourth_row.addStretch() - - batch_control_layout.addLayout(first_row) - batch_control_layout.addLayout(mode_layout) - batch_control_layout.addLayout(third_row) - batch_control_layout.addLayout(fourth_row) + self.group_tab_widget.setFixedHeight(150) + self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed) + right_layout.addWidget(self.group_tab_widget) - right_layout.addLayout(batch_control_layout) + # 預設建立 Group A + self._add_mission_group() # 添加地圖 right_layout.addWidget(self.drone_map.get_widget()) @@ -263,13 +216,11 @@ class ControlStationUI(QMainWindow): self.drone_map.get_toggle_select_all_drones_signal().connect(self.handle_toggle_select_all_drones) # ================================================================================ - # 連接任務控制 + 矩形選取 + 任務模式切換 + 路徑確認信號 + # 連接地圖信號（任務模式改由 Group Tab 控制，不再從地圖下拉選單） # ================================================================================ - self.drone_map.get_start_mission_signal().connect(self.handle_start_mission) - self.drone_map.get_pause_mission_signal().connect(self.handle_pause_mission) self.drone_map.get_rectangle_selected_signal().connect(self.handle_rectangle_selected) - self.drone_map.get_mission_mode_changed_signal().connect(self.on_mission_mode_changed) self.drone_map.get_route_confirmed_signal().connect(self.handle_route_confirmed) + self.drone_map.get_drone_box_selected_signal().connect(self._handle_drone_box_selected) # ================================================================================ main_splitter.addWidget(self.left_tab) @@ -426,7 +377,13 @@ class ControlStationUI(QMainWindow): # ================================================================================ def handle_mode_change(self, drone_id): - mode = self.mode_combo.currentText() + # 從 active group 的 mode_combo 讀取模式 + group = self._get_active_group() + if group: + panel = self.group_panels.get(group.group_id) + mode = panel.mode_combo.currentText() if panel else "GUIDED" + else: + mode = "GUIDED" loop = asyncio.get_event_loop() future = self.monitor.set_mode(drone_id, mode) loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}")) @@ -488,13 +445,266 @@ class ControlStationUI(QMainWindow): for drone_id in self.monitor.selected_drones: future = self.monitor.takeoff_drone(drone_id, 10.0) loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) - - def handle_batch_mode_change(self): - mode = self.mode_combo.currentText() + + # ================================================================================ + # 任務群組管理 + # ================================================================================ + + def _next_group_id(self): + """產生下一個群組 ID (A, B, C, ...)""" + gid = chr(ord('A') + self._group_counter) + self._group_counter += 1 + return gid + + def _add_mission_group(self): + """新增一個任務群組""" + gid = self._next_group_id() + color = GROUP_COLORS[(self._group_counter - 1) % len(GROUP_COLORS)] + group = MissionGroup(gid, color) + self.mission_groups[gid] = group + + panel = GroupPanel(group) + panel.assign_drones_requested.connect(self._handle_assign_drones) + panel.mission_type_changed.connect(self._handle_mission_type_changed) + panel.start_requested.connect(self._handle_group_start) + panel.pause_requested.connect(self._handle_group_pause) + panel.stop_requested.connect(self._handle_group_stop) + panel.mode_change_requested.connect(self._handle_group_mode_change) + panel.arm_requested.connect(self._handle_group_arm) + panel.takeoff_requested.connect(self._handle_group_takeoff) + panel.box_select_requested.connect(self._handle_box_select) + panel.select_all_requested.connect(self._handle_select_all_for_group) + panel.clear_group_requested.connect(self._handle_clear_group) + self.group_panels[gid] = panel + + # 用帶顏色的 tab 標題 + scroll = QScrollArea() + scroll.setWidget(panel) + scroll.setWidgetResizable(True) + idx = self.group_tab_widget.addTab(scroll, f"Group {gid}") + self.group_tab_widget.tabBar().setTabTextColor(idx, QColor(color)) + self.group_tab_widget.setCurrentIndex(idx) + self.active_group_id = gid + self.statusBar().showMessage(f"已新增 Group {gid}", 2000) + + def _on_group_tab_changed(self, index): + """Tab 切換時更新 active group 並同步地圖模式""" + if index < 0: + self.active_group_id = None + return + # tab 標題是 "Group X" + tab_text = self.group_tab_widget.tabText(index) + gid = tab_text.replace("Group ", "") + if gid in self.mission_groups: + self.active_group_id = gid + group = self.mission_groups[gid] + self.drone_map.set_mission_mode(group.mission_type) + + def _get_active_group(self): + """取得當前 active 的 MissionGroup""" + if self.active_group_id and self.active_group_id in self.mission_groups: + return self.mission_groups[self.active_group_id] + return None + + def _get_other_assigned(self, exclude_gid): + """取得其他群組已佔用的無人機 {drone_id: group_id}""" + assigned = {} + for gid, group in self.mission_groups.items(): + if gid == exclude_gid: + continue + for did in group.drone_ids: + assigned[did] = gid + return assigned + + def _handle_assign_drones(self, group_id): + """開啟無人機分配對話框（已勾選的 checkbox 會預先帶入）""" + group = self.mission_groups.get(group_id) + if not group: + return + all_ids = list(self.drones.keys()) + other_assigned = self._get_other_assigned(group_id) + + # 將目前 checkbox 已勾選的無人機（且未被其他群組佔用）合併進 pre-selected + currently_checked = self.get_selected_drones() + pre_selected = set(group.drone_ids) + for did in currently_checked: + if did not in other_assigned: + pre_selected.add(did) + + dialog = DroneAssignDialog(self, all_ids, pre_selected, other_assigned) + if dialog.exec() == QDialog.DialogCode.Accepted: + group.drone_ids = dialog.get_selected() + panel = self.group_panels.get(group_id) + if panel: + panel.update_drone_list() + panel.update_status() + self.statusBar().showMessage( + f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000) + + def _handle_mission_type_changed(self, group_id, mission_type): + """群組任務類型切換""" + group = self.mission_groups.get(group_id) + if group: + group.mission_type = mission_type + # 如果是 active group，同步更新地圖的選擇模式 + if group_id == self.active_group_id: + self.drone_map.set_mission_mode(mission_type) + + def _create_executor_for_group(self, group): + """為群組建立 MissionExecutor""" + executor = MissionExecutor( + sender=self.command_sender, + drone_gps=self.monitor.drone_gps, + arrival_radius=2.0, + send_rate_hz=2.0 + ) + executor.drone_waypoint_reached.connect(self.on_drone_waypoint_reached) + executor.mission_completed.connect( + lambda gid=group.group_id: self._on_group_mission_completed(gid)) + group.executor = executor + + def _handle_group_start(self, group_id): + """啟動群組任務""" + group = self.mission_groups.get(group_id) + if not group: + return + if group.planned_waypoints is None: + self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000) + return + if group.executor is None: + self._create_executor_for_group(group) + group.executor.start(group.planned_waypoints) + panel = self.group_panels.get(group_id) + if panel: + panel.update_status() + self.statusBar().showMessage(f"🚀 Group {group_id}: 任務已啟動", 3000) + + def _handle_group_pause(self, group_id): + """暫停/恢復群組任務""" + group = self.mission_groups.get(group_id) + if not group or not group.executor: + return + if group.executor.state == MissionState.RUNNING: + group.executor.pause() + self.statusBar().showMessage(f"⏸ Group {group_id}: 任務已暫停", 3000) + elif group.executor.state == MissionState.PAUSED: + group.executor.resume() + self.statusBar().showMessage(f"▶ Group {group_id}: 任務已恢復", 3000) + panel = self.group_panels.get(group_id) + if panel: + panel.update_status() + + def _handle_group_stop(self, group_id): + """停止群組任務""" + group = self.mission_groups.get(group_id) + if not group: + return + if group.executor: + group.executor.stop() + group.planned_waypoints = None + self.drone_map.clear_mission_plan_for_group(group_id) + panel = self.group_panels.get(group_id) + if panel: + panel.update_status() + panel.clear_mission_info() + self.statusBar().showMessage(f"■ Group {group_id}: 任務已停止", 3000) + + def _handle_group_mode_change(self, group_id, mode): + """切換群組內所有無人機的飛行模式""" + group = self.mission_groups.get(group_id) + if not group: + return loop = asyncio.get_event_loop() - for drone_id in self.monitor.selected_drones: + for drone_id in group.drone_ids: future = self.monitor.set_mode(drone_id, mode) - loop.create_task(self.handle_service_response(future, f"{drone_id} 切換模式 {mode}")) + loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}")) + + def _handle_group_arm(self, group_id): + """解鎖群組內所有無人機""" + group = self.mission_groups.get(group_id) + if not group: + return + loop = asyncio.get_event_loop() + for drone_id in group.drone_ids: + future = self.monitor.arm_drone(drone_id, True) + loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}")) + + def _handle_group_takeoff(self, group_id, altitude): + """群組內所有無人機起飛""" + group = self.mission_groups.get(group_id) + if not group: + return + loop = asyncio.get_event_loop() + for drone_id in group.drone_ids: + future = self.monitor.takeoff_drone(drone_id, altitude) + loop.create_task(self.handle_service_response(future, f"起飛 {drone_id} ({altitude}m)")) + + def _handle_box_select(self, group_id): + """觸發地圖框選 → 框選完成後直接分配到該群組""" + self._pending_box_assign = group_id + self.drone_map.toggle_drone_box_select() + self.statusBar().showMessage( + f"請在地圖上框選要分配到 Group {group_id} 的無人機", 5000) + + def _handle_drone_box_selected(self, drone_ids_json): + """地圖框選完成 — 直接分配到指定群組""" + group_id = self._pending_box_assign + self._pending_box_assign = None + if not group_id: + return + group = self.mission_groups.get(group_id) + if not group: + return + drone_ids = json.loads(drone_ids_json) + other = self._get_other_assigned(group_id) + valid = {did for did in drone_ids if did not in other} + group.drone_ids = valid + panel = self.group_panels.get(group_id) + if panel: + panel.update_drone_list() + panel.update_status() + self.statusBar().showMessage( + f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000) + + def _handle_select_all_for_group(self, group_id): + """全選所有可用無人機，直接分配到該群組""" + group = self.mission_groups.get(group_id) + if not group: + return + other = self._get_other_assigned(group_id) + available = {did for did in self.drones.keys() if did not in other} + group.drone_ids = available + panel = self.group_panels.get(group_id) + if panel: + panel.update_drone_list() + panel.update_status() + self.statusBar().showMessage( + f"Group {group_id}: 全選分配 {len(available)} 台無人機", 3000) + + def _handle_clear_group(self, group_id): + """清除群組的無人機分配""" + group = self.mission_groups.get(group_id) + if not group: + return + group.drone_ids = set() + group.planned_waypoints = None + if group.executor: + group.executor.stop() + self.drone_map.clear_mission_plan_for_group(group_id) + panel = self.group_panels.get(group_id) + if panel: + panel.update_drone_list() + panel.update_status() + panel.clear_mission_info() + self.statusBar().showMessage( + f"Group {group_id}: 已清除分組", 3000) + + def _on_group_mission_completed(self, group_id): + """群組任務完成回呼""" + panel = self.group_panels.get(group_id) + if panel: + panel.update_status() + self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000) # ================================================================================ # UI 更新 @@ -610,73 +820,76 @@ class ControlStationUI(QMainWindow): self.update_group_checkbox_state(socket_id) # ================================================================================ - # 任務模式切換 + # 任務規劃 — 點擊地圖（路由到 active group） # ================================================================================ - def on_mission_mode_changed(self, mode): - self.current_mission_mode = mode - mode_names = { - 'M_FORMATION': '列隊飛行', - 'CIRCLE_FORMATION': '環狀包圍', - 'LEADER_FOLLOWER': '跟隨模式', - 'GRID_SWEEP': '柵狀偵查' - } - display_name = mode_names.get(mode, mode) - self.statusBar().showMessage(f"🔄 任務模式: {display_name}", 3000) - print(f"任務模式切換: {mode}") + def _get_group_drones(self, group): + """取得群組的無人機 ID 列表（排序後）""" + return sorted(group.drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1]))) - # ================================================================================ - # 任務規劃 — 點擊地圖 (M-Formation / Circle) - # ================================================================================ - def handle_map_click(self, lat, lon): - """處理地圖點擊事件 — 根據選單模式規劃""" - print(f"地圖點擊位置: {lat:.6f}, {lon:.6f} (模式: {self.current_mission_mode})") - - # Grid Sweep 和 Leader-Follower 由各自的觸發方式處理，點擊地圖不處理 + """處理地圖點擊事件 — 根據 active group 的任務類型規劃""" + group = self._get_active_group() + if not group: + self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + return + mode_map = { 'M_FORMATION': MissionType.M_FORMATION, 'CIRCLE_FORMATION': MissionType.CIRCLE_FORMATION, } - mission_type = mode_map.get(self.current_mission_mode) + mission_type = mode_map.get(group.mission_type) if mission_type is None: - # Grid Sweep / Leader-Follower 模式下點擊地圖不處理 - return - - selected_drones = self.get_selected_drones() + return # Grid Sweep / Leader-Follower 由各自的觸發方式處理 + + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage("⚠ 請先選擇無人機（勾選 checkbox）", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return - - base_alt = 10.0 + + print(f"地圖點擊: {lat:.6f}, {lon:.6f} → Group {group.group_id} ({group.mission_type})") + panel = self.group_panels.get(group.group_id) + params = panel.get_mission_params() if panel else {} + base_alt = params.get('base_altitude', params.get('altitude', 10.0)) target_gps = (lat, lon, base_alt) - self.statusBar().showMessage(f"⏳ 正在規劃 {self.current_mission_mode} ({len(selected_drones)} 台) ...", 2000) - + self.statusBar().showMessage( + f"⏳ Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台) ...", 2000) + try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) - if drone_gps_positions is None: return - + if drone_gps_positions is None: + return + waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( - drone_gps_positions, target_gps, mission_type + drone_gps_positions, target_gps, mission_type, params=params ) - - self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + center_lat, center_lon, _ = center_origin - self.drone_map.draw_mission_plan(center_lat, center_lon, lat, lon) - + self.drone_map.draw_mission_plan_for_group( + group.group_id, group.color, + center_lat, center_lon, lat, lon + ) + self._launch_verification( - self.current_mission_mode, drone_gps_positions, selected_drones, + group.mission_type, drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, target_gps=target_gps ) - + + panel = self.group_panels.get(group.group_id) + if panel: + panel.update_status() + panel.update_mission_info(center_lat, center_lon, lat, lon) + total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ {self.current_mission_mode} 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 + f"✓ Group {group.group_id}: {group.mission_type} 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() @@ -685,66 +898,93 @@ class ControlStationUI(QMainWindow): # ================================================================================ def handle_rectangle_selected(self, points_json): - print(f"矩形選取: {points_json}") - selected_drones = self.get_selected_drones() + group = self._get_active_group() + if not group: + self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + return + if group.mission_type != 'GRID_SWEEP': + return # 非 Grid Sweep 模式不處理矩形選取 + + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage("⚠ 請先選擇無人機再框選區域", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return + + print(f"矩形選取 → Group {group.group_id}: {points_json}") try: rect_corners = [(p[0], p[1]) for p in json.loads(points_json)] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000) return - base_alt = 10.0 - self.statusBar().showMessage(f"⏳ 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000) + panel = self.group_panels.get(group.group_id) + params = panel.get_mission_params() if panel else {} + base_alt = params.get('altitude', 10.0) + self.statusBar().showMessage( + f"⏳ Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) - if drone_gps_positions is None: return + if drone_gps_positions is None: + return target_lat = sum(c[0] for c in rect_corners) / 4 target_lon = sum(c[1] for c in rect_corners) / 4 target_gps = (target_lat, target_lon, base_alt) + params['rect_corners'] = rect_corners waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( drone_gps_positions, target_gps, MissionType.GRID_SWEEP, - params={'rect_corners': rect_corners, 'line_spacing': 5.0, 'altitude': base_alt} + params=params ) - - self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + center_lat, center_lon, _ = center_origin - self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon) + self.drone_map.draw_mission_plan_for_group( + group.group_id, group.color, + center_lat, center_lon, target_lat, target_lon + ) self._launch_verification( 'grid_sweep', drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, rect_corners=rect_corners ) - + + panel = self.group_panels.get(group.group_id) + if panel: + panel.update_status() + panel.update_mission_info(center_lat, center_lon, target_lat, target_lon) + total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ Grid Sweep 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 + f"✓ Group {group.group_id}: Grid Sweep 規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ Grid Sweep 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ - # 任務規劃 — 路徑確認 (Leader-Follower 跟隨模式) + # 任務規劃 — 路徑確認 (Leader-Follower) # ================================================================================ def handle_route_confirmed(self, points_json): - """路徑確認 → Leader-Follower 任務規劃""" - print(f"路徑確認: {points_json}") - - selected_drones = self.get_selected_drones() + group = self._get_active_group() + if not group: + self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + return + if group.mission_type != 'LEADER_FOLLOWER': + return + + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage("⚠ 請先選擇無人機再標記路徑", 3000) + self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) return - + + print(f"路徑確認 → Group {group.group_id}: {points_json}") try: - route_points = json.loads(points_json) # [[lat, lon], ...] + route_points = json.loads(points_json) route_waypoints = [(p[0], p[1]) for p in route_points] except (json.JSONDecodeError, IndexError): self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000) @@ -754,81 +994,67 @@ class ControlStationUI(QMainWindow): self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000) return - base_alt = 10.0 - self.statusBar().showMessage(f"⏳ 正在規劃跟隨模式 ({len(selected_drones)} 台, {len(route_waypoints)} 個路徑點) ...", 2000) - + panel = self.group_panels.get(group.group_id) + params = panel.get_mission_params() if panel else {} + base_alt = params.get('altitude', 10.0) + self.statusBar().showMessage( + f"⏳ Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台) ...", 2000) + try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: return - # 目標點 = 路徑中心（供 origin 計算） target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints) target_gps = (target_lat, target_lon, base_alt) + params['route_waypoints'] = route_waypoints waypoints_per_drone, center_origin = self.mission_planner.plan_formation_mission( - drone_gps_positions, - target_gps, - MissionType.LEADER_FOLLOWER, - params={ - 'route_waypoints': route_waypoints, - 'lateral_offset': 3.0, - 'longitudinal_spacing': 5.0, - 'altitude': base_alt - } + drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER, + params=params ) - - self.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} - self.show_planned_waypoints() - + + group.planned_waypoints = {'drone_ids': selected_drones, 'waypoints': waypoints_per_drone} + group.center_origin = center_origin + self.show_planned_waypoints(group) + center_lat, center_lon, _ = center_origin - self.drone_map.draw_mission_plan(center_lat, center_lon, target_lat, target_lon) - - # 啟動視覺化驗證 + self.drone_map.draw_mission_plan_for_group( + group.group_id, group.color, + center_lat, center_lon, target_lat, target_lon + ) + self._launch_verification( 'LEADER_FOLLOWER', drone_gps_positions, selected_drones, waypoints_per_drone, center_origin, target_gps=target_gps, route_waypoints=route_waypoints ) - + + panel = self.group_panels.get(group.group_id) + if panel: + panel.update_status() + panel.update_mission_info(center_lat, center_lon, target_lat, target_lon) + total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ 跟隨模式規劃完成！{len(selected_drones)} 台，{len(route_waypoints)} 個路徑點，共 {total_wps} 個航點", 5000 + f"✓ Group {group.group_id}: 跟隨模式規劃完成！{len(selected_drones)} 台，共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ 跟隨模式規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000) import traceback traceback.print_exc() # ================================================================================ - # 任務執行控制 + # 任務執行回呼 # ================================================================================ - def handle_start_mission(self, center_lat, center_lon, target_lat, target_lon): - if self.planned_waypoints is None: - self.statusBar().showMessage("⚠ 請先規劃任務", 3000) - return - self.mission_executor.start(self.planned_waypoints) - self.statusBar().showMessage("🚀 任務已啟動", 3000) - - def handle_pause_mission(self): - if self.mission_executor.state.value == "running": - self.mission_executor.pause() - self.statusBar().showMessage("⏸ 任務已暫停", 3000) - elif self.mission_executor.state.value == "paused": - self.mission_executor.resume() - self.statusBar().showMessage("▶ 任務已恢復", 3000) - def on_drone_waypoint_reached(self, drone_id, wp_index, total): if wp_index >= total: self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000) else: self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000) - def on_mission_completed(self): - self.statusBar().showMessage("✅ 所有無人機已完成任務", 5000) - # ================================================================================ # 輔助方法 # ================================================================================ @@ -877,20 +1103,23 @@ class ControlStationUI(QMainWindow): subprocess.Popen([sys.executable, script_path, '--file', json_path]) print(f"驗證視窗已啟動: {json_path}") - def show_planned_waypoints(self): - if not self.planned_waypoints: return - print("\n" + "=" * 60) - print("任務規劃結果") - print("=" * 60) - drone_ids = self.planned_waypoints['drone_ids'] - waypoints = self.planned_waypoints['waypoints'] + def show_planned_waypoints(self, group=None): + pw = group.planned_waypoints if group else None + if not pw: + return + gid = group.group_id if group else "?" + print(f"\n{'=' * 60}") + print(f"任務規劃結果 — Group {gid}") + print(f"{'=' * 60}") + drone_ids = pw['drone_ids'] + waypoints = pw['waypoints'] print(f"\n共 {len(drone_ids)} 台無人機") for i, drone_id in enumerate(drone_ids): wps = waypoints[i] print(f"\n【{drone_id}】({len(wps)} 個航點)") for j, wp in enumerate(wps): print(f" WP{j}: ({wp[0]:.6f}°, {wp[1]:.6f}°, {wp[2]:.1f}m)") - print("\n" + "=" * 60) + print(f"\n{'=' * 60}") def get_selected_drones(self): return [did for did, panel in self.drones.items() if hasattr(panel, 'checkbox') and panel.checkbox.isChecked()] @@ -1095,7 +1324,9 @@ class ControlStationUI(QMainWindow): print(f"ROS spin error: {e}") def closeEvent(self, event): - self.mission_executor.stop() + for group in self.mission_groups.values(): + if group.executor: + group.executor.stop() self.command_sender.close() for receiver in self.udp_receivers: receiver.stop() diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 0e0a252..2be9cb9 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -178,35 +178,8 @@ class DroneMap:
-
- -
- - -
- - -
-
-
- - -
-
- 中心點: - 未設定 -
-
- 目標點: - 未設定 -
- - +