chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Upload files to 'src/GUI'

Browse Source
This commit is contained in:
yeh9106
2026-02-07 18:31:22 +08:00
parent 7694f29d1a
commit f7f90c3168
3 changed files with 582 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/communication.py
+99 -57
View File
@@ -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")