sensor_scan_generation（雷達點雲換系與底盤、雲臺位姿轉換）

1.sensor_scan_generation（雷達點雲換系與底盤、雲臺位姿轉換）

這個節點把 loam_interface 輸出的雷達位姿和配準點雲近似配對，再結合機器人機械 TF，釋出底盤 TF、指定機體系的里程計和雷達系掃描點雲。當前底盤 TF 的子係為 base_footprint，odometry 訊息的子系則為 gimbal_yaw，兩者用途不同。

以下按原始碼與實物 YAML 核對。轉換成立的前提是上游輸入確實為雷達位姿；上一塊記錄的內部 IMU／雷達位姿介面缺口仍會影響本節點，不能在這裡靠改座標系名稱補齊。

1.1.引數

1.1.1.ros__parameters（ROS節點引數）

1.1.1.1.參數列

sensor_scan_generation:
  ros__parameters:
    use_sim_time: False
    lidar_frame: "front_mid360"
    base_frame: "base_footprint"
    robot_base_frame: "gimbal_yaw"

四項都有作用，本塊沒有當前完全不使用的引數。三個座標系引數只在構造時讀取，執行時寫入同名引數不會更新成員值；後續應用變更需另行授權重啟。座標系名稱不屬於數值調參，調大／調小均不適用。

1.1.1.2.引數詳解

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

False 使用系統時間；True 使用 /clock 的 ROS 時間。輸入位姿、配準掃描、機械 JointState／TF 和節點應使用一致的時間域，否則按訊息時間查詢機械 TF 可能失敗。

這個開關影響節點與 TF 快取時鐘，但原始碼計算速度時另用 std::chrono::steady_clock。所以 True 不會讓速度自動按模擬時間計算，也不會修復輸入點時間單位或時鐘同步問題。

1.1.1.2.2.lidar_frame（掃描點雲的目標雷達系）

輸入和輸出話題名在原始碼中固定為相對名，可受節點名稱空間或重對映影響；本塊沒有話題名引數，也沒有 odom_frame 引數。座標系字串不自動新增節點名稱空間。

sensor_scan 是配準掃描轉換回雷達系的結果，不會恢復已被 Point-LIO 丟棄的點，也不執行新的估計或運動補償。位置與點座標沿用 m，姿態為無量綱四元數，線速度欄位按 m/s、角速度欄位按 rad/s 表達；速度含義仍需結合下文的原始碼缺口判斷。

節點直接把輸入位姿當作雷達位姿，並用其逆變換處理點雲；沒有核驗輸入 child_frame_id 是否對應 lidar_frame，也沒有驗證兩種輸入的參考系一致。上游若仍是內部 IMU 位姿，雷達→底盤的機械 TF 不能代替缺失的 IMU→雷達外參。

1.1.1.2.3.base_frame（底盤位姿TF的目標系）

每個配對回撥按點雲時間戳查詢 front_mid360 ← base_footprint，最大等待0.5秒，不使用上游節點那種一次性初始化快取。設 T_AB 把 B 系座標轉換到 A 系，O 為輸入里程計參考系、L 為雷達、B 為底盤，則計算：

$$T_{OB}=T_{OL}\,T_{LB}$$

當前通過這項結果廣播 odom → base_footprint。當底盤靜止、只有實際 yaw 轉動時，正確的雷達位姿和機械相對 yaw 應能互相抵消，得到基本不動的底盤位姿；這是待驗證的轉換目標，本輪未進行該項測試。

當前模型中 base_footprint → chassis 是固定連線，高度為 chassis_height=0.0625 m。本節點保留完整三維變換，並不根據名稱把底盤投影到地面或強制水平。

兩次機械 TF 查詢都通過 getTransform；任一次失敗都會告警並返回單位變換，回撥仍繼續釋出。例如雷達→底盤查詢失敗時，底盤位姿會直接變成輸入雷達位姿，安裝偏移和關節旋轉未被扣除。有輸出不能證明機械 TF 查詢成功。

1.1.1.2.4.robot_base_frame（輸出雲臺層里程計的座標系）

每次回撥還按點雲時間查詢 front_mid360 ← gimbal_yaw，再計算：

$$T_{OR}=T_{OL}\,T_{LR}$$

R 為 robot_base_frame。該位姿寫入 odometry，因此當前訊息表示 yaw 雲臺層的原點和姿態。原始碼的釋出器變數雖叫 pub_chassis_odometry_，實際子系取的是 robot_base_frame，不能據變數名把它認作底盤裡程計。

MID360 固定在 gimbal_yaw，雷達→yaw 雲臺層的安裝關係固定；雷達→底盤關係則包含實際機械 yaw，隨關節角變化。實際 pitch 在 gimbal_pitch_joint，位於雷達掛載層之後，不應進入雷達到這兩個目標系的機械變換。兩個 odom 關節保持固定，JointState 繼續使用 {relative_pitch, relative_yaw, 0, 0}。

