@ -68,15 +68,16 @@ class PublishRateController:
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
self . topic_intervals = {
' position_gnss ' : 0.0 , # GNSS位置
' position_ned ' : 0.0 , # LOCAL_POSITION_NED (位置+速度)
' summary ' : 0.0 , # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
' position_gnss ' : 0.0 , # GNSS位置 (海拔高度)
' position_ned ' : 0.0 , # LOCAL_POSITION_NED (位置+速度+相對高度)
' attitude ' : 0.0 , # 姿態 (pitch yaw row 與其加速狀態)
' velocity ' : 0.0 , # 速度 (已經包含在 vfr_hud 未來移除)
' battery ' : 0.0 , # 電池
' vfr_hud ' : 0.0 , # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門)
' sys_diags ' : 0.0 , # SYS_STATUS 系統診斷
' status_text ' : 0.0 , # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
' mode ' : 0.0 , # 飛行模式 (已經在 summary 裡 未來移除)
' summary ' : 0.0 , # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
' system_diagnostics ' : 0.0 , # SYS_STATUS 系統診斷
' velocity ' : 0.0 , # 速度 (已經包含在 vfr_hud 未來移除)
# 在這裡新增更多 topics...
}
# 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
@ -113,6 +114,10 @@ class PublishRateController:
return False
def is_topic_enabled ( self , topic : str ) - > bool :
""" 檢查 topic 是否啟用（ ） """
return self . topic_intervals . get ( topic , 0 ) > 0
def reset ( self ) :
""" 重置所有計時器 """
self . last_publish_time . clear ( )
@ -123,7 +128,7 @@ class VehicleStatusPublisher(Node):
職責 :
- 定期從 vehicle_registry 讀取載具狀態
- 頻率控制 （ 位置 / 姿態 2 Hz ， 電池 / 摘要 1 Hz ）
- 頻率控制 ( 位置 / 姿態 2 Hz , 電池 / 摘要 1 Hz )
- 發布標準 ROS2 消息類型
- 檢測訂閱者 ， 按需發布
"""
@ -182,12 +187,13 @@ class VehicleStatusPublisher(Node):
self . _publish_position_gnss ( sysid , status )
self . _publish_position_ned ( sysid , status )
self . _publish_attitude ( sysid , status )
self . _publish_velocity ( sysid , status )
self . _publish_battery ( sysid , status )
self . _publish_vfr_hud ( sysid , status )
self . _publish_mode ( sysid , status )
self . _publish_summary ( vehicle )
self . _publish_system_diagnostics ( sysid , status )
self . _publish_status_text ( sysid , status )
self . _publish_velocity ( sysid , status )
self . _publish_mode ( sysid , status )
# 在這裡新增更多 publish 方法調用...
def _get_or_create_publisher ( self , sysid : int , topic : str , msg_type , qos : int = 1 ) :
@ -452,7 +458,7 @@ class VehicleStatusPublisher(Node):
def _publish_system_diagnostics ( self , sysid : int , status : mvv . ComponentStatus ) :
""" 發布 SYS_STATUS 系統診斷資訊 """
if not self . rate_controller . should_publish ( sysid , ' sys tem _diagnostic s' ) :
if not self . rate_controller . should_publish ( sysid , ' sys _diags' ) :
return
diag = status . sys_diag
@ -460,7 +466,7 @@ class VehicleStatusPublisher(Node):
return
publisher = self . _get_or_create_publisher (
sysid , ' sys tem _diagnostic s' , fcmsg . SystemDiagnosticsRaw
sysid , ' sys _diags' , fcmsg . SystemDiagnosticsRaw
)
if publisher . get_subscription_count ( ) == 0 :
@ -481,6 +487,31 @@ class VehicleStatusPublisher(Node):
publisher . publish ( msg )
def _publish_status_text ( self , sysid : int , status : mvv . ComponentStatus ) :
""" 發布 STATUSTEXT 飛控文字 (佇列 drain, 無訂閱者直接丟棄) """
# 是否啟用
if not self . rate_controller . is_topic_enabled ( ' status_text ' ) :
return
# 是否有資料
queue = status . status_text_queue
if not queue :
return
publisher = self . _get_or_create_publisher ( sysid , ' status_text ' , std_msgs . msg . String )
# 是否有監聽者
if publisher . get_subscription_count ( ) == 0 :
queue . clear ( )
return
while queue :
entry = queue . popleft ( )
msg = std_msgs . msg . String ( )
ts = entry . timestamp if entry . timestamp is not None else 0.0
sev = entry . severity if entry . severity is not None else - 1
msg . data = f ' [ { ts : .3f } ] [ { sev } ] { entry . text } '
publisher . publish ( msg )
# ═══════════════════════════════════════════════════════════════
# 【新增 Topic 位置 3/4】
# 若要新增 topic，
@ -499,29 +530,6 @@ class VehicleStatusPublisher(Node):
# # ... 實作發布邏輯
# ═══════════════════════════════════════════════════════════════
@staticmethod
def _euler_to_quaternion ( roll , pitch , yaw ) :
"""
歐拉角轉四元數
Args :
roll : 橫滾角 ( 弧度 )
pitch : 俯仰角 ( 弧度 )
yaw : 偏航角 ( 弧度 )
Returns :
tuple : ( qx , qy , qz , qw )
"""
qx = math . sin ( roll / 2 ) * math . cos ( pitch / 2 ) * math . cos ( yaw / 2 ) - \
math . cos ( roll / 2 ) * math . sin ( pitch / 2 ) * math . sin ( yaw / 2 )
qy = math . cos ( roll / 2 ) * math . sin ( pitch / 2 ) * math . cos ( yaw / 2 ) + \
math . sin ( roll / 2 ) * math . cos ( pitch / 2 ) * math . sin ( yaw / 2 )
qz = math . cos ( roll / 2 ) * math . cos ( pitch / 2 ) * math . sin ( yaw / 2 ) - \
math . sin ( roll / 2 ) * math . sin ( pitch / 2 ) * math . cos ( yaw / 2 )
qw = math . cos ( roll / 2 ) * math . cos ( pitch / 2 ) * math . cos ( yaw / 2 ) + \
math . sin ( roll / 2 ) * math . sin ( pitch / 2 ) * math . sin ( yaw / 2 )
return ( qx , qy , qz , qw )
def stop ( self ) :
""" 停止發布 """
self . running = False
@ -560,6 +568,14 @@ class MavlinkCommandService(Node):
每次接到一個 service 請求 要整個系統丟某種指令給載具時
會做兩件事 1. " 丟出mavlink封包 " 2. " 創造一個臨時信箱 Pending "
然後透過每次 manager spin
會去呼叫 return_router ( ) 方法
這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
配對到的解開 Pending
解開後 相對應的 handle_XXX 就會開始做事
"""
serviceString_prefix = ' /fc_network/vehicle '
@ -1182,6 +1198,10 @@ class fc_ros_manager:
- RtcmRelay
提供統一的啟動 / 停止介面給 mainOrchestrator
另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性
使得就算 executor 在跑一些需要等待的方法
常態的 spin_once 也不會被 block ( spin_thread 是另一個支線 )
"""
def __init__ ( self ) :
@ -1461,6 +1481,10 @@ ros2_manager = fc_ros_manager()
2. schedule_restart_node / _restart_node : 手動重啟單一 node ( spin thread 內執行 ）
3. orchestrator cmd : ( " RESTART_ROS_NODE " , node_key ) , node_key 見 NODE_KEYS
2026.06 .10
1. 增加了 _publish_system_diagnostics 與 _publish_status_text 功能
TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
