loam_interface（Point-LIO輸出的座標與話題適配）

1.loam_interface（Point-LIO輸出的座標與話題適配）

這個節點把 Point-LIO 的位姿與配準掃描接到後續導航介面：訂閱 aft_mapped_to_init 和 cloud_registered，釋出 lidar_odometry 和 registered_scan。它執行座標變換，不負責 SLAM 估計，也不直接釋出 odom → base_footprint 的 TF。

以下按 2026-10-10 當前原始碼與實物 YAML 核對。引數名與當前值可以確認，但 IMU、雷達和底盤的完整位姿轉換存在原始碼疑點，不能把話題有輸出等同於座標關係正確。

1.1.引數

1.1.1.ros__parameters（ROS節點引數）

1.1.1.1.參數列

loam_interface:
  ros__parameters:
    use_sim_time: False
    state_estimation_topic: "aft_mapped_to_init"
    registered_scan_topic: "cloud_registered"
    odom_frame: "odom"
    base_frame: "base_footprint"
    lidar_frame: "front_mid360"

六項都有作用，本塊沒有當前完全不參與執行的引數。五項業務引數只在構造時讀取，原始碼沒有執行時更新處理；執行中寫入同名引數不會重建訂閱或更新快取，需在另行授權的重啟中應用。use_sim_time 由 ROS2 管理，不屬於這五項。

1.1.1.2.引數詳解

1.1.1.2.1.use_sim_time（系統時間與模擬時間選擇）

False 使用系統時間；True 使用 /clock 提供的 ROS 時間。TF 快取用的是節點時鐘，Point-LIO、關節 TF 和本節點必須處於一致的時間域。

本節點不會用當前時間重新標記輸入位姿。切換時間源也不會修正雷達點時間單位、感測器時鐘同步或舊時間戳；此前模擬轉換器的秒／微秒不一致仍未解決。

1.1.1.2.2.state_estimation_topic 與 registered_scan_topic（輸入里程計與配準點雲）

兩訂閱和兩釋出的佇列深度均為5。輸入話題名是相對名，隨節點名稱空間和重對映解析；座標系字串不自動新增節點名稱空間。兩個輸出話題名在原始碼中固定，不能通過修改輸入引數改名。

位置與點座標沿用上游的 m，姿態使用無量綱四元數，時間戳由秒和納秒欄位表達；本節點不縮放單位、不再次降取樣，也不做兩種輸入的時間同步。里程計沿用輸入位姿時間戳，正常點雲轉換沿用輸入點雲時間戳；後者已對照容器安裝的 pcl_ros 2.4.5 對應實現核對。

本節點只寫輸出里程計的位姿，不轉傳輸入速度和協方差，這些欄位保持預設零值，不能據此判斷車輛靜止或估計沒有不確定性。

1.1.1.2.3.odom_frame（輸出里程計與點雲參考系）

當前保持 odom。節點對點雲執行剛體變換，對位姿左乘同一快取矩陣，然後填寫輸出參考系名稱。它沒有驗證輸入位姿的 header.frame_id 與 child_frame_id 是否符合預期，也沒有按輸入名稱查詢 camera_init → odom。

因此改為其他名字不會自動獲得該參考系的變換。odom 是輸出命名，實際軸向、原點和重力關係取決於快取與 Point-LIO 初始化，不能靠名稱判定正確。

1.1.1.2.4.base_frame（初始化基準的底盤座標系）

首次收到位姿時，節點按該訊息時間戳查詢 base_footprint ← front_mid360，最大等待0.5秒；失敗則告警並放棄本次位姿，後續位姿回撥重試。首次成功後，把當時的雷達安裝與關節變換存為固定矩陣 C，後續位姿和點雲都使用它。

這項指定初始對齊所依賴的機體系，不表示本節點輸出底盤位姿。當前 MID360 掛在 gimbal_yaw，所以該 TF 包含底盤到雲臺、機械 yaw 和雷達安裝關係；它隨 yaw 變化，不隨 pitch 變化。快取時刻也不一定等於 Point-LIO 初始化時刻，需要核對啟動時的位姿與關節狀態。

