Nav2 行為樹

問題定義

前面兩節分別建立了地圖跟代價地圖，也知道 planner_server 能規劃路徑、controller_server 能執行路徑。但一次完整的導航任務遠不只「規劃、執行」這麼單純——如果規劃失敗要重試嗎？如果機器人卡住不動要怎麼辦？路徑執行到一半失敗了要不要重新規劃？這些決策邏輯如果寫死在單一節點的程式碼裡，會變成一大坨難以維護的條件判斷。Nav2 用**行為樹（Behavior Tree, BT）**來組織這些決策，把「遇到什麼情況該做什麼」用一棵清楚的樹狀結構表達出來。

核心概念說明

行為樹的基本節點類型

行為樹由幾種基本節點類型組成，執行時從根節點開始，每個節點執行後回傳三種狀態之一：SUCCESS、FAILURE、RUNNING（仍在執行中，尚未有結果）。

• Sequence（循序節點）：依序執行子節點，只要有一個子節點回傳 FAILURE，整個 Sequence 就立刻回傳 FAILURE 並停止往下執行；全部子節點都 SUCCESS，Sequence 才回傳 SUCCESS。概念上類似程式碼裡的 A() and B() and C()。
• Fallback（後備節點，有時稱 Selector）：依序嘗試子節點，只要有一個子節點回傳 SUCCESS，整個 Fallback 就立刻回傳 SUCCESS；全部子節點都失敗，才回傳 FAILURE。概念上類似 A() or B() or C()——這正是「規劃失敗就換個方式重試」這類邏輯的核心節點類型。
• Action（動作節點）：樹的葉節點，實際執行某個具體動作（例如呼叫 ComputePathToPose），執行期間回傳 RUNNING，完成後回傳 SUCCESS 或 FAILURE。
• Decorator（裝飾節點）：包裹單一子節點，修改其行為，例如 RateController 限制子節點的執行頻率，RecoveryNode 在子節點失敗時執行一段復原行為後重試。

用 Fallback + RecoveryNode 理解「失敗重試」的實際運作

Nav2 官方預設行為樹裡，處理「路徑執行失敗」的典型結構大致是：

RecoveryNode
├── FollowPath（嘗試沿著路徑走）
└── Fallback（如果失敗，依序嘗試以下復原行為）
    ├── ClearCostmapService（清除代價地圖裡可能過時的障礙物標記，重新評估）
    └── Spin（原地旋轉，讓感測器重新掃描周圍環境，收集更新的資料）

RecoveryNode 這個裝飾節點的邏輯是：先執行主要行為（FollowPath），如果失敗，執行復原分支，復原完成後回到主要行為重新嘗試，重複這個循環直到成功或達到重試上限。這個結構直接對應到真實場景：機器人卡住走不動，先清除可能過時的代價地圖標記，如果還是不行就原地旋轉重新掃描環境，這些都是實際除錯導航失敗時常見的人工介入手段，行為樹把它們自動化了。

實作範例：撰寫一份客製化的簡化行為樹

1. 基本行為樹 XML 結構

檔案位置：~/ros2_ws/src/my_package/behavior_trees/simple_nav.xml

<root BTCPP_format="4">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      </RateController>
      <RecoveryNode number_of_retries="3" name="FollowPathRecovery">
        <FollowPath path="{path}" controller_id="FollowPath"/>
        <Fallback>
          <ClearEntireCostmap name="ClearLocalCostmap" service_name="local_costmap/clear_entirely_local_costmap"/>
          <Spin spin_dist="1.57"/>
        </Fallback>
      </RecoveryNode>
    </PipelineSequence>
  </BehaviorTree>
</root>

這份簡化版行為樹的邏輯：RateController 限制路徑重新規劃的頻率不超過每秒一次（避免過度頻繁重新規劃浪費運算資源），規劃出的路徑交給 RecoveryNode 包裹的 FollowPath 執行，如果執行失敗（最多重試 3 次），先嘗試清除 local costmap，不行的話原地旋轉約 90 度（1.57 弳度）重新掃描。

2. 讓 bt_navigator 載入這份行為樹

ros2 run nav2_bt_navigator bt_navigator --ros-args \
  -p default_nav_to_pose_bt_xml:=/home/user/ros2_ws/src/my_package/behavior_trees/simple_nav.xml

3. 送出一個導航目標，觀察行為樹執行

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \
  "{pose: {pose: {position: {x: 2.0, y: 1.0, z: 0.0}}}}" --feedback

預期輸出

Sending goal:
     pose: ...
Feedback:
    distance_remaining: 2.236
    number_of_recoveries: 0
    current_pose: ...
Feedback:
    distance_remaining: 1.102
    number_of_recoveries: 0
...
Result:
    result: {}

如果途中機器人卡住觸發了復原行為，number_of_recoveries 這個 feedback 欄位會遞增，這是不需要另外打開行為樹視覺化工具、也能快速確認「導航過程中有沒有觸發過復原邏輯」的方式。

常見錯誤與除錯技巧

錯誤一：bt_navigator 啟動時報錯，找不到某個行為樹節點類型

Error: Could not find TreeNode with name: ClearEntireCostmap

原因：行為樹 XML 裡使用的每一種節點類型，都需要對應的 BT 節點插件在啟動時被註冊進 bt_navigator；如果只是把節點名稱寫進 XML，但沒有在啟動參數的 plugin_lib_names 裡列出對應的插件套件，bt_navigator 解析 XML 時就會找不到這個節點類型的實作。

排除方式：確認啟動 bt_navigator 時的 plugin_lib_names 參數，包含了 XML 裡用到的所有節點類型對應的插件：

ros2 param get /bt_navigator plugin_lib_names

Nav2 官方提供的預設插件清單通常已經涵蓋大部分常用節點，客製化行為樹時如果用到官方預設之外的自訂節點，才需要額外註冊。

錯誤二：導航目標送出後，行為樹陷入無限重試循環，永遠不會回報結果

現象：number_of_recoveries 持續遞增，導航目標始終沒有 SUCCESS 也沒有明確 FAILURE。

原因：RecoveryNode 的 number_of_retries 如果設定過大（或本範例示範的結構本身，復原行為本身沒有真正解決根本問題，例如障礙物是永久性的，不是感測誤差造成的暫時性判斷錯誤），會讓系統陷入「失敗、復原、重試、再失敗」的循環，遲遲不會真正終止。

排除方式：檢查 number_of_retries 是否設定了合理的上限（本範例設為 3），確保重試次數用盡後行為樹會明確回傳 FAILURE 讓上層知道任務失敗，而不是無限重試；同時要判斷復原行為本身是否對症下藥——如果障礙物是永久性的，正確做法應該是觸發重新規劃（讓 planner_server 找一條完全不同的路徑），而不是單純清除代價地圖或原地旋轉。

小結

行為樹用 Sequence、Fallback、Action、Decorator 這幾種基本節點類型，把導航過程中的決策與復原邏輯組織成一棵可視化、可局部替換的樹狀結構，RecoveryNode 是實現「失敗後嘗試復原、再重試」這類常見模式的核心機制。到這裡，第 6 章導航支線（Nav2 架構、SLAM 建圖、代價地圖、行為樹）已經涵蓋了自主導航的完整流程——如果你的專案方向是導航相關應用，這四篇已經足以支撐後續深入的參數調校與實機部署；如果想繼續探索其他方向，可以接著看第 7 章機械手臂操作，或第 8 章感知與 AI 整合。

延伸閱讀

• Nav2 代價地圖與路徑規劃
• Nav2 導航系統總覽