From bbd120d25ae493eba132159e79d380dc1c6e1cfc Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 26 Jan 2026 13:16:21 +0800 Subject: [PATCH] =?UTF-8?q?(Tested)=20=E5=B0=87ROS2=20Topic=20=E7=B4=8D?= =?UTF-8?q?=E5=85=A5=20mainOrchestrator=20=E7=B3=BB=E7=B5=B1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 3 +- .../fc_network_adapter/mainOrchestrator.py | 44 ++++++++++++++++--- .../fc_network_adapter/mavlinkROS2Nodes.py | 39 ++++++++-------- .../tests/test_vehicleStatusPublisher.py | 23 +++++++--- 4 files changed, 77 insertions(+), 32 deletions(-) diff --git a/README.md b/README.md index ce0703a..78b7d1a 100644 --- a/README.md +++ b/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 + ``` + diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index c7b573f..2078490 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/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 = [] # 顯示在面板上的資訊訊息 @@ -240,12 +242,13 @@ class ControlPanel: stdscr.border() # 更新模組狀態顯示 - stdscr.addstr(0, 10, " MavLink MiddleWare ", curses.A_BOLD) - stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") - stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") - 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(0, 10, " MavLink MiddleWare ", curses.A_BOLD) + stdscr.addstr(1, 2, f" Panel Status : {state.panel_status}") + stdscr.addstr(2, 2, f"Object Manager State : {state.object_manager_state}") + 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,9 +1309,16 @@ 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!") @@ -1560,6 +1592,6 @@ if __name__ == "__main__": 2. 修正 udp port 在移除後仍被記錄為佔用的問題 3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 - +5. 系統納入 mavlink ROS2 Manager 模組 ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 496ce48..97ff5d0 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/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""" @@ -535,9 +540,7 @@ class fc_ros_manager: logger.warning("fc_ros_manager not running") return False - try: - logger.info("Stopping fc_ros_manager...") - + try: # 標記停止 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 結構（稍後實現） ''' diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py index 5c902bd..6fd1914 100644 --- a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py +++ b/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( @@ -299,12 +300,23 @@ 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)