fake_vel_transform（雲臺系速度轉換與底盤自旋）

1.fake_vel_transform（雲臺系速度轉換與底盤自旋）

1.1.速度和旋轉究竟參考誰

當前導航鏈最終下發的 vx、vy 按實際雲臺自身座標系 gimbal_yaw 表達；wz 按介面約定是底盤的目標角速度。 “速度用誰的軸表達”與“哪個部件執行動作”是兩件事：用雲臺的軸描述平移，仍然是讓底盤移動；輸出旋轉指令也沒有直接控制雲臺 yaw 關節。

這來自北極熊原包的設計：先用一個朝向較穩定的虛擬系供導航計算，再把速度轉換到實際雲臺系；cmd_spin 用於疊加底盤自旋。北極熊原包說明、原包速度轉換原始碼。這是這套工程的約定，不能認為所有 ROS2 的 cmd_vel 都按雲臺系表達。

vx、vy 單位為 m/s，分別沿所選參考系的 x、y 軸；wz 單位為 rad/s，表示繞 z 軸的旋轉速度指令。下面按平面運動、x朝前、y朝左、z朝上的約定解釋；正 wz 從上方看為逆時針。實車軸向、MCU正負號和單位仍需核對。

實際雲臺系是隨 yaw 轉動的普通機體系，不是世界座標系。 雲臺朝前時，其 vx>0 朝車前；雲臺向左轉90°，同樣的正 vx 方向就轉到車左側。底盤朝向不變，也不妨礙用另一套軸描述它的運動。

gimbal_yaw_fake 是計算用的虛擬系：位置與 gimbal_yaw 重合，原始碼廣播 gimbal_yaw → gimbal_yaw_fake，旋轉取輸入 yaw 的相反數，沒有平移。在平面運動、輸入位姿與機械 TF 正確且及時更新時，它的水平朝向與 odom 對齊，而位置跟著機器人走。它不是 map，也不是雲臺或底盤的世界原點；“方向與 odom 對齊”不表示完整座標系與 odom 重合。原始碼只抵消 yaw，沒有抵消 roll、pitch，坡面上不能直接套用完全水平的結論。

1.1.1.一個直行例子：雲臺左轉90°，車仍沿原方向走

假設底盤朝 odom 的 +x，雲臺原本也朝前；現在只有雲臺向左轉90°。導航仍要求沿 odom 的 +x，以0.3 m/s移動，自旋為0：

數值變了，運動目標沒有變。 MCU若把收到的 (0,-0.3) 直接當成底盤系分量，就會理解成往車右側走。該例只解釋座標轉換，沒有進行車輛運動測試。

原始碼使用的角度是 odometry.pose 中的 yaw，當前預期為“gimbal_yaw 相對 odom 的朝向”，同時包含底盤整體朝向與機械相對 yaw 的影響；它不是單獨的機械關節角。線速度計算為：

vx_out =  cos(θ) × vx_in + sin(θ) × vy_in
vy_out = -sin(θ) × vx_in + cos(θ) × vy_in
θ      = 输入里程计位姿中的 yaw，单位 rad

MCU從雲臺系換到底盤系時，要另外使用兩者的機械相對角。若 q 表示雲臺系相對底盤系的平面朝向，已計入固定安裝關係，方向換算為：

vx_chassis = cos(q) × vx_out - sin(q) × vy_out
vy_chassis = sin(q) × vx_out + cos(q) × vy_out
q          = 云台相对底盘的角度，单位 rad

θ 相對 odom，q 相對底盤，二者不能直接混用。 不能把MCU世界 IMU 姿態當關節角；q 應由機械相對 yaw與安裝零位確定。上述90°例子中，底盤與 odom方向恰好一致，所以兩角相同；底盤自己也轉向時，就不再相同。

上位機這一步沒有查詢雲臺→底盤 TF，也沒有處理不同原點之間的旋轉速度補償；上面的MCU公式僅解釋平面方向換軸。原點偏置是否需要處理，要結合真實安裝和下位機速度定義核對。

1.1.2.wz：同一個底盤指令的兩項相加

wz_final = wz_nav2 + wz_spin

兩項單位相同，都是 rad/s：

• wz_nav2：導航提出的旋轉分量，例如調整朝向；它是控制指令，不是測得的雲臺或底盤轉速。
• wz_spin：人為附加的底盤自旋量，由啟動引數或 cmd_spin 訊息提供。
• wz_final：傳送給底盤的總目標角速度。沒有分別給兩個機械部件各發一個轉速。

平面運動時，雲臺和底盤僅水平朝向不同，z軸方向相同；繞 z 的角速度分量不因水平 yaw 換軸而改變，所以原始碼只旋轉 vx、vy，對 wz 直接相加。此處沒有求導雲臺 yaw，也沒有把雲臺相對轉速作為底盤轉速輸入。底盤整體轉向與 gimbal_yaw_joint 的機械相對轉動必須分開理解，不能根據話題參考系名稱混為同一個動作。

