ROS2 sub

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(
+            'summary': self.create_subscription(
-                Float64,
+                String,
-                f'{base_topic}/global_position/rel_alt',
+                f'{base_topic}/summary',
-                lambda msg, did=drone_id: self.altitude_callback(did, msg),
+                lambda msg, did=drone_id: self.summary_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(
@@ -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,