(Tested) 將ROS2 Topic 納入 mainOrchestrator 系統

README.md

@@ -68,6 +68,7 @@ N. logs 是執行時期的記錄檔
 . ./install/local_setup.bash
 # 範例
 python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
-python -m fc_network_adapter.tests.test_ringBuffer
 python -m fc_network_adapter.tests.demo_integration
+
 ```
+

src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py

@@ -22,6 +22,7 @@ from pymavlink import mavutil
 from . import mavlinkObject as mo
 from . import serialManager as sm
 from . import mavlinkVehicleView as mvv
+from . import mavlinkROS2Nodes as mros
 
 from .utils import RingBuffer, setup_logger
 from .utils import acquireSerial, acquirePort
@@ -37,6 +38,7 @@ class PanelState:
         self.mavlink_bridge_state = "Stopped"
         self.object_manager_state = "Stopped"
         self.serial_manager_state = "Stopped"
+        self.ros2_manager_state = "Stopped"
         self.socket_object_list = [] # 已有的 mavlink object 
         self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string
         self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 
@@ -246,6 +248,7 @@ class ControlPanel:
                 stdscr.addstr(3,  2, f"Mavlink Bridge State : {state.mavlink_bridge_state}")
                 stdscr.addstr(4,  2, f"Socket Object number : {len(state.socket_object_list)}")
                 stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}")
+                stdscr.addstr(3, 36, f"ROS2 Manager State   : {state.ros2_manager_state}")
 
                 # 顯示當前選單項目
                 start_line = 6
@@ -1133,6 +1136,20 @@ class Orchestrator:
         # === 3) serial_manager 部分的準備 ===
         self.plumber = sm.serial_manager()
 
+        # === 4) ros 部分的準備 ===
+        self.fc_ros_manager = mros.ros2_manager
+        if not self.fc_ros_manager.initialized:
+            self.fc_ros_manager.initialize()
+        self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
+            'position': 1.0,   
+            'attitude': 0.0,    
+            'velocity': 0.0,     
+            'battery': 1.0,   
+            'vfr_hud': 1.0,    
+            'mode': 0.0,       
+            'summary': 1.0,    
+        }
+
     def engageWholeSystem(self):
         """啟動整個系統"""
         # === 1) 面板部分的啟動 ===
@@ -1146,6 +1163,9 @@ class Orchestrator:
         # === 3) serial_manager 部分的啟動 ===
         self.plumber.start()
 
+        # === 4) ros 部分的啟動 ===
+        self.fc_ros_manager.start()
+
     def mainLoop(self):
         logger.info("Main orchestrator started <-")
         try:
@@ -1174,6 +1194,11 @@ class Orchestrator:
                 linked_serial_dict = self.plumber.get_serial_link()
                 self.panelState.linked_serial_dict = linked_serial_dict
 
+                if self.fc_ros_manager.running:
+                    self.panelState.ros2_manager_state = 'Running'
+                else:
+                    self.panelState.ros2_manager_state = 'Stopped'
+
                 # B. 更新載具列表（從 vehicle_registry 獲取）
                 self._update_vehicles_list()
 
@@ -1284,10 +1309,17 @@ class Orchestrator:
                     self.plumber.shutdown()
                 self.plumber.thread.join(timeout=2)
 
+            if self.fc_ros_manager.spin_thread.is_alive():
+                if self.fc_ros_manager.running:
+                    self.fc_ros_manager.stop()
+                self.fc_ros_manager.spin_thread.join(timeout=2)
+
             # 關閉面板執行緒
             if self.panel_thread.is_alive():
                 self.panel_thread.join(timeout=2)
 
+            time.sleep(0.5) # 等待各模組穩定關閉
+                
             logger.info("Main orchestrator END!")
 
     # =============== 面板動作 - Mavlink Object ===============
@@ -1560,6 +1592,6 @@ if __name__ == "__main__":
 2. 修正 udp port 在移除後仍被記錄為佔用的問題
 3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式
 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
-
+5. 系統納入 mavlink ROS2 Manager 模組
 
 '''

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -41,13 +41,13 @@ class PublishRateController:
     def __init__(self):
         # 各 topic 的發布間隔（秒）
         self.topic_intervals = {
-            'position': 0.5,       # GPS位置 2Hz
-            'attitude': 0.5,       # 姿態 2Hz
-            'velocity': 0.5,       # 速度 2Hz
-            'battery': 1.0,        # 電池 1Hz
-            'vfr_hud': 0.5,        # VFR HUD 2Hz
-            'mode': 1.0,           # 飛行模式 1Hz
-            'summary': 1.0,        # 載具摘要 1Hz
+            'position': 0.5,       # GPS位置
+            'attitude': 0.5,       # 姿態
+            'velocity': 0.5,       # 速度
+            'battery': 1.0,        # 電池
+            'vfr_hud': 0.5,        # VFR HUD
+            'mode': 1.0,           # 飛行模式
+            'summary': 1.0,        # 載具摘要
         }
         # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
         self.last_publish_time: Dict[tuple, float] = {}
@@ -66,13 +66,17 @@ class PublishRateController:
         key = (sysid, topic)
         now = time.time()
         
+        # 當間隔設定為0或負數時 關閉該 topic 的發布
+        interval = self.topic_intervals.get(topic, 0)
+        if interval <= 0:
+            return False 
+
         # 首次發布
         if key not in self.last_publish_time:
             self.last_publish_time[key] = now
             return True
         
         # 檢查時間間隔
-        interval = self.topic_intervals.get(topic, 1.0)
         if now - self.last_publish_time[key] >= interval:
             self.last_publish_time[key] = now
             return True
@@ -98,6 +102,7 @@ class VehicleStatusPublisher(Node):
     - 發布標準 ROS2 消息類型
     - 檢測訂閱者，按需發布
     """
+    topicString_prefix = f'/fc_network/vehicle'
     
     def __init__(self):
         super().__init__('vehicle_status_publisher')
@@ -169,7 +174,7 @@ class VehicleStatusPublisher(Node):
         """
         key = (sysid, topic)
         if key not in self.fc_publishers:
-            topic_name = f'/vehicle_status/sys{sysid}/{topic}'
+            topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
             publisher = self.create_publisher(msg_type, topic_name, qos)
             self.fc_publishers[key] = publisher
             logger.info(f"Created publisher: {topic_name}")
@@ -462,8 +467,8 @@ class fc_ros_manager:
         self.command_service: Optional[MavlinkCommandService] = None
         
         # Executor & Thread
-        self.executor: Optional[MultiThreadedExecutor] = None
         self.spin_thread: Optional[threading.Thread] = None
+        self.executor: Optional[MultiThreadedExecutor] = None
     
     def initialize(self):
         """初始化 ROS2 环境和 nodes"""
@@ -536,8 +541,6 @@ class fc_ros_manager:
             return False
         
         try:            
-            logger.info("Stopping fc_ros_manager...")
-            
             # 標記停止
             self.running = False
             
@@ -602,10 +605,10 @@ ros2_manager = fc_ros_manager()
 ================= 改版記錄 ============================
 
 2026.01.20
-1. 重構自 mavlinkPublish.py
-2. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態
-3. 添加頻率控制器 - 按需發布（2Hz 位置/姿態，1Hz 電池/摘要）
-4. 預留 MavlinkCommandService 結構（稍後實現）
-5. 提供 fc_ros_manager 統一管理介面
+1. 重構自 mavlinkPublish.py (該檔案將被棄用)
+2. 提供 fc_ros_manager 統一管理介面
+3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態
+4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
+5. 預留 MavlinkCommandService 結構（稍後實現）
 
 '''

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -55,7 +55,8 @@ class TestSubscriber(Node):
     
     def _create_subscriptions(self):
         """建立所有 topic 的訂閱者"""
-        base_topic = f'/vehicle_status/sys{self.sysid}'
+        
+        base_topic = f'{VehicleStatusPublisher.topicString_prefix}/sys{self.sysid}'
         
         # Position
         self.create_subscription(
@@ -300,11 +301,22 @@ def test_frequency_control():
     # 建立測試訂閱者
     test_sub = TestSubscriber(sysid=1)
 
+    # 修改頻率設定
+    publisher_node = ros2_manager.status_publisher
+    publisher_node.rate_controller.topic_intervals = {
+        'position': 1.5,   
+        'attitude': 1.0,    
+        'velocity': 1.0,     
+        'battery': 1.0,   
+        'vfr_hud': 0.5,    
+        'mode': 0.0,       
+        'summary': 0.0,    
+    }
+    
     # 啟動 publisher
     ros2_manager.start()
     
     print("\n--- 測試頻率控制，運行 3 秒 ---")
-    print("預期：position/attitude 約 6 條 (2Hz)，battery/mode/summary 約 3 條 (1Hz)")
     
     # 運行 3 秒
     start_time = time.time()
@@ -314,15 +326,12 @@ def test_frequency_control():
     
     # 檢查頻率
     print("\n--- 頻率分析 ---")
+    print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
     
-    # 2Hz topics (預期約 6 條)
     print("2Hz Topics (預期 ~6 條):")
     for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
-    
-    # 1Hz topics (預期約 3 條)
-    print("\n1Hz Topics (預期 ~3 條):")
     for topic in ['battery', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
@@ -466,7 +475,7 @@ def main():
     try:
         # 執行各項測試
         test_basic_publishing()
-        time.sleep(1)
+        # time.sleep(1)
         
         # test_frequency_control()
         # time.sleep(1)