當前實物、模擬的 FollowPath.enable_rotation 都是 False，普通路徑跟蹤時該控制器自身給出的導航角速度為0。因此尚未改自旋設定時，實物預設不附加自旋，模擬預設附加3.14 rad/s，約每2秒一圈。Nav2恢復旋轉等行為另有指令來源，速度平滑器也可能保留過渡過程，不能推成整個導航鏈永遠只輸出0角速度。

1.2.引數

1.2.1.ros__parameters（ROS節點引數）

1.2.1.1.參數列

本表以當前實物 YAML 為基準；模擬差異單列。local_plan_topic 沒寫在這兩份 YAML 中，但原始碼會宣告並讀取，當前使用其預設值。

fake_vel_transform:
  ros__parameters:
    use_sim_time: False
    odom_topic: "odometry"
    robot_base_frame: "gimbal_yaw"
    fake_robot_base_frame: "gimbal_yaw_fake"
    input_cmd_vel_topic: "cmd_vel_nav2_result"
    output_cmd_vel_topic: "cmd_vel"
    cmd_spin_topic: "cmd_spin"
    init_spin_speed: 0.0

1.2.1.2.引數詳解

1.2.1.2.1.odom_topic、robot_base_frame 與 fake_robot_base_frame（輸入里程計與真實、虛擬雲臺系）

odom_topic 當前接 sensor_scan_generation 的 odometry；訊息位姿參考系預期為 odom，child_frame_id 為 gimbal_yaw。本節點讀這個位姿的 yaw，釋出父系 gimbal_yaw 到子系 gimbal_yaw_fake 的反向 yaw 變換，約50 Hz，平移為零。

因此，換話題或改座標系名稱前，必須同時檢查輸入位姿究竟屬於誰。改名字不會把底盤位姿自動變成雲臺位姿，也不會把世界 IMU 姿態變成關節角。原始碼沒有查詢或校驗這些關係；上游內部 IMU→雷達轉換、機械TF失敗返回單位變換等已記錄問題，也可能使這裡用錯角度。

MID360繼續掛在 gimbal_yaw，隨實際 yaw、不隨實際 pitch 轉動；實際 yaw、pitch 分別只在 gimbal_yaw_joint、gimbal_pitch_joint，兩個 odom 關節固定。虛擬系不是增加第二個實際 yaw，序列埠 JointState仍按機械相對角填 {relative_pitch, relative_yaw, 0, 0}。

1.2.1.2.2.input_cmd_vel_topic、output_cmd_vel_topic 與MCU配合（速度輸入與輸出話題）

當前跟蹤控制器把路徑轉到區域性代價地圖的機體系；該系配置為 gimbal_yaw_fake，據此計算 x、y速度。跟蹤輸出經 cmd_vel_controller → velocity_smoother → cmd_vel_nav2_result，再進入本節點；導航行為也可輸出到 cmd_vel_nav2_result。

轉換後釋出的 cmd_vel 是 geometry_msgs/msg/Twist，訊息本身沒有寫明座標系，也沒有時間戳。輸出變數名字雖然含 chassis，原始碼只是用輸入 yaw 旋轉平面分量，並未把速度再轉換到 base_footprint 或 chassis。判斷方向要看配置、計算和上下游約定，不能靠變數名或“最後給底盤用”推斷它已是底盤系。

serial_comm 直接把 linear.x、linear.y、angular.z 寫入協議欄位，按週期傳送，沒有用 JointState再換方向。MCU需核對四件事：線速度是否按雲臺系接收；機械相對 yaw 的零位與正負號；m/s與rad/s的單位；角速度是否已經包含自旋、是否會重複相加。當前沒有核對MCU執行程式碼或實物響應，不能把上位機約定寫成下位機已正確實現。

這條參考系結論只針對上述導航鏈。手柄等若繞過本節點直接釋出 cmd_vel，不自動獲得同樣的轉換；它們的速度參考系及同名話題的使用方式需要另行核對，本塊不展開修改。

1.2.1.2.3.init_spin_speed 與 cmd_spin_topic：改的是附加量

當前實物 init_spin_speed=0.0，模擬為3.14。本節原先統一寫“預設3.14 rad/s”，現按兩份配置區分；前文“資料對接”仍保留3.14示例，不代表當前實物預設值，本輪不改寫該舊章節。北極熊官方實物 YAML 當前採用3.14，這是原包設定，當前工程的具體值應以本表為準。北極熊官方配置

啟動初值只用於給 spin_speed_ 賦值；收到 cmd_spin 後直接用訊息數值替換，後續一直保留，原始碼沒有自動恢復初值或訊息過期規則。重啟時才重新使用啟動引數。下面保留話題使用示例，僅作為說明，本輪沒有執行：

# 设置附加自旋速度为 +1 rad/s，约每秒 57.3°
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: 1.0}"

# 改为反方向自旋
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: -1.0}"