當前 fake_vel_transform 也配置為訂閱 odometry、使用 gimbal_yaw，兩者名稱相符。robot_base_frame 決定里程計訊息表達的機體系，不單獨釋出該機體系的 TF；已有機器人 TF 鏈連線 base_footprint 到 gimbal_yaw。雲臺獨立 yaw 的角運動不能直接當作底盤角運動。

1.1.1.2.5.同步與速度計算的原始碼疑點

兩訂閱使用 Best Effort、Keep Last 深度1；近似時間同步器 ApproximateTime(100) 的內部佇列長度為100；兩釋出佇列深度為2。這些都寫在原始碼中，沒有對應 YAML 引數，100 不是輸出頻率或100毫秒容差。

兩個輸入必須形成配對才進入釋出回撥。原始碼沒有設定最大配對時間差，也沒有將輸入位姿插值到點雲時間：位姿取配對的里程計訊息，機械 TF 按點雲時間查詢，輸出 TF 與 odometry 都標記為點雲時間。近似配對不等於同一取樣時刻；實際配對誤差和運動影響仍需驗證。

速度由連續兩次轉換後的 robot_base_frame 位姿差分得到，沒有使用輸入里程計的 twist。有三項明確的實現問題需要記錄：

• 時間基準： 分母是兩次釋出回撥之間的真實單調時間，不是兩幀訊息時間戳之差。模擬加速、減速、暫停或回撥積壓時，差分速度可能失真；use_sim_time=True 也不會改變這條計算路徑。
• 座標表達： 位置差在輸入參考系（當前 odom）中計算，姿態差使用 q_current × q_previous.inverse()，也在該參考系表達；寫入 twist 前沒有轉到 gimbal_yaw。而 ROS 的 Odometry 定義要求速度在 child_frame_id 中表達，當前實現與訊息約定不一致。
• 首幀處理： 沒有“已收到有效歷史位姿”的標誌，首幀也會進入差分；previous_transform 未用首幀位姿建立歷史，首次 previous_time 與當前時間又相隔很短。首個速度結果可能異常，不能把它作為有效測量。

輸出位姿和速度協方差均保持預設零值；原始碼沒有估計不確定性。上述實現問題可以由原始碼確認，實際速度誤差、首幀數值和對下游的影響本輪未執行測量。

1.1.1.2.6.啟動配置來源

獨立預設 base_frame=chassis 與當前整套配置不同。若同時運行當前 robot_state_publisher，已有固定的 base_footprint → chassis，再發布 odom → chassis 會讓同一子系出現不同父系的釋出；獨立預設值不能直接視為與整套啟動等價。

整套啟動的獨立程序和元件模式都使用同一份處理後的引數檔案，slam 不會跳過這個介面。本輪在 ysu_ros_humble 中僅載入環境並查詢包路徑，確認安裝後的本包 launch、導航 launch 和兩份 YAML 與當前原始碼一致；未啟動節點，也未讀取執行節點的有效引數。

1.1.1.2.7.已觀察與待核實事項

• 原始碼能確認： 四項引數均有作用；底盤 TF 與 yaw 雲臺層里程計的區別；機械 TF 按每次點雲時間查詢；TF 失敗繼續使用單位變換；近似配對方式；速度的時間、座標與首幀問題。
• 模擬已觀察： 2026-10-08 的靜止及約20 cm往返檢查覆蓋獨立 Point-LIO 輸出，沒有驗證本節點的 sensor_scan、底盤 TF、雲臺層里程計及差分速度；不能據此認定本介面正確。
• 必須實物核實： 雷達與雲臺安裝外參、機械相對 yaw 的零位／方向／弧度單位、關節與雷達資料的時鐘一致性，以及上游內部 IMU 位姿到雷達位姿的完整關係。後續獲准測試時，應區分底盤運動、獨立 yaw 和獨立 pitch，分別檢查輸出；當前重力方向與模擬點時間介面仍保留待核實狀態。

1.2.實際問題與解決方案

已確認的直接故障：

sensor_scan_generation::getTransform() 在 TF 查詢失敗時返回單位變換，呼叫者仍把這個結果用於釋出 odom → base_footprint。當用於雷達至底盤的查詢失敗時，釋出的底盤位姿接近直接複製雷達位姿，丟掉雷達到底盤的安裝旋轉與平移。TF 查詢恢復時則重新執行正常轉換，因此出現突然騰空、傾斜，隨後落回原位置的間歇性跳變。

它讀取的主要話題是：

它將前兩個話題按時間近似配對，然後以點雲時間戳查詢安裝關係。

準確地說，故障順序是：

1. sensor_scan_generation 查詢掃描時刻的雷達到底盤變換失敗，這條關係包含動態雲臺 yaw。
2. 程式碼用單位變換代替，繼續計算併發布 odom → base_footprint。
3. 底盤位姿因此近似變成雷達位姿，帶上雷達的安裝高度和約 45° 安裝傾角。
4. 查詢恢復後，底盤 TF 又恢復正常。 查詢失敗的是雷達與底盤之間的變換；最終“炸掉”的輸出是 odom → base_footprint。