@ -37,7 +37,7 @@ import mavros_msgs.msg
# ROS2 Service imports # ROS2 Service imports
import fc_interfaces . srv as fcsrv import fc_interfaces . srv as fcsrv
import fc_interfaces . msg as fcmsg import fc_interfaces . msg as fcmsg
from .ackEnum import s erviceAckResultfrom fc_interfaces. msg import S erviceAckResult
# 自定義 imports # 自定義 imports
from . import mavlinkVehicleView as mvv from . import mavlinkVehicleView as mvv
@ -45,7 +45,10 @@ from . import mavlinkObject as mo
from . utils import setup_logger from . utils import setup_logger
logger = setup_logger ( os . path . basename ( __file__ ) ) logger = setup_logger ( os . path . basename ( __file__ ) )
MODULE_VER = " 1.50 " MODULE_VER = " 2.10 "
FC_ROS_DOMAIN_ID = " 0 "
NODE_KEYS = ( " status_publisher " , " command_service " , " rtcm_relay " )
# ============================================================================ # ============================================================================
# VehicleStatusPublisher Node # VehicleStatusPublisher Node
@ -581,7 +584,7 @@ class MavlinkCommandService(Node):
self . handle_set_position_target_global_int
self . handle_set_position_target_global_int
)
)
logger . info ( " MavlinkCommandService initialized "
# logger.info("MavlinkCommandService initialized"
def _index ( self , target_sysid , target_compid ) :
def _index ( self , target_sysid , target_compid ) :
@ -608,20 +611,19 @@ class MavlinkCommandService(Node):
藉由 event . set ( ) 解開 service 中的 block
藉由 event . set ( ) 解開 service 中的 block
'''
'''
return_tuple = mo . return_packet_ring . get ( )
return_tuple = mo . return_packet_ring . get ( )
# 確認 return_packet_ring 有資料
if return_tuple == None : return
if return_tuple == None : return
socketid , timestamp , msg = return_tuple
socketid , timestamp , msg = return_tuple
sysid = msg . get_srcSystem ( )
sysid = msg . get_srcSystem ( )
_pending = self . _pending_by_sysid . get ( sysid )
_pending = self . _pending_by_sysid . get ( sysid )
# 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略
if _pending is None :
if _pending == None :
return
return
if _pending . match_fn ( msg ) :
if _pending . match_fn ( msg ) :
_pending . result_mav_msg = msg
_pending . result_mav_msg = msg
_pending . event . set ( )
_pending . event . set ( )
return
# ═══════════════════════════════════════════════════════════════════════
# ═══════════════════════════════════════════════════════════════════════
# Service Handler: 發送 TIMESYNC 可以作為模板
# Service Handler: 發送 TIMESYNC 可以作為模板
@ -706,20 +708,20 @@ class MavlinkCommandService(Node):
# 設定失效回應
# 設定失效回應
response . success = False
response . success = False
response . message = " Unknown error "
response . message = " Unknown error "
response . ack_result = serviceAckResult. UNKNOWN
response . ack_result = ServiceAckResult. FCSYS_ UNKNOWN
timeout_sec = request . timeout_sec
timeout_sec = request . timeout_sec
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
if request . target_sysid in self . _pending_by_sysid :
if request . target_sysid in self . _pending_by_sysid :
response . message = f " sysid { request . target_sysid } already has pending request "
response . message = f " sysid { request . target_sysid } already has pending request "
response . ack_result = serviceAckResult. MAVLINK _BUSY
response . ack_result = ServiceAckResult. FCSYS _BUSY
return response
return response
# 2) 找到 socket 標地
# 2) 找到 socket 標地
socketObject = self . _index ( request . target_sysid , request . target_compid )
socketObject = self . _index ( request . target_sysid , request . target_compid )
if socketObject is None :
if socketObject is None :
response . message = " This system id not found. "
response . message = " This system id not found. "
response . ack_result = serviceAckResult. MAVLINK_DEV_NOT FOUND
response . ack_result = ServiceAckResult. FCSYS_DEVICE_NOT_ FOUND
return response
return response
# 3) 接收封包系統 的設定
# 3) 接收封包系統 的設定
@ -750,12 +752,13 @@ class MavlinkCommandService(Node):
# 5) 等待回應封包
# 5) 等待回應封包
if not evt . wait ( timeout_sec ) :
if not evt . wait ( timeout_sec ) :
response . message = " mavlink timeout - CommLONG "
response . message = " mavlink timeout - CommLONG "
response . ack_result = serviceAckResult. MAVLINK_TIMEOUT
response . ack_result = ServiceAckResult. FCSYS_ MAVLINK_TIMEOUT
fail_skip = True
fail_skip = True
# 6) 處理回應封包
# 6) 處理回應封包
if not fail_skip :
if not fail_skip :
ack_msg = _pending . result_mav_msg
ack_msg = _pending . result_mav_msg
response . success = ServiceAckResult . FCSYS_MAVLINK_BREAK # dev 故意觸發錯誤用的
response . success = ( ack_msg . result == 0 ) # mavutil.mavlink.MAV_RESULT_ACCEPTED
response . success = ( ack_msg . result == 0 ) # mavutil.mavlink.MAV_RESULT_ACCEPTED
response . message = " " # 沒有消息就是好消息
response . message = " " # 沒有消息就是好消息
response . ack_result = ack_msg . result
response . ack_result = ack_msg . result
@ -779,20 +782,20 @@ class MavlinkCommandService(Node):
'''
'''
# 設定失效回應
# 設定失效回應
response . message = " Unknown error "
response . message = " Unknown error "
response . ack_result = serviceAckResult. UNKNOWN
response . ack_result = ServiceAckResult. FCSYS_ UNKNOWN
timeout_sec = request . timeout_sec
timeout_sec = request . timeout_sec
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
# 1) 確認是否已經有相同 sysid 的其他需求正在 pending
if request . target_sysid in self . _pending_by_sysid :
if request . target_sysid in self . _pending_by_sysid :
response . message = f " sysid { request . target_sysid } already has pending request "
response . message = f " sysid { request . target_sysid } already has pending request "
response . ack_result = serviceAckResult. MAVLINK _BUSY
response . ack_result = ServiceAckResult. FCSYS _BUSY
return response
return response
# 2) 找到 socket 標地
# 2) 找到 socket 標地
socketObject = self . _index ( request . target_sysid , request . target_compid )
socketObject = self . _index ( request . target_sysid , request . target_compid )
if socketObject is None :
if socketObject is None :
response . message = " This system id not found. "
response . message = " This system id not found. "
response . ack_result = serviceAckResult. MAVLINK_DEV_NOT FOUND
response . ack_result = ServiceAckResult. FCSYS_DEVICE_NOT_ FOUND
return response
return response
# 3) 接收封包系統 的設定
# 3) 接收封包系統 的設定
@ -827,7 +830,7 @@ class MavlinkCommandService(Node):
# 5) 等待回應封包
# 5) 等待回應封包
if not evt . wait ( timeout_sec ) :
if not evt . wait ( timeout_sec ) :
response . message = " mavlink timeout - SET POSITION 86 "
response . message = " mavlink timeout - SET POSITION 86 "
response . ack_result = MAVLINK_SEND_BUT_NO_EXP_STEAM
response . ack_result = ServiceAckResult. FCSYS_NO_EXPECTED_FEEDBACK
fail_skip = True
fail_skip = True
# 6) 處理回應封包
# 6) 處理回應封包
@ -835,7 +838,7 @@ class MavlinkCommandService(Node):
ack_msg = _pending . result_mav_msg
ack_msg = _pending . result_mav_msg
response . message = " "
response . message = " "
response . ack_result = 0
response . ack_result = ServiceAckResult . FCSYS_GET_EXPECTED_FEEDBACK
response . r_time_boot_ms = ack_msg . time_boot_ms
response . r_time_boot_ms = ack_msg . time_boot_ms
response . r_type_mask = ack_msg . type_mask
response . r_type_mask = ack_msg . type_mask
response . r_lat_int = ack_msg . lat_int
response . r_lat_int = ack_msg . lat_int
@ -952,11 +955,10 @@ class MavlinkCommandService(Node):
return response
return response
def stop ( self ) :
def stop ( self ) :
""" 停止服務 """
""" 停止服務 """
self . running = False
self . running = False
logger . info ( " MavlinkCommandService stopped "
# logger.info("MavlinkCommandService stopped"
# ============================================================================ # ============================================================================
@ -1025,7 +1027,7 @@ class RtcmRelay(Node):
rtcm_qos ,
rtcm_qos ,
)
)
logger . info ( " RtcmRelay initialized "
# logger.info("RtcmRelay initialized"
# ── callback ──────────────────────────────────────────────────────
# ── callback ──────────────────────────────────────────────────────
@ -1129,7 +1131,7 @@ class RtcmRelay(Node):
def stop ( self ) :
def stop ( self ) :
""" 停止轉發 """
""" 停止轉發 """
self . running = False
self . running = False
logger . info ( " RtcmRelay stopped "
# logger.info("RtcmRelay stopped"
# ============================================================================ # ============================================================================
@ -1145,7 +1147,7 @@ class fc_ros_manager:
stop 是停下止 ROS2 nodes 的運行 ， 但不銷毀節點實例 ， 允許後續再次 start 。
stop 是停下止 ROS2 nodes 的運行 ， 但不銷毀節點實例 ， 允許後續再次 start 。
shutdown 是完全關閉 ROS2 並銷毀節點實例 。
shutdown 是完全關閉 ROS2 並銷毀節點實例 。
管理三個獨立的 ROS2 Node ：
管理三個獨立的 ROS2 Node :
- VehicleStatusPublisher
- VehicleStatusPublisher
- MavlinkCommandService
- MavlinkCommandService
- RtcmRelay
- RtcmRelay
@ -1156,6 +1158,9 @@ class fc_ros_manager:
def __init__ ( self ) :
def __init__ ( self ) :
self . initialized = False
self . initialized = False
self . running = False
self . running = False
self . _restarting = False
self . _pending_restart : Optional [ str ] = None
self . _restart_lock = threading . Lock ( )
# node 實例
# node 實例
self . status_publisher : Optional [ VehicleStatusPublisher ] = None
self . status_publisher : Optional [ VehicleStatusPublisher ] = None
@ -1173,7 +1178,7 @@ class fc_ros_manager:
return False
return False
try :
try :
# 初始化 ROS2
os . environ . setdefault ( " ROS_DOMAIN_ID " , FC_ROS_DOMAIN_ID )
if not rclpy . ok ( ) :
if not rclpy . ok ( ) :
rclpy . init ( args = None )
rclpy . init ( args = None )
@ -1209,6 +1214,10 @@ class fc_ros_manager:
try :
try :
self . running = True
self . running = True
self . status_publisher . running = True
self . command_service . running = True
self . rtcm_relay . running = True
self . spin_thread = threading . Thread (
self . spin_thread = threading . Thread (
target = self . _spin_executor ,
target = self . _spin_executor ,
daemon = True ,
daemon = True ,
@ -1224,21 +1233,102 @@ class fc_ros_manager:
self . running = False
self . running = False
return False
return False
# 給 orchestrator 呼叫的函數
def schedule_restart_node ( self , node_key : str ) - > bool :
""" 由 orchestrator 排程單一 node 重啟（於 spin thread 下一輪執行） """
if node_key not in NODE_KEYS :
logger . warning ( f " schedule_restart_node: unknown key { node_key !r} " )
return False
if not self . running :
logger . warning ( " schedule_restart_node: fc_ros_manager not running " )
return False
if not self . spin_thread or not self . spin_thread . is_alive ( ) :
logger . warning ( " schedule_restart_node: spin thread not alive " )
return False
with self . _restart_lock :
self . _pending_restart = node_key
logger . info ( f " schedule_restart_node: queued { node_key } " )
return True
# 循環執行的地方
# 循環執行的地方
def _spin_executor ( self ) :
def _spin_executor ( self ) :
""" 在 thread 中運行的 executor """
""" 在 thread 中運行的 executor """
try :
try :
# logger.info("ROS2 executor spinning...")
while self . running :
while self . running :
self . executor . spin_once ( timeout_sec = 0.1 )
self . executor . spin_once ( timeout_sec = 0.1 )
pending = None
with self . _restart_lock :
if self . _pending_restart :
pending = self . _pending_restart
self . _pending_restart = None
if pending :
self . _restart_node ( pending )
elif self . command_service and not self . _restarting :
self . command_service . return_router ( )
self . command_service . return_router ( )
except Exception as e :
except Exception as e :
logger . error ( f " fc_ros_manager error in spinning executor: { e } " )
logger . error ( f " fc_ros_manager error in spinning executor: { e } " )
finally :
self . running = False
def _restart_node ( self , node_key : str ) - > bool :
""" 僅由 spin thread 呼叫 : destroy 並重建單一 node """
old_node = getattr ( self , node_key , None )
if old_node is None :
logger . error ( f " _restart_node: { node_key } is None " )
return False
# 重置前保留舊有或處理設定
saved_topic_intervals = None
if node_key == " status_publisher " :
saved_topic_intervals = dict ( old_node . rate_controller . topic_intervals )
# 如果是 command_service 清除所有等待的 pending 並且清除 return message
if node_key == " command_service " :
old_node . _pending_by_sysid . clear ( )
for sockObj in mo . mavlink_object . mavlinkObjects . values ( ) :
sockObj . set_return_message_types ( [ ] )
try :
self . _restarting = True
# 停止運作 移出執行迴圈 銷毀資源
old_node . stop ( )
self . executor . remove_node ( old_node )
old_node . destroy_node ( )
# 建立新的 node
if node_key == " status_publisher " :
new_node = VehicleStatusPublisher ( )
if saved_topic_intervals is not None :
new_node . rate_controller . topic_intervals = saved_topic_intervals
elif node_key == " command_service " :
new_node = MavlinkCommandService ( )
elif node_key == " rtcm_relay " :
new_node = RtcmRelay ( )
else :
return False
# 加回去管理系統中
setattr ( self , node_key , new_node )
self . executor . add_node ( new_node )
logger . info ( f " _restart_node: { node_key } restarted " )
return True
except Exception as e :
logger . error ( f " _restart_node failed for { node_key } : { e } " )
return False
finally :
self . _restarting = False
def stop ( self ) :
def stop ( self ) :
""" 停止 ROS2 nodes """
""" 停止 ROS2 nodes """
if not self . running :
# if not self.running:
logger . warning ( " fc_ros_manager not running " )
# logger.warning("fc_ros_manager not running")
# return False
if not self . spin_thread :
logger . warning ( " fc_ros_manager without thread to stop " )
return False
return False
try :
try :
@ -1289,7 +1379,7 @@ class fc_ros_manager:
except Exception as e :
except Exception as e :
logger . error ( f " Error during shutdown: { e } " )
logger . error ( f " Error during shutdown: { e } " )
def get_status ( self ) - > dict :
def get_status ( self ) :
return {
return {
' initialized ' : self . initialized ,
' initialized ' : self . initialized ,
' running ' : self . running ,
' running ' : self . running ,
@ -1299,6 +1389,12 @@ class fc_ros_manager:
}
}
# ============================================================================ # ============================================================================
# 全域實例 # 全域實例
# ============================================================================ # ============================================================================
@ -1331,8 +1427,13 @@ ros2_manager = fc_ros_manager()
2. 實作過期丟棄 ( 1 s ) 、 blake2s hash 去重 、 最短轉發間隔節流 、 分片 ( 180 bytes / 片 ) 2. 實作過期丟棄 ( 1 s ) 、 blake2s hash 去重 、 最短轉發間隔節流 、 分片 ( 180 bytes / 片 )
3. 接入 fc_ros_manager 的 initialize / start / stop / shutdown 生命週期 3. 接入 fc_ros_manager 的 initialize / start / stop / shutdown 生命週期
2026.05 .22
1. initialize ( ) 設定 ROS_DOMAIN_ID ( FC_ROS_DOMAIN_ID )
2. schedule_restart_node / _restart_node : 手動重啟單一 node ( spin thread 內執行 ）
3. orchestrator cmd : ( " RESTART_ROS_NODE " , node_key ) , node_key 見 NODE_KEYS
TODO TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
''' '''