固定快取本身不等於錯誤：兩個固定參考系之間的對齊通常應保持固定，運動中的雷達→底盤變換由下游 sensor_scan_generation 按點雲時間查詢。需要確認的是 C 是否正確代表 odom 與 camera_init 的關係，不能簡單改成每幀跟隨雲臺更新。Point-LIO 的初始旋轉還受重力對齊影響，不能預設輸入初始姿態一定是單位旋轉。

1.1.1.2.5.lidar_frame（雷達座標系與輸出里程計子系）

當前 front_mid360 與模型中的雷達名稱一致，但上游 body 是內部 IMU。設 T_AB 表示把 B 系座標轉換到 A 系，W=camera_init、I=內部 IMU、L=front_mid360、O=odom，完整雷達位姿一般應滿足：

$$T_{OL}(t)=T_{OW}\,T_{WI}(t)\,T_{IL}$$

當前原始碼實際計算 C × 输入位姿，即 C × T_WI(t)，隨後將子系名設為 L；沒有右乘 T_IL，也沒有用初始輸入位姿核對 C=T_OW 的條件。

Point-LIO 配準點雲已經使用 mapping.extrinsic_R/T，而位姿釋出直接取濾波器的 IMU 位置和姿態。當前實物 extrinsic_R 為單位矩陣，extrinsic_T=[-0.011,-0.02329,+0.04412] m 非零；軸向一致不代表兩個原點相同。缺少這一步時，雷達繞 IMU 轉動的杆臂效應可能表現為位置誤差。單純改 lidar_frame 或話題名無法補齊外參；先記錄這一介面缺口，工程修改另行授權。

1.1.1.2.6.啟動配置與原始碼缺口

整套啟動的獨立程序和元件模式都載入同一份處理後的引數檔案；slam 選擇建圖或定位，不會跳過這段導航介面。本次在 ysu_ros_humble 中僅載入環境並查詢包路徑，確認相關安裝後的 launch 和 YAML 指向當前原始碼；沒有啟動節點，也沒有讀取執行節點的有效引數。

另一個已確認的原始碼缺口是初始化保護：點雲回撥沒有檢查初始 TF 是否成功，直接使用快取。容器的 tf2::Transform 預設構造不初始化數值，而節點也沒有先將快取設為單位變換。如果點雲先到、或首次 TF 查詢一直失敗，可能使用未初始化矩陣；觸發後的執行表現本輪未測試。

原始碼位置：loam_interface/src/loam_interface.cpp 的引數讀取、點雲回撥和位姿回撥；point_lio/src/laserMapping.cpp 的 set_posestamp/publish_odometry，以及 point_lio/src/Estimator.cpp 的 pointBodyToWorld。上述包均位於 src/pb2025_sentry_nav/。

1.1.1.2.7.已觀察與待核實事項

• 原始碼能確認： 六項引數的使用、輸入輸出話題、首次 TF 快取、IMU 位姿未補雷達外參、初始化保護缺失，以及 launch 配置來源。
• 模擬已觀察： 2026-10-08 的獨立 Point-LIO 檢查在靜止和約20 cm低速往返時有正常位姿與配準點雲；該檢查沒有覆蓋本節點和下游的完整轉換，也沒有獨立雲臺 yaw 驗證，不能據此認定這裡的介面正確。
• 必須實物核實： MID360 實際安裝位置與軸向、內部雷達／IMU 外參、機械 yaw/pitch 零位與方向、序列埠角度單位和含義、各訊息的時鐘一致性，以及 Point-LIO 重力目標參考系與導航 odom 的關係。當前約定要求 JointState 使用機械相對角，世界 IMU 姿態不能代替關節角；本輪不修改重力方向。

機械關係與Linux與MCU資料對接、MID360安裝位置一致：兩個odom關節固定，實際yaw在gimbal_yaw_joint、實際pitch在gimbal_pitch_joint，序列埠position為{relative_pitch, relative_yaw, 0, 0}。MID360掛在實際yaw關節之後，隨yaw、不隨pitch；不能用關節之前的gimbal_yaw_odom替代其掛載層。