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的參考系和中間點移除、請求響應耗時、恢復方向及速度鏈是否符合實車接口。