|
|
|
|
@ -1,13 +1,11 @@
|
|
|
|
|
"""
|
|
|
|
|
MAVLink ROS2 Nodes
|
|
|
|
|
主要包含三個獨立的 ROS2 Node :
|
|
|
|
|
主要包含兩個獨立的 ROS2 Node :
|
|
|
|
|
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
|
|
|
|
|
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
|
|
|
|
|
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
|
|
|
|
|
以最基礎的 mavlink microservice 會實現目標
|
|
|
|
|
並不會包含額外的功能
|
|
|
|
|
3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
|
|
|
|
|
過期丟棄、去重、節流、分片
|
|
|
|
|
|
|
|
|
|
與一個節點管理器
|
|
|
|
|
- fc_ros_manager
|
|
|
|
|
@ -17,7 +15,6 @@ MAVLink ROS2 Nodes
|
|
|
|
|
import os
|
|
|
|
|
import time
|
|
|
|
|
import math
|
|
|
|
|
import hashlib
|
|
|
|
|
import threading
|
|
|
|
|
from typing import Dict, Optional
|
|
|
|
|
|
|
|
|
|
@ -25,7 +22,6 @@ from typing import Dict, Optional
|
|
|
|
|
import rclpy
|
|
|
|
|
from rclpy.node import Node
|
|
|
|
|
from rclpy.executors import MultiThreadedExecutor
|
|
|
|
|
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
|
|
|
|
|
|
|
|
|
|
# ROS2 Message imports
|
|
|
|
|
import std_msgs.msg
|
|
|
|
|
@ -65,7 +61,7 @@ class PublishRateController:
|
|
|
|
|
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
|
|
|
|
|
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
|
|
|
|
|
self.topic_intervals = {
|
|
|
|
|
'position_gnss': 0.0, # GNSS位置
|
|
|
|
|
'position': 0.0, # GNSS位置
|
|
|
|
|
'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
|
|
|
|
|
'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
|
|
|
|
|
'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
|
|
|
|
|
@ -135,8 +131,8 @@ class VehicleStatusPublisher(Node):
|
|
|
|
|
self.fc_publishers: Dict[tuple, any] = {}
|
|
|
|
|
|
|
|
|
|
# 定時器：以較高頻率檢查 vehicle_registry 並發布
|
|
|
|
|
# 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
|
|
|
|
|
self.timer_period = 0.05 # 50ms
|
|
|
|
|
# 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
|
|
|
|
|
self.timer_period = 0.1 # 100ms
|
|
|
|
|
self.timer = self.create_timer(self.timer_period, self.timer_callback)
|
|
|
|
|
|
|
|
|
|
# 狀態標誌
|
|
|
|
|
@ -175,7 +171,7 @@ class VehicleStatusPublisher(Node):
|
|
|
|
|
# 例如：self._publish_ekf_status(sysid, status)
|
|
|
|
|
# ═══════════════════════════════════════════════════════════════
|
|
|
|
|
# 發布各種狀態（通過頻率控制器判斷是否發布）
|
|
|
|
|
self._publish_position_gnss(sysid, status)
|
|
|
|
|
self._publish_position(sysid, status)
|
|
|
|
|
self._publish_position_ned(sysid, status)
|
|
|
|
|
self._publish_attitude(sysid, status)
|
|
|
|
|
self._publish_velocity(sysid, status)
|
|
|
|
|
@ -206,9 +202,9 @@ class VehicleStatusPublisher(Node):
|
|
|
|
|
logger.info(f"Created publisher: {topic_name}")
|
|
|
|
|
return self.fc_publishers[key]
|
|
|
|
|
|
|
|
|
|
def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
|
|
|
|
|
def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
|
|
|
|
|
"""發布 GPS 位置"""
|
|
|
|
|
if not self.rate_controller.should_publish(sysid, 'position_gnss'):
|
|
|
|
|
if not self.rate_controller.should_publish(sysid, 'position'):
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
pos = status.position
|
|
|
|
|
@ -216,27 +212,22 @@ class VehicleStatusPublisher(Node):
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
publisher = self._get_or_create_publisher(
|
|
|
|
|
sysid, 'position_gnss', fcmsg.GnssRaw
|
|
|
|
|
sysid, 'position', sensor_msgs.msg.NavSatFix
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
# 檢查是否有訂閱者
|
|
|
|
|
if publisher.get_subscription_count() == 0:
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
# GPS 狀態資訊
|
|
|
|
|
gps = status.gps
|
|
|
|
|
if gps.fix_type is not None:
|
|
|
|
|
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')
|
|
|
|
|
msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
|
|
|
|
|
|
|
|
|
|
publisher.publish(msg)
|
|
|
|
|
|
|
|
|
|
@ -440,7 +431,7 @@ class VehicleStatusPublisher(Node):
|
|
|
|
|
# 'longitude': status.position.longitude if status.position.longitude else 0.0,
|
|
|
|
|
# 'altitude': status.position.altitude if status.position.altitude else 0.0,
|
|
|
|
|
# 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
|
|
|
|
|
# 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
|
|
|
|
|
'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
|
|
|
|
|
'connection_type': vehicle.connected_via.value,
|
|
|
|
|
'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
|
|
|
|
|
}
|
|
|
|
|
@ -959,179 +950,6 @@ class MavlinkCommandService(Node):
|
|
|
|
|
logger.info("MavlinkCommandService stopped")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ============================================================================
|
|
|
|
|
# RtcmRelay Node
|
|
|
|
|
# ============================================================================
|
|
|
|
|
|
|
|
|
|
class RtcmRelay(Node):
|
|
|
|
|
"""
|
|
|
|
|
RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
|
|
|
|
|
|
|
|
|
|
職責:
|
|
|
|
|
- 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
|
|
|
|
|
- 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
|
|
|
|
|
- 去重：與上一筆 payload blake2s hash 相同則丟棄
|
|
|
|
|
- 節流：最短轉發間隔保護（可參數化）
|
|
|
|
|
- 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
|
|
|
|
|
|
|
|
|
|
MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
|
|
|
|
|
超過 180 bytes 需分片，最多 4 片 = 720 bytes
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
FRAGMENT_MAX_LEN = 180
|
|
|
|
|
MAX_TOTAL_LEN = 4 * 180 # 720 bytes
|
|
|
|
|
STALE_THRESHOLD_SEC = 1.0
|
|
|
|
|
|
|
|
|
|
def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
|
|
|
|
|
super().__init__('rtcm_relay')
|
|
|
|
|
|
|
|
|
|
self.running = True
|
|
|
|
|
self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
|
|
|
|
|
self.clock_offset_sec = clock_offset_ms / 1000.0
|
|
|
|
|
|
|
|
|
|
# 去重：上一筆 payload 的短 hash
|
|
|
|
|
self._last_hash: Optional[bytes] = None
|
|
|
|
|
|
|
|
|
|
# 節流：上次成功轉發的時間
|
|
|
|
|
self._last_forward_time: float = 0.0
|
|
|
|
|
|
|
|
|
|
# MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
|
|
|
|
|
self._seq: int = 0
|
|
|
|
|
|
|
|
|
|
# 觀測 counters
|
|
|
|
|
self.counters = {
|
|
|
|
|
'received': 0,
|
|
|
|
|
'forwarded': 0,
|
|
|
|
|
'dropped_stale': 0,
|
|
|
|
|
'dropped_duplicate': 0,
|
|
|
|
|
'dropped_throttle': 0,
|
|
|
|
|
'dropped_oversize': 0,
|
|
|
|
|
'no_route': 0,
|
|
|
|
|
'send_fail': 0,
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
# QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
|
|
|
|
|
rtcm_qos = QoSProfile(
|
|
|
|
|
history=HistoryPolicy.KEEP_LAST,
|
|
|
|
|
depth=1,
|
|
|
|
|
reliability=ReliabilityPolicy.BEST_EFFORT,
|
|
|
|
|
durability=DurabilityPolicy.VOLATILE,
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
self.subscription = self.create_subscription(
|
|
|
|
|
mavros_msgs.msg.RTCM,
|
|
|
|
|
'/fc_network/rtcm_input',
|
|
|
|
|
self._rtcm_callback,
|
|
|
|
|
rtcm_qos,
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
logger.info("RtcmRelay initialized")
|
|
|
|
|
|
|
|
|
|
# ── callback ──────────────────────────────────────────────────────
|
|
|
|
|
|
|
|
|
|
def _rtcm_callback(self, msg):
|
|
|
|
|
"""處理順序：過期檢查 → 去重 → 節流 → 轉發"""
|
|
|
|
|
if not self.running:
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
self.counters['received'] += 1
|
|
|
|
|
data = bytes(msg.data)
|
|
|
|
|
now = time.time()
|
|
|
|
|
|
|
|
|
|
# 1) 過期丟棄
|
|
|
|
|
stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
|
|
|
|
|
age = (now + self.clock_offset_sec) - stamp_sec
|
|
|
|
|
if age > self.STALE_THRESHOLD_SEC:
|
|
|
|
|
self.counters['dropped_stale'] += 1
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# 2) 去重：blake2s 短 hash 與上一筆比對
|
|
|
|
|
h = hashlib.blake2s(data, digest_size=8).digest()
|
|
|
|
|
if h == self._last_hash:
|
|
|
|
|
self.counters['dropped_duplicate'] += 1
|
|
|
|
|
return
|
|
|
|
|
self._last_hash = h
|
|
|
|
|
|
|
|
|
|
# 3) 節流：最短轉發間隔
|
|
|
|
|
if self.min_forward_interval_sec > 0:
|
|
|
|
|
if (now - self._last_forward_time) < self.min_forward_interval_sec:
|
|
|
|
|
self.counters['dropped_throttle'] += 1
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# 4) 超長丟棄
|
|
|
|
|
if len(data) > self.MAX_TOTAL_LEN:
|
|
|
|
|
self.counters['dropped_oversize'] += 1
|
|
|
|
|
logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# 5) 取得本次的分片序號，然後遞增給下一筆
|
|
|
|
|
seq = self._seq
|
|
|
|
|
self._seq = (self._seq + 1) & 0x1F
|
|
|
|
|
|
|
|
|
|
# 6) 轉發給所有載具
|
|
|
|
|
forwarded_any = False
|
|
|
|
|
for sysid, vehicle in mvv.vehicle_registry.read_all():
|
|
|
|
|
socket_id = vehicle.custom_meta.get("socket_id")
|
|
|
|
|
if socket_id is None:
|
|
|
|
|
self.counters['no_route'] += 1
|
|
|
|
|
continue
|
|
|
|
|
|
|
|
|
|
mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
|
|
|
|
|
if mav_obj is None:
|
|
|
|
|
self.counters['no_route'] += 1
|
|
|
|
|
continue
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
self._send_rtcm(mav_obj, data, seq)
|
|
|
|
|
forwarded_any = True
|
|
|
|
|
except Exception as e:
|
|
|
|
|
self.counters['send_fail'] += 1
|
|
|
|
|
logger.debug(f"RTCM send fail sysid={sysid}: {e}")
|
|
|
|
|
|
|
|
|
|
if forwarded_any:
|
|
|
|
|
self.counters['forwarded'] += 1
|
|
|
|
|
self._last_forward_time = now
|
|
|
|
|
|
|
|
|
|
# ── MAVLink 編碼與分片 ────────────────────────────────────────────
|
|
|
|
|
|
|
|
|
|
def _send_rtcm(self, mav_obj, data: bytes, seq: int):
|
|
|
|
|
"""
|
|
|
|
|
對單一載具送出 GPS_RTCM_DATA 必要時做分片
|
|
|
|
|
|
|
|
|
|
GPS_RTCM_DATA flags 欄位:
|
|
|
|
|
bit 0 : 1=fragmented
|
|
|
|
|
bit 1-2 : fragment_id (0~3)
|
|
|
|
|
bit 3-7 : sequence number (0~31)
|
|
|
|
|
"""
|
|
|
|
|
if len(data) <= self.FRAGMENT_MAX_LEN:
|
|
|
|
|
flags = (seq & 0x1F) << 3
|
|
|
|
|
padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
|
|
|
|
|
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
|
|
|
|
|
else:
|
|
|
|
|
fragments = [
|
|
|
|
|
data[i:i + self.FRAGMENT_MAX_LEN]
|
|
|
|
|
for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
|
|
|
|
|
]
|
|
|
|
|
for frag_id, frag_data in enumerate(fragments):
|
|
|
|
|
flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
|
|
|
|
|
padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
|
|
|
|
|
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
|
|
|
|
|
|
|
|
|
|
# ── 外部介面 ──────────────────────────────────────────────────────
|
|
|
|
|
|
|
|
|
|
def set_clock_offset(self, offset_ms: float):
|
|
|
|
|
"""更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
|
|
|
|
|
self.clock_offset_sec = offset_ms / 1000.0
|
|
|
|
|
|
|
|
|
|
def get_counters(self) -> dict:
|
|
|
|
|
return dict(self.counters)
|
|
|
|
|
|
|
|
|
|
def stop(self):
|
|
|
|
|
"""停止轉發"""
|
|
|
|
|
self.running = False
|
|
|
|
|
logger.info("RtcmRelay stopped")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# ============================================================================
|
|
|
|
|
# ROS2 節點管理器
|
|
|
|
|
# ============================================================================
|
|
|
|
|
@ -1145,10 +963,9 @@ class fc_ros_manager:
|
|
|
|
|
stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
|
|
|
|
|
shutdown 是完全關閉 ROS2 並銷毀節點實例。
|
|
|
|
|
|
|
|
|
|
管理三個獨立的 ROS2 Node：
|
|
|
|
|
管理兩個獨立的 ROS2 Node：
|
|
|
|
|
- VehicleStatusPublisher
|
|
|
|
|
- MavlinkCommandService
|
|
|
|
|
- RtcmRelay
|
|
|
|
|
|
|
|
|
|
提供統一的啟動/停止介面給 mainOrchestrator
|
|
|
|
|
"""
|
|
|
|
|
@ -1157,10 +974,9 @@ class fc_ros_manager:
|
|
|
|
|
self.initialized = False
|
|
|
|
|
self.running = False
|
|
|
|
|
|
|
|
|
|
# node 實例
|
|
|
|
|
# 两个 node 實例
|
|
|
|
|
self.status_publisher: Optional[VehicleStatusPublisher] = None
|
|
|
|
|
self.command_service: Optional[MavlinkCommandService] = None
|
|
|
|
|
self.rtcm_relay: Optional[RtcmRelay] = None
|
|
|
|
|
|
|
|
|
|
# Executor & Thread
|
|
|
|
|
self.spin_thread: Optional[threading.Thread] = None
|
|
|
|
|
@ -1180,13 +996,11 @@ class fc_ros_manager:
|
|
|
|
|
# 創建節點 node
|
|
|
|
|
self.status_publisher = VehicleStatusPublisher()
|
|
|
|
|
self.command_service = MavlinkCommandService()
|
|
|
|
|
self.rtcm_relay = RtcmRelay()
|
|
|
|
|
|
|
|
|
|
# 創建執行者 MultiThreadedExecutor 並把 node 加入其中
|
|
|
|
|
self.executor = MultiThreadedExecutor()
|
|
|
|
|
self.executor.add_node(self.status_publisher)
|
|
|
|
|
self.executor.add_node(self.command_service)
|
|
|
|
|
self.executor.add_node(self.rtcm_relay)
|
|
|
|
|
|
|
|
|
|
self.initialized = True
|
|
|
|
|
# logger.info("fc_ros_manager initialized")
|
|
|
|
|
@ -1250,8 +1064,6 @@ class fc_ros_manager:
|
|
|
|
|
self.status_publisher.stop()
|
|
|
|
|
if self.command_service:
|
|
|
|
|
self.command_service.stop()
|
|
|
|
|
if self.rtcm_relay:
|
|
|
|
|
self.rtcm_relay.stop()
|
|
|
|
|
|
|
|
|
|
# 等待 spin 執行緒結束
|
|
|
|
|
if self.spin_thread and self.spin_thread.is_alive():
|
|
|
|
|
@ -1276,8 +1088,6 @@ class fc_ros_manager:
|
|
|
|
|
self.status_publisher.destroy_node()
|
|
|
|
|
if self.command_service:
|
|
|
|
|
self.command_service.destroy_node()
|
|
|
|
|
if self.rtcm_relay:
|
|
|
|
|
self.rtcm_relay.destroy_node()
|
|
|
|
|
|
|
|
|
|
# 關閉 ROS2
|
|
|
|
|
if rclpy.ok():
|
|
|
|
|
@ -1295,7 +1105,6 @@ class fc_ros_manager:
|
|
|
|
|
'running': self.running,
|
|
|
|
|
'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
|
|
|
|
|
'command_service_active': self.command_service is not None,
|
|
|
|
|
'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running,
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -1326,11 +1135,6 @@ ros2_manager = fc_ros_manager()
|
|
|
|
|
3. 新增 _publish_position_ned() 發布剛體座標的訊息
|
|
|
|
|
4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
|
|
|
|
|
|
|
|
|
|
2026.05.11
|
|
|
|
|
1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
|
|
|
|
|
2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
|
|
|
|
|
3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
|
|
|
|
|
|
|
|
|
|
TODO
|
|
|
|
|
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
|
|
|
|
|
2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題
|
|
|
|
|
|