Behavior Tree: cách bt_navigator điều phối toàn bộ Nav2

Các bài Planner và Controller giải quyết "tính đường đi" và "bám đường đi" — nhưng ai quyết định trình tự gọi hai việc đó, và phải làm gì khi một bước thất bại? Đây là vai trò của Behavior Tree, điều phối bởi node bt_navigator.

Khái niệm chính

State Machine: các trạng thái nối với nhau bằng transition (IF điều kiện THEN chuyển trạng thái)
Behavior Tree: cấu trúc CÂY, mỗi node trả về một trong 3 kết quả:
    SUCCESS — hoàn thành
    FAILURE — thất bại
    RUNNING — đang chạy, chưa xong

Ưu điểm của Behavior Tree so với State Machine truyền thống: dễ ghép lại (compose) các hành vi nhỏ thành hành vi lớn hơn mà không cần vẽ lại toàn bộ sơ đồ transition — chỉ cần thêm một node con vào đúng vị trí trong cây.

Nguyên lý hoạt động

Hai loại node chính:

Control Node — quyết định thứ tự/điều kiện chạy các node con:
    Sequence   — chạy lần lượt, dừng ngay khi 1 con FAILURE
    Fallback   — thử lần lượt, dừng ngay khi 1 con SUCCESS (thử phương án tiếp theo nếu thất bại)

Action Node — thực thi công việc thật:
    ComputePathToPose  — gọi planner_server (bài Planner)
    FollowPath         — gọi controller_server (bài Controller)
    Spin, BackUp, Wait — gọi các recovery behavior (bài Recovery)

Ví dụ cây đơn giản cho điều hướng cơ bản:

<BehaviorTree>
  <PipelineSequence>
    <ComputePathToPose goal="{goal}" path="{path}"/>
    <FollowPath path="{path}"/>
  </PipelineSequence>
</BehaviorTree>

PipelineSequence (biến thể của Sequence) chạy ComputePathToPose trước — nếu SUCCESS, chuyển sang FollowPath. Nếu ComputePathToPose FAILURE (không tìm được đường đi), toàn bộ cây trả về FAILURE ngay, không chạy FollowPath.

Tóm lại: Behavior Tree không chứa logic thuật toán (planner/controller làm việc đó) — nó chỉ chứa logic điều phối: gọi cái gì, theo thứ tự nào, làm gì khi thất bại. Tách logic điều phối khỏi logic thuật toán giúp mỗi phần dễ hiểu, dễ test độc lập hơn nhiều so với gộp chung vào một khối code lớn.

Kiến thức kỹ thuật

Công thức

Kết hợp Recovery khi Fallback thất bại — cấu trúc lồng nhau giữa Sequence chính và Fallback recovery:

<Fallback>
  <PipelineSequence>
    <ComputePathToPose goal="{goal}" path="{path}"/>
    <FollowPath path="{path}"/>
  </PipelineSequence>
  <ReactiveFallback>
    <Spin spin_dist="1.57"/>
    <Wait wait_duration="5"/>
  </ReactiveFallback>
</Fallback>

Nếu luồng chính (Sequence trên) thất bại — ví dụ FollowPath bị kẹt — Fallback cấp cao hơn chuyển sang thử recovery behavior (bài Recovery): xoay tại chỗ để "nhìn" rõ hơn, rồi thử lại. Đây chính xác là cơ chế bên dưới hành vi "robot tự xoay khi bị kẹt" thường thấy ở AMR thực tế.

Thông số

Loại node	Kết quả khi con SUCCESS	Kết quả khi con FAILURE
Sequence	Chuyển sang con tiếp theo	Dừng ngay, cả Sequence FAILURE
Fallback	Dừng ngay, cả Fallback SUCCESS	Thử con tiếp theo

Ứng dụng thực tế

Behavior Tree trong Nav2 được định nghĩa bằng file XML riêng biệt (không nhúng trong code C++/Python) — đổi cấu trúc điều phối (thêm bước recovery mới, đổi thứ tự thử) chỉ cần sửa file XML và restart bt_navigator, không cần biên dịch lại bất kỳ node nào — đúng tinh thần tách cấu hình khỏi code đã thấy nhiều lần ở các bài Parameter và Launch File. Với AMR vận hành trong môi trường cụ thể (VD: nhà kho nhiều người qua lại cần ưu tiên Wait hơn Spin), việc chỉnh sửa cấu trúc cây XML trực tiếp — không cần đụng vào code C++ của planner_server/controller_server — giúp tinh chỉnh hành vi robot nhanh hơn nhiều so với sửa code và build lại.

Ví dụ thực tế

Theo dấu 1 lệnh điều hướng qua cây Behavior Tree có Fallback recovery:

1. bt_navigator nhận goal, bắt đầu chạy PipelineSequence
2. ComputePathToPose -> SUCCESS (tìm được đường)
3. FollowPath -> RUNNING, rồi gặp vật cản bất ngờ chặn hoàn toàn -> FAILURE
4. Fallback cấp cao phát hiện luồng chính FAILURE, chuyển sang ReactiveFallback
5. Spin (xoay 90°) -> SUCCESS (robot xoay xong)
6. Fallback báo SUCCESS toàn bộ nhánh recovery -> quay lại thử luồng chính
7. ComputePathToPose tính lại (costmap đã cập nhật sau khi xoay) -> SUCCESS
8. FollowPath tiếp tục -> robot di chuyển bình thường trở lại

Toàn bộ quá trình này không cần con người can thiệp — cây Behavior Tree
tự động thử lại theo đúng cấu trúc đã định nghĩa trong XML

Lỗi thường gặp

• Nhầm lẫn giữa Sequence và Fallback khi thiết kế cây — dùng sai loại node khiến logic điều phối chạy ngược ý muốn (VD: dùng Sequence khi thực chất cần thử lần lượt cho tới khi thành công).
• Nhồi logic thuật toán vào Behavior Tree thay vì Action Node riêng — vi phạm nguyên tắc tách logic điều phối khỏi logic thuật toán, khiến cây khó đọc và khó test độc lập.
• Không kiểm tra file XML sau khi chỉnh sửa trước khi deploy — lỗi cú pháp XML hoặc node không tồn tại khiến bt_navigator không load được cây, thường chỉ phát hiện khi robot không phản ứng gì với goal mới.
• Thiết kế cây Fallback recovery quá đơn giản cho môi trường phức tạp — chỉ có 1-2 recovery behavior có thể không đủ cho mọi tình huống kẹt thực tế, cần xem thêm bài Recovery để hiểu đầy đủ các lựa chọn.

Kết luận

Behavior Tree tách biệt rõ ràng logic điều phối (gọi cái gì, thứ tự nào, làm gì khi thất bại) khỏi logic thuật toán (planner/controller tự thực hiện công việc của chúng) — cấu trúc cây với Sequence/Fallback và định nghĩa bằng XML giúp tinh chỉnh hành vi robot mà không cần build lại code. Bài tiếp theo trong chuyên mục đi vào Recovery Behaviors — chi tiết các hành vi robot thực hiện để tự thoát khỏi tình huống bế tắc.