behavior_server（恢復行為與空閒方向移動）

1.behavior_server（恢復行為與空閒方向移動）

1.1.實車先看：當前恢復動作、版本介面與運動檢查

當前預設行為樹實際呼叫的是BackUp，後端為自定義BackUpFreeSpace；它尋找空閒方向做平面移動，不保證朝車後方倒退。 Spin、DriveOnHeading、AssistedTeleop、Wait會載入，但沒有進入當前兩棵預設樹。清代價地圖則呼叫costmap服務，不是由這五個行為外掛完成。

本塊還有兩類需要先解決的問題，不能只按數值大小調參：

• 版本介面： 本次安裝的Nav2 1.1.20讀取單組costmap_topic、footprint_topic，不讀取本YAML中的四個local／global話題鍵或local_frame。實際碰撞檢查預設接區域性圖，位姿卻按global_frame=map取得，需要統一。
• 自定義實現： BackUpFreeSpace覆蓋onConfigure而未呼叫父類，繼承的預測時長保持0，週期碰撞預測迴圈不執行；選向函式沒有安全方向時仍返回0。僅增大simulate_ahead_time或max_radius不能修復這些路徑。

默认BT导航失败
    → 清局部／全局代价地图服务，与BackUp轮换恢复
    → BackUp：target.x=1 m，speed=1 m/s，time_allowance默认10 s
        → BackUpFreeSpace
            ├─ global_costmap/get_costmap服务：取一次当前全局图
            ├─ map中的机器人位置：扫描各方向0～max_radius射线
            ├─ 试图选择较宽的安全方向区间
            └─ 按所选角度发vx、vy，按离初始位置的平面距离判断完成

Nav2 1.1.20共有的碰撞检查输入：
    costmap_topic、footprint_topic → 单个CostmapTopicCollisionChecker
    当前默认为local_costmap话题；不会因四个双地图键自动创建两套检查器

這裡的1 m、1 m/s來自預設XML埠，並非本YAML中的max_radius=2；不能據此認定實車已經正確以該速度恢復。圖中的移動流程還受後面列出的選向、座標、預測和輸出接線問題影響。當前BT恢復分支

本次nav2_behaviors為1.1.20，pb_nav2_plugins為1.0.1。伺服器、TimedBehavior、DriveOnHeading安裝標頭檔案與對應Nav2版本一致；本地BackUpFreeSpace實現／標頭檔案與上游提交e6778ca44d9e88aa612a3c6c87d536a8cfa21705一致，安裝標頭檔案也一致。下文按這套實現說明。1.1.20伺服器、自定義恢復實現

1.2.引數

下面原樣保留髮送的23項引數與原註釋。原始碼和安裝副本的reality段一致；simulation段僅use_sim_time=True不同。版本適配建議另列，沒有改工程YAML或行為程式碼。

behavior_server:
  ros__parameters:
    use_sim_time: False
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "pb_nav2_behaviors/BackUpFreeSpace"
    drive_on_heading:
      plugin: "nav2_behaviors/DriveOnHeading"
    wait:
      plugin: "nav2_behaviors/Wait"
    assisted_teleop:
      plugin: "nav2_behaviors/AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: gimbal_yaw_fake
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2
    # params for pb_nav2_behaviors/BackUpFreeSpace
    service_name: "global_costmap/get_costmap"
    max_radius: 2.0
    visualize: false

本頁把“實現未讀取的鍵”“已讀取但預設BT未呼叫對應行為”“當前BackUp實際使用的引數”分別寫在備註中。表中的調大／調小效果僅指對應實現分支，不表示這些值同時限制所有導航速度。

1.2.1.ros__parameters（更新、地圖介面與座標）

1.2.1.1.參數列

1.2.1.2.引數詳解

1.2.1.2.1.當前1.1.20真正使用的是單地圖介面

BehaviorServer宣告並讀取costmap_topic和footprint_topic，建立一個地圖訂閱、一個輪廓訂閱和一個CostmapTopicCollisionChecker，再把同一個檢查器交給各行為。沒有讀取上述四個雙地圖話題鍵，也沒有為local_frame建立分支。宣告與訂閱建立

本YAML沒有寫實際的兩個鍵，因此當前取原始碼預設：

costmap_topic   = local_costmap/costmap_raw
footprint_topic = local_costmap/published_footprint

話題是相對名稱，例如namespace為/red_standard_robot1時，預設檢查輸入位於該機器人的local_costmap下。寫了global_costmap_topic並不會改變這組訂閱。另一方面，BackUpFreeSpace自己的service_name確實被讀取，仍從全域性服務取一次圖；選向服務的地圖和共有檢查器的地圖是兩條輸入鏈。

1.2.1.2.2.global_frame與地圖座標必須一起適配

當前TimedBehavior統一取global_frame=map中的gimbal_yaw_fake位姿。共有檢查器卻預設使用local_costmap的odom網格；它直接把候選pose.x／y當作所訂閱地圖座標，沒有在內部將map位姿變到odom。僅TF鏈存在不能自動修復這處使用方式。共同位姿引數、檢查器座標使用

如果map與odom暫時恰好重合，錯誤組合可能不明顯；定位有平移／轉角修正後，不應繼續把兩系座標混用。BackUp當前預測迴圈為0又會掩蓋這一輸入問題，不能以它“還能移動”證明檢查有效。

若繼續使用此Nav2版本，應明確選擇一組自洽輸入：

不能只把global_frame改成odom而保留全域性選向服務，否則自定義射線會把odom位置直接放進map網格。也不能只更換服務而不統一檢查器與位姿。

下面僅展示與當前map／全域性服務一致的1.1.20介面寫法，不是完整修復補丁，未寫入工程：

behavior_server:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    global_frame: map
    robot_base_frame: gimbal_yaw_fake
    service_name: global_costmap/get_costmap

之後仍須處理本頁列出的預測初始化、側向預測、速度換系和無安全方向返回等程式碼缺口。如果改用支援雙地圖介面的其他Nav2版本，也要重新核對pb外掛繼承介面，不能只繼續保留新名字就認定適配完成。

1.2.1.2.3.cycle_frequency（行為迴圈與預測取樣）

TimedBehavior在onRun成功後，以WallRate(cycle_frequency)迴圈檢查取消／搶佔並執行onCycleUpdate。目標10 Hz即100 ms／輪；TF、碰撞、計算和釋出耗時都會影響實際週期。onRun期間等待服務還沒有進入這個迴圈，10 Hz不會讓那段等待主動檢查取消。

標準Spin與DriveOnHeading還用1／cycle_frequency作為預測取樣步長，最多int(cycle_frequency×simulate_ahead_time)個取樣。當前10與2名義最多20個，時間索引從0起，不是嚴格檢查到第2秒；達到其提前結束條件時更少。按1 m/s計算，100 ms對應0.1 m指令位移尺度，這是配置計算，不是實測。

自定義BackUp繼承成員時長為0，當前int(10×0)=0；改10或YAML中的2都不能替代缺少的成員初始化。執行迴圈與取消

1.2.2.行為外掛（載入了哪些，預設樹呼叫哪些）

1.2.2.1.參數列

這裡的“當前執行不使用”指預設BT沒有發起該Action；外掛仍會在生命週期配置時載入、讀取自己的引數。外部客戶端或更換XML後，使用條件會改變。

五個查詢名均有當前安裝XML註冊，標準行為使用帶斜槓別名，自定義也按pb別名載入。列表載入順序不會讓Wait自動排在BackUp後執行；是否呼叫和恢復順序由BT或外部客戶端決定。

1.2.3.simulate_ahead_time與旋轉引數（對應行為分別使用）

1.2.3.1.參數列

1.2.3.2.引數詳解

1.2.3.2.1.自定義BackUp的預測時長為何沒有得到2

BackUpFreeSpace繼承DriveOnHeading。父類建構函式把simulate_ahead_time_設為0；本應由父類onConfigure讀取YAML中的2，但自定義覆蓋函式只讀取global_frame、max_radius、service_name、visualize，沒有呼叫父類版本，也沒有自行設定該成員。父類初始化、自定義初始化

所以自定義onCycleUpdate雖然呼叫繼承的isCollisionFree，後者max_cycle_count=int(10×0)=0，while不進入，直接返回True。Spin對同一個節點引數的讀取只更新Spin自己的成員，不會替BackUp初始化。

本次離線輔助檢查在匹配安裝標頭檔案的自定義物件上觀察到構造後時長0；將迴圈頻率設10、目標距離設1、檢查器保持未配置的空指標後，繼承檢查仍返回True而未訪問檢查器。再結合原始碼覆蓋函式可確認此初始化缺口。沒有呼叫真實configure／onRun或運動Action，此檢查不能寫成已經完成整套執行驗證。

僅補呼叫父類還不構成完整修復：繼承預測只用cmd_vel.linear.x沿pose.theta推算x、y，忽略自定義真正傳送的linear.y。需要同時讓預測覆蓋實際二維速度，並統一候選位姿與檢查地圖參考系。繼承預測公式

1.2.3.2.2.Spin的三項速度引數（幅值、末段下限與制動包絡）

Spin根據global_frame中robot_base_frame的yaw變化累計relative_yaw。剩餘角為abs(請求轉角)−abs(已轉角)，小於1e-6時發零並結束；否則計算：

角速度幅值 = clamp(sqrt(2 × rotational_acc_lim × 剩余转角),
                    min_rotational_vel, max_rotational_vel)
角速度正负 = 请求target_yaw的正负

當前剩餘角較大時上限1 rad/s；理想剩餘角降到1²／(2×3.2)=0.15625 rad附近才離開最大速度區，再降到0.4²／(2×3.2)=0.025 rad附近後受最小速度限制。數值為公式計算，不是實車轉動記錄。

rotational_acc_lim在這裡構成剩餘距離制動速度包絡，沒有按上一輪速度和實際dt做起步加速度限幅。10 Hz下0.4 rad/s對應每理想週期約0.04 rad指令轉動尺度，但是否正確停到目標還取決於真實反饋和執行鏈。Spin速度與反饋

當前gimbal_yaw_fake在前文的平面條件下抵消輸入yaw，其方向與odom近似對齊，不是底盤真實yaw。把它用於Spin累計轉角可能無法反映真實底盤轉動；map↔odom定位修正也可能進入該反饋。啟用Spin前應先確定要轉動哪個實體及相應位姿參考，不能僅調角速度解決；本次未呼叫Spin驗證這個座標設計推論。虛擬系方向

1.1.20的Spin預測提前結束條件使用relative_yaw已轉角，而非剩餘目標角；首次relative_yaw=0時會在第一次檢查前退出。即便Spin成員讀取了2，也不能據此保證每輪完整預測未來2 s。這與自定義BackUp時長成員為0是不同問題。Spin預測分支

1.2.4.自定義BackUp引數（地圖服務、選向半徑與顯示）

1.2.4.1.參數列

1.2.4.2.引數詳解

1.2.4.2.1.service_name（一次服務取圖，等待不受10 s運動期限完整覆蓋）

自定義onRun先每次等待服務可用1 s，直到服務存在或ROS結束；該迴圈沒有Action取消或任務期限檢查。發請求後最多等待響應1 s，超時返回FAILED。隨後取位姿、選方向、讀取請求值，才設定end_time=ROS now+time_allowance並進入運動迴圈。服務等待與取圖

因此BT預設10 s是運動階段的請求期限，不覆蓋前面可能反覆等待服務的全部時長。common TimedBehavior的elapsed結果計時也在onRun成功後開始；等待階段不能靠cycle_frequency=10或BT請求響應超時100 ms推斷為10 s內自動結束。

該服務在一次onRun中取一次圖用於選方向，不在每週期重新選向。GetCostmap返回當前服務記憶體圖，不需等待global_costmap下一次2 Hz釋出；但取到後障礙變化仍需真正生效的週期檢查處理，當前這一步存在前述缺口。

1.2.4.2.2.max_radius（檢查一整條射線，不是挑最遠空閒點）

原始碼掃描−π到+π，固定角步長π／32，即5.625°，約64個方向；每條射線以服務返回的地圖resolution為徑向步長，從r=0檢查到max_radius。點必須在網格內且代價<253；253、254、255都讓該方向不安全，1～252軟代價則不會比較高低。射線與代價檢查

若有效解析度恰為0.05 m，2 m對應約40個格距。實際全域性圖格距由靜態地圖決定，不能總按本costmap YAML初始0.05推算；方向角度固定，也不隨更細格距自動加密。它檢查的是取樣中心線，不逐點放置真實車體輪廓。

例如目標移動1 m，卻在某方向1.5 m處有障礙：max_radius=2會淘汰該方向，1可能不淘汰。這說明半徑是選向檢查條件，不是目標移動長度。反過來，半徑小於目標距離也不能保證目標範圍內都已檢查；更何況當前無安全方向處理沒有可靠失敗出口。

gatherFreePoints這個輔助函式雖然會收集radius內代價0的格子，但當前onRun沒有呼叫它；實際選向不是“尋找距離最近的空閒點”。

1.2.4.2.3.找較寬扇區的實現邊界（沒有安全方向也返回0）

函式嘗試記錄第一個安全形和之後的第一個不安全形，把更寬區間儲存，再取兩邊界平均值。當前實現不能直接稱為完整的“最大安全扇區演算法”：

• 若掃描結束前一直安全，沒有最後不安全形，尾部區間不結算；−π／+π兩端的相鄰區間也不做環繞合併。
• 一個已關閉區間若不比當前最優更寬，first_safe_angle和last_unsafe_angle沒有復位，可能阻止後續區間繼續被正確記錄。
• final_safe_angle與final_unsafe_angle初始均為0；沒有記錄到有效區間時，最終返回(0+0)／2=0，不區分全空閒、全障礙或其他未閉合情況，也不返回“不可移動”。

本次直接呼叫安裝的自定義庫選向函式，用101×101、0.05 m格距、原點(-2.525,-2.525)、機器人在(0,0)、半徑2 m的合成網格得到：

這兩個輔助函式檢查沒有建立ROS節點或地圖服務，更沒有發運動指令。實際onRun得到該角後不會再次檢查“是否找到安全方向”，仍可設定速度並返回初始化成功；要實現可靠失敗處理，需要修改選向／呼叫邏輯，而不是繼續調半徑。

1.2.4.2.4.選向角在map，速度輸出卻沒有轉換到機體系

射線用pose.x／y和地圖x／y計算，angle相對global_frame軸；pose.theta在findBestDirection中沒有參與角度換系。之後直接寫：

vx = cos(angle) × 请求speed
vy = sin(angle) × 请求speed

Twist沒有frame_id，後續導航轉換鏈按gimbal_yaw_fake速度約定解釋它。若機器人在map中的yaw為theta，地圖方向angle對應的機體系方向應核對angle−theta，而當前程式碼沒有減theta。map←odom有yaw修正時，顯示在map的射線與實際輸出方向可能不同；不能因虛擬系平時與odom對齊就認定它也與map對齊。

標準BackUp會把距離／速度取負絕對值，實現固定機體系負x倒退；本自定義類覆蓋了onRun，不執行該邏輯。當前BT直接把backup_dist=1、backup_speed=1寫入請求，故自定義速度模長按請求為1 m/s，方向按其選擇角；請求target.x正負不改變完成用的abs距離，負speed則會把輸出方向反過來。BT請求欄位

1.2.4.2.5.完成距離、恆定速度與停止路徑

進入運動後，自定義迴圈保持啟動時的twist_x／y，不用標準DriveOnHeading的加減速或最小速度成員，也沒有讀取當前PID的線速度限值。完成判據為map中當前位置與第二次取得的initial_pose之間hypot差達到abs(target.x)。這是起終位置位移，不是沿運動軌跡累計里程，也不是沿選中射線投影距離。

定位修正可能改變map位移統計，橫向偏移同樣能增加該距離；只看反饋達到1 m不能證明沿選定方向實際走了1 m。理想恆定1 m/s且方向正確時1 m約需1 s，只是請求換算，未驗證實車。

完成、運動期限超時、明確碰撞失敗會呼叫stopRobot；但當前預測繞過意味著不能依賴其“Collision Ahead”分支。取得當前TF失敗時直接返回FAILED，common TimedBehavior的FAILED分支也不統一發零，不能籠統寫“任何失敗都會剎停”。取消／搶佔則在進入執行迴圈後顯式發零。自定義週期處理、共同執行狀態

1.2.5.相關原始碼預設項（原YAML未顯式配置）

1.2.5.1.參數列

以下8項是真正讀取的地圖鍵、標準DriveOnHeading和AssistedTeleop預設項；沒有向原YAML追加。Drive引數不自動進入當前自定義BackUp的覆蓋實現。

1.2.5.2.AssistedTeleop與Wait的使用邊界

AssistedTeleop把最新遙控Twist儲存在成員中，執行Action時才做投影和輸出；收到preempt_teleop的Empty訊息會在週期處理時發零併成功結束，time_allowance正值到期則發零失敗。正常結束後清成員，但沒有按最後一條遙控訊息時間自動判過期，不能把輸入斷流等同於立即剎停。

遇預測碰撞時，第一步碰撞把全部速度分量置0；之後某步碰撞則按time／projection_time等比縮放vx、vy、wz。當前projectPose的側向項符號還需核對：

源码：Δx = dt ×（vx cos(theta) + vy sin(theta)）
      Δy = dt ×（vx sin(theta) − vy cos(theta)）

按ROS常見機體系正y向左約定，theta=0、vy>0時真實指令方向應為正y，而這裡預測負y；例如vy=1 m/s、dt=0.1 s計算得到−0.1 m。若後續啟用輔助橫移，應核對／修正這處投影，不能只加大projection_time就認定已正確檢查。此例是原始碼計算，未發遙控指令。輔助遙控實現

Wait只計算請求結束時間和釋出time_left，正常執行不呼叫stopRobot。是否已經停住必須由呼叫流程和速度鏈保證，而不是在樹中放一個Wait就證明車輛停止。Wait實現

1.2.6.輸出接線、動態引數與驗證範圍

1.2.6.1.兩種啟動分支的恢復速度輸出不同

每個TimedBehavior釋出相對cmd_vel。當前navigation_launch存在下列差異，原始碼與安裝副本一致：

整套實物／模擬入口的use_composition預設True；單獨navigation_launch預設False，使用者顯式切換時也會進入不同分支。bt_navigator的重對映不會替另一個節點修改cmd_vel釋出器。獨立分支需要修正behavior_server接線，不能只修改本YAML的cycle_frequency。獨立分支、元件分支

當前元件恢復不經過velocity_smoother，因此其速度／加速度設定不自動平滑本行為命令。進入fake_vel_transform後，cmd_spin附加角速度還會相加；Spin的1 rad/s上限不是最終總角速度上限，stopRobot的零Twist也不自動清除附加量。具體座標與MCU協議見fake_vel_transform。

各行為還有各自的Action執行上下文，列表本身不提供多個運動Action之間的統一速度仲裁；當前BT按恢復順序呼叫，不應通過併發發起多個行為來測試引數。

1.2.6.2.動態引數與啟動時讀取

1.1.20的BehaviorServer、TimedBehavior、當前標準行為以及BackUpFreeSpace沒有為本組引數建立完整動態更新路徑。cycle_frequency、global_frame、robot_base_frame、transform_tolerance及外掛專用成員在configure階段讀取；話題和服務客戶端也在初始化建立。執行中ROS引數值改變不代表成員、迴圈頻率或訂閱已經聯動更新，應按配置重新配置／重啟再核對。

重新啟動也不會自動修復未讀取的五個鍵、缺少的父類初始化或無安全方向返回邏輯。自定義BackUp的目標距離、速度和運動期限則來自每次Action請求；它們不能用本YAML的max_radius、max_rotational_vel替代。

1.2.6.3.本次核對與實物待處理事項

• 本次已核對： nav2_behaviors 1.1.20、pb外掛1.0.1，五個外掛註冊與庫存在；伺服器／TimedBehavior／DriveOnHeading安裝標頭檔案匹配；自定義原始碼／標頭檔案匹配固定上游提交且安裝標頭檔案一致；兩套引數和安裝副本一致，僅時鐘不同；預設XML與相關launch安裝副本一致。
• 本次離線輔助驗證： 不初始化ROS、不建立節點、不呼叫configure／onRun或Action，僅構造輔助物件並呼叫繼承預測／安裝庫選向函式；觀察到預測時長0、未配置檢查器時預測直接True，以及全254／全0合成網格均返回0 rad。臨時執行檔隨臨時目錄清理，沒有釋出任何速度。
• 原始碼能確認： 23項原引數的實際使用條件；單地圖介面與五個無讀取鍵；共同幀與地圖組合問題；自定義預測初始化遺漏、x-only繼承預測、扇區結算／無安全方向退回值、角度未換到機體系；Spin制動公式與反饋／預測條件；等待和失敗停速邊界；輔助橫移投影公式及兩種輸出分支。
• 實物待處理： 先適配當前版本真正讀取的地圖／輪廓鍵並統一座標，修正BackUp預測、選向失敗處理及速度換系，核對獨立分支重對映和各失敗停速路徑，再驗證真實車體、恢復方向、有效速度與運動期限；僅改變本表數值不能替代這些修正。
• 本次驗證範圍： 未啟動新的恢復行為，未修改執行引數、工程YAML、預設樹或外掛程式碼，未進行實車／模擬運動和碰撞測試；本文不把輔助函式結果當整套恢復安全驗證。