Update GUI 2.0.7 group selected list

src/GUI/communication.py

@@ -13,10 +13,11 @@ import sys
 import os
 import traceback
 from pymavlink import mavutil
-from geometry_msgs.msg import Point, Vector3
+from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
 from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
+from nav_msgs.msg import Odometry
 from mavros_msgs.srv import CommandBool, CommandTOL
 
 # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
@@ -588,8 +589,25 @@ class DroneMonitor(Node):
                 # 暂时所有 ROS2 topic 共享同一个 socket_id = 0
                 self.sys_to_socket_id[sys_id] = 0
             
-            if not hasattr(self, f'drone_{sys_id}_subs'):
+            subs_attr = f'drone_{sys_id}_subs'
+            if not hasattr(self, subs_attr):
                 self.setup_drone(sys_id)
+            else:
+                # 檢查既有訂閱是否包含 position_ned，如果不包含就添加（兼容舊訂閱）
+                subs = getattr(self, subs_attr, {})
+                if isinstance(subs, dict) and 'position_ned' not in subs:
+                    base_topic = f'/fc_network/vehicle/{sys_id}'
+                    try:
+                        pos_ned_sub = self.create_subscription(
+                            Odometry,
+                            f'{base_topic}/position_ned',
+                            lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                            10
+                        )
+                        subs['position_ned'] = pos_ned_sub
+                        setattr(self, subs_attr, subs)  # 明確保存更新後的字典
+                    except Exception as e:
+                        pass
     
     def setup_drone(self, sys_id):
         # sys_id 格式： sys11, sys12, ...
@@ -636,6 +654,12 @@ class DroneMonitor(Node):
                 f'{base_topic}/vfr_hud',
                 lambda msg, sid=sys_id: self.hud_callback(sid, msg),
                 10
+            ),
+            'position_ned': self.create_subscription(
+                Odometry,
+                f'{base_topic}/position_ned',
+                lambda msg, sid=sys_id: self.position_ned_callback(sid, msg),
+                10
             )
         }
         
@@ -879,6 +903,7 @@ class DroneMonitor(Node):
                 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
+                print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)")
             
             # 先發送連接類型資訊
             self.signals.update_signal.emit('connection_type', actual_drone_id, {
@@ -959,6 +984,45 @@ class DroneMonitor(Node):
             'climb': msg.climb
         }
 
+    def position_ned_callback(self, sys_id, msg):
+        """處理 position_ned topic (nav_msgs/msg/Odometry)
+        NED 座標系中的位置（在 msg.pose.pose.position）：
+        - x: 北向位移 (m)
+        - y: 東向位移 (m)
+        - z: 向下位移 (m) - 負值表示向下，轉換為高度需要 * (-1)
+        """
+        try:
+            # 使用映射獲取實際的 drone_id
+            actual_drone_id = self.sys_to_actual_id.get(sys_id, None)
+            # 如果還沒有從 summary 獲取到映射，則不處理
+            if actual_drone_id is None:
+                return
+            
+            # 從 Odometry 消息中提取位置數據 (msg.pose.pose.position)
+            # Odometry 結構：header, child_frame_id, pose(包含PoseWithCovariance), twist
+            x = msg.pose.pose.position.y  # NED 座標系中交換 x/y（與 local_pose 對齐）
+            y = msg.pose.pose.position.x
+            z = -msg.pose.pose.position.z  # 將向下的 NED z 轉換為向上的高度（z 為負表示向下）
+            
+            # 儲存高度信息
+            self.latest_data[(actual_drone_id, 'altitude')] = {
+                'altitude': z
+            }
+            
+            # 儲存本地位置信息
+            self.latest_data[(actual_drone_id, 'local_pose')] = {
+                'x': x,
+                'y': y,
+                'z': z
+            }
+            
+            # 發送信號給 GUI 更新高度顯示
+            self.signals.update_signal.emit('altitude', actual_drone_id, {
+                'altitude': z
+            })
+        except Exception as e:
+            pass
+
     def loss_rate_callback(self, drone_id, msg):
         self.latest_data[(drone_id, 'loss_rate')] = {
             'loss_rate': msg.data