# 关闭附加自旋
ros2 topic pub --once /cmd_spin example_interfaces/msg/Float32 "{data: 0.0}"

上面是無名稱空間示例。相對話題名會受啟動名稱空間影響，常見模擬為 /red_standard_robot1/cmd_spin；實際以啟動的 namespace為準，實物也不一定就是根目錄話題。

附加量設為0，只關自旋附加項；輸入速度全零，也不會自動清掉自旋附加項。 輸入全零且 spin_speed_≠0 時，轉換仍會輸出非零角速度。收到 cmd_spin 也只改變數，沒有立刻釋出新 cmd_vel；之後進入速度處理回撥才體現新值。它本身不是持續輸出速度的獨立旋轉控制器。

原始碼直接相加，沒有對最終角速度限幅或平滑。上游平滑器處理的是相加之前的速度，不能據其限值認定相加後的總量已受同樣限制。是否適合實車需結合底盤能力及下位機限制核對，本輪不調整轉速。

1.2.1.2.4.local_plan_topic、use_sim_time 與原始碼疑點

本節點近似配對的是 Odometry 和 Path，不是帶時間戳的速度；輸入速度沒有時間戳，只快取最近值。收到區域性路徑會重新整理“控制器活躍”時間；最近0.5秒內有路徑時，非零速度等待配對回撥處理，否則直接按當前儲存的 yaw 轉換。近似配對不能證明路徑、位姿、速度對應同一時刻。

以下是原始碼能確認的缺口，實際影響尚未執行驗證：

• 首幀方向： current_robot_base_angle_ 未顯式初始化，釋出TF與轉換速度前也沒有“已取得有效 yaw”的保護；首個有效里程計前的結果不能視為可靠。
• 舊速度複用： 配對回撥使用後沒有清空快取速度。零速度走直接釋出分支，卻沒有清掉先前快取的非零指令；之後存在再次通過配對回調發布舊指令的路徑。
• 超時含義： 寫死的0.5秒只檢查最近是否收到區域性路徑，不檢查速度指令的新鮮度，也不是自動停車機制；20 ms定時器只發布TF，不會定時釋出停車速度。
• 時鐘混用： 活躍時間判斷使用預設 rclcpp::Clock()，Humble中是系統時間；TF時間戳使用節點時鐘，可受 use_sim_time 影響，定時器又用真即時間。True不能讓這些步驟統一按模擬時鐘執行。

這裡不展開此前擱置的導航走停或 map→odom 時效排查，僅記錄本節點的程式碼前提。

1.2.1.2.5.啟動來源與當前驗證範圍

整套 reality／simulation 啟動分別預設使用對應目錄的 nav2_params.yaml，可由 params_file 指定；導航 launch給節點或元件載入處理後的引數檔案，並重寫已有 use_sim_time。兩種模式都把該節點命名為 fake_vel_transform，匹配 YAML。

包內獨立 fake_vel_transform_launch.py 只傳 use_sim_time，預設 True，不讀取整套 YAML；其餘採用C++預設值，例如參考系 gimbal_link/gimbal_link_fake、里程計 odom、自旋0，輸入／輸出速度話題預設還是空字串。不能把這個獨立入口當作已配置好的整套導航啟動。

8項自定義引數僅在構造時讀取，沒有動態引數更新回撥；執行時寫同名引數不會更新成員、重建話題或改變初始自旋。ROS標準的 use_sim_time 由節點時鐘處理，執行時改變自旋應通過 cmd_spin，兩者不要混淆。

本輪在 ysu_ros_humble 中只查詢環境、安裝路徑與檔案，確認獨立 launch、導航 launch和兩份 YAML 與當前原始碼檔案一致，並核對Humble時鐘預設型別；沒有啟動節點、傳送上述訊息或進行運動測試。

• 原始碼能確認： 導航輸入與輸出的參考系約定、二維速度換軸、角速度直接相加、序列埠原樣傳值、實物／模擬預設差異、引數讀取方式及上述程式碼缺口。
• 模擬已觀察： 2026-10-08僅檢查獨立 Point-LIO 的靜止和約20 cm往返；沒有驗證本節點、獨立雲臺 yaw、MCU速度換算、附加自旋或停車行為。本節90°例子與相加表均為計算示例。
• 必須實物或下位機原始碼核實： 軸向與單位、機械相對 yaw零位及方向、輸入位姿是否正確、MCU是否把雲臺系線速度轉到底盤執行方向、最終角速度的執行物件與限制，以及是否重複疊加自旋。此前重力方向和點時間單位問題仍保留待核實。

原始碼位置：src/pb2025_sentry_nav/fake_vel_transform/src/fake_vel_transform.cpp、對應標頭檔案及 README；上游：sensor_scan_generation、pb_omni_pid_pursuit_controller 和導航 launch；序列埠：src/serial_comm/src/serial_comm.cpp。本輪僅修改日誌這一引數塊，工程原始碼、配置和引數值均保留。