1. (modify) 更新 ros2 對於 GNSS topic 的發佈, 使其包涵 epv eph 衛星數與 fix_type

2. (adding) 新增相應的 msg 檔案與修改 CMakeLists, package
3. (adding) test_vehicleStatusPublisher 相對應修正

src/fc_interfaces/CMakeLists.txt

@@ -9,13 +9,15 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AttitudeRaw.msg"
+  "msg/GnssRaw.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"
-  DEPENDENCIES std_msgs
+  DEPENDENCIES std_msgs builtin_interfaces
   )
 
 ament_package()

src/fc_interfaces/msg/GnssRaw.msg

@@ -0,0 +1,8 @@
+builtin_interfaces/Time stamp
+float64 latitude
+float64 longitude
+float64 altitude
+uint8 fix_type
+uint8 satellites_visible
+float32 eph
+float32 epv

src/fc_interfaces/package.xml

@@ -17,6 +17,8 @@
 
   <build_depend>std_msgs</build_depend>
   <exec_depend>std_msgs</exec_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -1145,7 +1145,7 @@ class Orchestrator:
         self.fc_ros_manager = mros.ros2_manager
         self.fc_ros_manager.initialize()
         self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
-            'position': 1.0,
+            'position_gnss': 1.0,
             'position_ned': 1.0,
             'attitude': 1.0,
             'velocity': 0.0,

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -109,6 +109,7 @@ class mavlink_bridge:
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
             0: self._handle_heartbeat,        # HEARTBEAT
+            24: self._handle_gps_raw_int,     # GPS_RAW_INT
             30: self._handle_attitude,        # ATTITUDE
             32: self._handle_local_position,  # LOCAL_POSITION_NED
             33: self._handle_global_position, # GLOBAL_POSITION_INT
@@ -239,6 +240,14 @@ class mavlink_bridge:
         component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
         component.status.position.timestamp = timestamp
 
+    def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
+        """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
+        component.status.gps.fix_type = msg.fix_type
+        component.status.gps.satellites_visible = msg.satellites_visible
+        component.status.gps.eph = None if msg.eph == 65535 else msg.eph
+        component.status.gps.epv = None if msg.epv == 65535 else msg.epv
+        component.status.gps.timestamp = timestamp
+
     def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
         """處理 VFR_HUD 訊息 (msg_id: 74)"""
         component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +395,9 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
+        # 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, 
+        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -61,7 +61,7 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.0,       # GNSS位置
+            'position_gnss': 0.0,  # GNSS位置
             'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
             'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
             'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
@@ -171,7 +171,7 @@ class VehicleStatusPublisher(Node):
         # 例如：self._publish_ekf_status(sysid, status)
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
-        self._publish_position(sysid, status)
+        self._publish_position_gnss(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
@@ -202,9 +202,9 @@ class VehicleStatusPublisher(Node):
             logger.info(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
-    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
+    def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
         """發布 GPS 位置"""
-        if not self.rate_controller.should_publish(sysid, 'position'):
+        if not self.rate_controller.should_publish(sysid, 'position_gnss'):
             return
         
         pos = status.position
@@ -212,22 +212,27 @@ class VehicleStatusPublisher(Node):
             return
         
         publisher = self._get_or_create_publisher(
-            sysid, 'position', sensor_msgs.msg.NavSatFix
+            sysid, 'position_gnss', fcmsg.GnssRaw
             )
         
         # 檢查是否有訂閱者
         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
+        msg = fcmsg.GnssRaw()
+        msg.stamp = self.get_clock().now().to_msg()
+        msg.latitude = float(pos.latitude)
+        msg.longitude = float(pos.longitude)
+        msg.altitude = float(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
+            msg.fix_type = int(gps.fix_type)
+        if gps.satellites_visible is not None:
+            msg.satellites_visible = int(gps.satellites_visible)
+        msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
+        msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
         
         publisher.publish(msg)
 

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -17,6 +17,7 @@ import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
+import fc_interfaces.msg
 
 # 專案 imports
 from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
         
         self.sysid = sysid
         self.received_messages = {
-            'position': [],
+            'position_gnss': [],
             'attitude': [],
             'velocity': [],
             'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
         
         # Position
         self.create_subscription(
-            sensor_msgs.msg.NavSatFix,
-            f'{base_topic}/position',
-            lambda msg: self._on_message('position', msg),
+            fc_interfaces.msg.GnssRaw,
+            f'{base_topic}/position_gnss',
+            lambda msg: self._on_message('position_gnss', msg),
             10
         )
         
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
     
     def _format_msg(self, topic_name: str, msg) -> str:
         """格式化消息以便顯示"""
-        if topic_name == 'position':
+        if topic_name == 'position_gnss':
             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})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
     # 檢查收到的消息
     print("\n--- 消息統計 ---")
     total_messages = 0
-    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         total_messages += count
         print(f"  {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
     # 修改頻率設定
     publisher_node = ros2_manager.status_publisher
     publisher_node.rate_controller.topic_intervals = {
-        'position': 1.5,   
+        'position_gnss': 1.5,   
         'attitude': 1.0,    
         'velocity': 1.0,     
         'battery': 1.0,   
@@ -329,7 +330,7 @@ def test_frequency_control():
     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']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
     for topic in ['battery', 'mode', 'summary']: