bt_navigator（行為樹導航任務排程）

1.bt_navigator（行為樹導航任務排程）

1.1.實車先看：收到目標後，誰負責規劃、行走和恢復

bt_navigator按照行為樹XML組織導航任務：請求規劃、請求跟蹤路徑，並在失敗時按樹中的分支嘗試恢復。 實際路徑由planner_server計算，正常行走速度由controller_server計算，恢復動作由behavior_server執行；本塊引數決定如何組織這些呼叫。

當前工程的主要關係為：

单点目标 → NavigateToPose → 单点行为树
多点目标 → NavigateThroughPoses → 多点行为树
                                ├─ ComputePath… → planner_server
                                ├─ FollowPath → controller_server
                                │                    → velocity_smoother
                                └─ 失败恢复：清代价地图／BackUp → 对应服务器

导航速度 → fake_vel_transform → 实际云台系cmd_vel → 串口 → MCU执行底盘运动

這裡的“多點”指NavigateThroughPoses動作中按順序經過一組位姿，不等同於waypoint_follower的FollowWaypoints流程。

當前容器ysu_ros_humble中，nav2_bt_navigator、nav2_behavior_tree、nav2_util均為1.1.20，BehaviorTree.CPP為3.8.7。以下按這些版本及本地工程配置核對；官方線上文件包含後續發行版內容，不能直接把新版本引數和外掛載入規則套到Humble。

1.2.引數

1.2.1.ros__parameters（ROS節點引數）

1.2.1.1.參數列

下面保留當前實物配置。模擬段僅use_sim_time=True不同；兩段的其餘引數及47項外掛列表一致。

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: gimbal_yaw_fake
    odom_topic: odometry
    bt_loop_duration: 10
    default_server_timeout: 100
    wait_for_service_timeout: 1000
    default_nav_to_pose_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
    default_nav_through_poses_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node

前10項為本次配置；最後兩項為相關原始碼預設值，YAML未顯式配置，不代表需要把它們追加到當前檔案。

1.2.1.2.引數詳解

1.2.1.2.1.use_sim_time（時鐘選擇與launch覆蓋）

實物預設False，模擬預設True。navigation_launch.py通過RewrittenYaml重寫use_sim_time，並用ParameterFile載入；本節點已有該鍵，因此能夠被覆蓋。執行時究竟使用哪個值，應按當前啟動引數和有效節點引數判斷，不能只看實物YAML裡的False。

ROS時鐘用於TF和任務時間，也用於BT客戶端部分請求的已等待時間。BT主迴圈則用rclcpp::WallRate，當前RateController的節拍用真實高精度時鐘；模擬暫停／變速時，不能認為把use_sim_time設為True後，所有內部等待與檢查節拍都跟著模擬時間同比變化。BT執行迴圈（1.1.20）、RateController實現

1.2.1.2.2.global_frame 與 robot_base_frame（在哪裡看機器人，跟蹤哪個參考點）

當前導航器查詢的是gimbal_yaw_fake原點在map中的位姿，用於導航反饋等。map至該系的變換需要上游定位、底盤位姿、機械TF以及fake_vel_transform共同組成；這裡的引數不會替這些節點建立變換。導航器座標與反饋接線（1.1.20）

按前面的工程約定，gimbal_yaw_fake與實際gimbal_yaw原點重合，方向抵消輸入里程計yaw。在平面運動、輸入和機械TF正確時，它的水平朝向與odom對齊；機器人位置仍在移動。它不是map，也不與odom原點重合；原始碼只抵消yaw，坡道上的roll／pitch仍需核對。詳見fake_vel_transform。

這套配置為全向移動與雲臺獨立掃描服務。只把此項改成base_footprint，並不會同步修改控制器、代價地圖、behavior_server或後面的速度換系；若要改變整套參考系，應先檢查各處職責與原點偏置。

同名ROS引數不會自動替換每個BT節點的XML輸入埠。 有些節點從導航器複製的ROS引數讀取座標，有些從XML埠讀取，而且埠帶自己的預設值。當前多點樹的RemovePassedGoals就是後一種，具體問題見下文。

座標系名稱也不會因為ROS節點有namespace而自動加字首。應按實際TF frame名稱配置；namespace主要影響節點、相對話題、服務和動作名稱。

1.2.1.2.3.odom_topic（用於速度反饋，不代替TF定位）

本工程odometry由sensor_scan_generation釋出，位姿參考系預期為odom，child_frame_id為gimbal_yaw；本節點卻跟蹤gimbal_yaw_fake，兩者名稱不同有各自的用途，不能要求只為字串一致就改話題或子系。

BtNavigator用這個話題建立OdomSmoother，視窗為0.3 s，直接平均訊息的twist各分量，沒有在這裡重新做座標變換。導航器用平面速度大小sqrt(vx²+vy²)估計剩餘時間；當前實現僅在速度大於0.01 m/s且路徑剩餘長度大於0.1 m時計算，其他情況保持預設預計時間。它不是MCU實際輪速，也不是下一條控制指令。速度視窗實現、導航反饋計算

當前上游把參考系中的位姿差分直接寫入twist，且用回撥真即時間差而非訊息時間差，首幀歷史也有缺口；這些已經記錄在sensor_scan_generation。純平面旋轉換軸不會改變單幀速度模長，但錯誤時間差、首幀異常，以及先平均不同方向分量再取模長，都可能影響反饋。不能憑預計剩餘時間看起來合理就證明上游速度正確。

odometry是相對名。例如節點位於/red_standard_robot1時，通常訂閱/red_standard_robot1/odometry；最終還受remapping影響。本節點的當前位置取自TF，改odom_topic不能修復map至gimbal_yaw_fake的缺失變換。

1.2.1.2.4.bt_loop_duration（10 ms檢查一次，不是100 Hz重規劃）

tick可理解為“檢查行為樹當前執行到哪裡、動作是否完成、是否需要換分支”。10 ms對應目標約100次／秒；某次樹檢查、客戶端等待或其他計算超過這個週期，就達不到目標節拍，可能出現tick rate exceeded提示。

當前相關節拍分別為：

RateController的子節點已經處於RUNNING時，每次外層檢查都會繼續檢查它，直到結束；因此3 Hz不是“任何情況下都每0.333秒才理一次規劃”。實際重規劃頻率還受規劃耗時、恢復分支和BT排程影響。

減小10只會讓外層檢查更密，不能讓伺服器、點雲或TF自動變快。若CPU忙、定位跳變或控制斷續，應先查對應鏈路；不能把加快BT檢查當作提高導航精度。原始碼讀取整數毫秒，沒有完整的非正值保護，保持正整數。週期讀取

1.2.1.2.5.default_server_timeout（100 ms等請求響應，不限制整段導航時長）

100表示100 ms，即0.1 s。需要區分動作和服務：

• 動作客戶端： 發出ComputePathToPose、FollowPath或BackUp等目標後，等待伺服器確認收到／接受目標；接受之後，任務可以保持RUNNING並繼續執行，不會因為走了超過0.1秒就被此項終止。取消動作及停止時等待相關返回，也使用該響應超時。
• 服務客戶端： 例如ClearEntireCostmap，等待本次請求的服務結果；超過上限仍沒有結果時，該BT節點會失敗。

動作執行本身有另外的規則：路徑跟蹤有進度檢查；BackUp有time_allowance等。這些不能用default_server_timeout替代。動作確認與取消實現、服務響應等待實現

每個動作／服務節點可以通過XML的server_timeout埠覆蓋預設值。當前兩棵XML沒有顯式設定該埠，使用100 ms；當前安裝標頭檔案的埠沒有另設一個固定預設值，因此不能直接套用其他版本“埠預設值覆蓋ROS引數”的結論。

也不能認為一次tick必定阻塞100 ms。當前模板把單次等待片段限制到bt_loop_duration的一半，即5 ms；尚未到總響應上限時返回RUNNING，下次tick繼續判斷。100 ms是總請求等待尺度，5 ms是單次等待片段，兩者不矛盾；停止動作時的取消等待則有其單獨呼叫路徑。

實物先用100。若日誌明確出現等待目標確認／服務響應超時，先核對伺服器是否正常、名稱空間、計算負載與通訊，再考慮適度增加；延長不會修好碰撞、無路可走或定位失敗。

1.2.1.2.6.wait_for_service_timeout（1000 ms找伺服器，何時會使用）

1000表示1 s。雖然名稱帶service，當前BtActionNode也用它呼叫wait_for_action_server；服務節點用它呼叫wait_for_service。

它主要在建立XML中的動作／服務節點客戶端時使用。整棵樹建立會例項化恢復分支裡的節點，即使機器人還沒有遇到失敗，也可能已經需要backup動作伺服器及清圖服務可發現。但只是載入一個未被XML引用的外掛庫，不等於已經例項化它對應的動作節點。客戶端建立與發現等待

1 s是每次發現等待的上限，不是整套導航啟動限時。增大可容忍伺服器出現較慢；若樹中action/service名字與namespace錯誤，伺服器一直不會被發現，再長也無法解決。執行中已發出請求的等待，應看上一項。

1.2.1.2.7.兩項default_nav…bt_xml（預設行為樹與路徑展開）

兩條路徑指向本工程pb2025_nav_bringup包，而不是nav2_bt_navigator包的同名預設示例；檔名相同也不能據此認為樹的內容相同。

$(find-pkg-share pb2025_nav_bringup)是launch替換表示式，navigation_launch.py使用ParameterFile(allow_substs=True)才會展開成包的share目錄。YAML檔案本身、普通字串讀取以及BtNavigator的檔案開啟過程不會單獨解釋這段表示式；若繞過該launch直接給節點傳原始檔案，應提供可用的實際路徑或相應的替換處理。

預設採用當前環境找到的安裝包。修改原始碼裡的XML後，需核對安裝目錄是否為連結／同步副本；本次檢查兩份已安裝XML與當前原始碼一致。其他機器或重建方式變化後應重新核對。

單點和多點導航動作的請求裡都有behavior_tree路徑欄位：為空時用各自預設樹，提供其他路徑時載入該檔案。當前導航器啟用時也會載入預設XML。切換樹不等於隨意允許同時執行兩個導航任務，當前導航器還有任務互斥邏輯。單點預設路徑與目標處理、多點對應實現

兩棵當前預設樹的共同流程：

1. PipelineSequence組織持續重規劃與路徑跟蹤。規劃請求指定planner_id="GridBased"，跟蹤指定controller_id="FollowPath"。
2. 規劃失敗時嘗試清全域性代價地圖再重試，該內層RecoveryNode為number_of_retries=1；跟蹤失敗時嘗試清區域性代價地圖再重試，該內層為10。
3. 內層仍失敗後，進入外層恢復。先檢查GoalUpdated是否有新目標；否則RoundRobin輪換“清區域性及全域性代價地圖”與“BackUp”。外層RecoveryNode為10次恢復重試；不是整個任務至多呼叫所有伺服器10次。

清圖呼叫的是代價地圖清理服務，不是刪除磁盤裡的二維地圖圖片、PCD或SLAM位姿圖。靜態層和即時障礙輸入還可能把佔用重新加入地圖。

當前樹沒有Spin、Wait、SmoothPath或IsStuck節點。 不會僅因為庫列表有這些名稱，就自動增加原地轉動、等待、路徑平滑或該卡住檢測條件。當前規劃和跟蹤結果失敗仍可觸發RecoveryNode，不依賴IsStuck必須入樹。

BackUp在XML中請求backup_dist=1.0 m、backup_speed=1.0 m/s，未寫time_allowance，BT埠預設10 s。當前behavior_server的backup後端是pb_nav2_behaviors/BackUpFreeSpace：它按代價地圖選擇空閒方向，再做平面移動，並非固定沿車後方向倒退。請求值是目標距離和速度，不能據此聲稱實車已經以該速度正確移動；具體方向、碰撞檢查與速度輸出應在後續behavior_server引數塊一起核對。

1.2.1.2.8.多點樹的RemovePassedGoals（0.7 m中間點門檻與座標埠缺項）

多點XML在ComputePathThroughPoses前放入RemovePassedGoals，radius=0.7 m。當前實現會從列表開頭依次刪除距查詢位置不超過該門檻的中間目標，至少保留最後一個目標；這不是“所有目標都只要靠近0.7 m就算導航完成”，也不是最終停車的xy_goal_tolerance。移除中間目標實現

這裡有一個可以由當前XML和安裝標頭檔案確認的配置缺口：

• 本塊ROS引數設定robot_base_frame=gimbal_yaw_fake。
• RemovePassedGoals從自己的XML輸入埠讀取座標系，預設global_frame=map、robot_base_frame=base_link。
• 當前XML只給input_goals、output_goals、radius，沒有給兩個座標埠，因此查詢的是map至base_link，不會自動變成map至gimbal_yaw_fake。

若當前TF中沒有base_link，查詢會失敗，使多點規劃分支返回FAILURE；即使存在，也需確認其原點是否就是預期判定參考點。前面的模型與導航主鏈使用base_footprint、gimbal_yaw及gimbal_yaw_fake，不能憑庫已經存在就跳過這個檢查。埠預設值（1.1.20）

若沿用本工程虛擬參考點，XML可明確寫成：

<RemovePassedGoals input_goals="{goals}"
                   output_goals="{goals}"
                   radius="0.7"
                   global_frame="map"
                   robot_base_frame="gimbal_yaw_fake"/>

這是相應XML的配置建議，不是本次已修改或已執行驗證的結果。只改本塊ROS引數並不能補上XML埠。若換global_frame，兩個位置也應一起核對；以後使用DistanceController、GoalReached等其他帶座標埠的BT節點時，同樣不能假定自動繼承。

1.2.1.2.9.plugin_lib_names（動態庫名稱、XML標籤與執行動作的區別）

列表中每項是動態庫名稱，Linux下通常對應lib名稱.so檔案；XML寫的是庫註冊的BT節點標籤。兩者不是同一個字串。例如nav2_follow_path_action_bt_node註冊FollowPath，實際跟蹤演算法則來自controller_server中的FollowPath外掛配置。

Humble 1.1.20的BehaviorTreeEngine逐項載入這裡配置的庫並註冊節點型別；列表參與載入，不按當前XML自動過濾。47個庫中本次均找到對應安裝檔案，但檔案存在不等於所有庫載入、樹建立和導航執行已經執行通過。動態庫載入實現

下面逐項列出當前配置；“入樹”指XML中存在該節點，恢復節點仍要滿足失敗分支條件才執行。兩棵預設樹合計引用11個庫，其餘36個庫已配置載入，但沒有對應節點進入這兩棵樹。

1.2.1.3.外掛列表詳解（當前預設樹引用的11個庫）

1.2.1.4.外掛列表詳解（當前預設樹未引用的36個庫）

以下是已配置的可用節點型別。當前未入樹，不能依靠修改這些庫名稱開啟相應功能；今後採用其他XML時，需要重新判斷節點是否使用、埠是否正確和伺服器是否存在。

各庫與標籤的對應關係按1.1.20的動態庫構建列表及各BT節點註冊原始碼核對。Sequence、ReactiveSequence、ReactiveFallback是BehaviorTree.CPP內建控制節點，因此當前XML中有這些標籤，並不要求列表另有同名Nav2動態庫。

1.2.1.4.1.讀取時機、啟動來源與驗證範圍

global_frame、robot_base_frame、odom_topic及外掛列表主要在configure階段讀入；時間引數配置到BT動作伺服器及黑板；預設XML在configure時選定、activate時載入。當前沒有讓執行中修改這些引數後自動重建TF使用者、里程計訂閱、外掛工廠和超時成員的完整更新回撥。修改後重新配置／重啟並核對有效值；標準use_sim_time的時鐘處理另屬ROS機制。

always_reload_bt_xml=False時，同一路徑的已載入樹會複用。只修改磁碟XML再發同一路徑目標，不能假定內容已更新；可在除錯時配置該開關後重新配置，或重新啟動載入。True也不是在機器人執行任務途中隨檔案修改即時切換流程。樹檔案複用與重新載入實現

• 本次已核對： 容器內包版本；動作／服務兩個安裝標頭檔案與1.1.20原始碼一致；47個配置庫檔案全部存在；原始碼與安裝副本的實物／模擬bt_navigator段一致；兩份工程XML與安裝副本一致，XML內容與外掛註冊標籤已靜態對照。
• 原始碼能確認： 三項時間單位均為ms；BT檢查與3 Hz重規劃不同；請求響應超時不限制整段導航時長；列表庫均參與載入，但預設樹只引用11項；多點RemovePassedGoals當前採用自己的base_link預設埠；BackUp呼叫自定義空閒方向移動後端。
• 本次驗證範圍： 未啟動新的導航節點、未傳送導航目標、未測試行為樹實際載入或恢復移動；前文Point-LIO的靜止與短距離記錄不能作為本塊通過實車導航驗證的證據。
• 實物待核實： 有效map至gimbal_yaw_fake的TF、odometry速度與時間戳、單點／多點實際使用的樹、RemovePassedGoals的參考系和中間點移除、請求響應耗時、恢復方向及速度鏈是否符合實車介面。