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,