Kiến trúc recovery_core

Vị trí

Gói nằm ở Test/recovery_core. Nó định nghĩa interface recovery duy nhất của workspace (CLAUDE.md); bản legacy robot_nav_core::RecoveryBehavior đã dừng phát triển và không được port sang.

Consumer đầu tiên là RecoveryRunner của move_base2 — nó include thẳng recovery_core/recovery_behavior.h, nên contract được compiler kiểm đầy đủ ở một nơi.

Vì sao tick-based

Recovery thế hệ trước chạy blocking bên trong một lời gọi và không trả gì. Hệ quả: không cancel được giữa chừng, không báo tiến độ, và không phát được vận tốc — nên mọi behavior cần robot cử động phải tự quay vòng lặp bên trong, tranh quyền phát cmd_vel với controller.

Bản này trả kết quả từng cycle. Ràng buộc kéo theo: recovery phải được tick từ đúng thread đang sở hữu cmd_vel, không được có thread riêng.

Thành phần

Thành phần	Vai trò
`RecoveryBehavior`	Base template-method. Giữ toàn bộ bất biến; plugin chỉ triển khai hook
`RecoveryContext`	Các cổng môi trường: `PoseProvider`, `CollisionChecker`, `PlanProvider`, con trỏ costmap
`RecoveryGoal`	Mục tiêu runtime mỗi lượt: `trigger`, `angle`, `distance`, `target_pose`, `params`
`RecoveryResult`	Kết quả một tick: status, output theo họ, progress/remaining/elapsed/message
`RecoveryRegistry`	Danh sách behavior có thứ tự, nạp từ YAML qua Boost.DLL
`adapters/`	`CostmapPoseProvider`, `CostmapCollisionChecker` — nối cổng vào costmap thật
`plugins/`	Bộ mặc định: wait, clear costmap, rotate, back up

Ai giữ bất biến gì

Đây là điểm khác quan trọng nhất so với bản trước, nơi state là protected và plugin ghi thẳng được vào status_.

Base giữ:

guard vòng đời: start sau configure, update sau start, configure chỉ một lần;
cổng bắt buộc theo outputKind() — họ velocity không có PoseProvider/CollisionChecker thì configure() trả false ngay, chứ không để plugin phát hiện lúc đang lái;
validate goal (NaN/Inf, distance > 0) trước khi plugin nhìn thấy;
đo dt và elapsed bằng đồng hồ thật, ép timeout;
cưỡng chế output_type ∈ {outputKind(), kNone};
chặn NaN/Inf trong lệnh vận tốc;
sinh stop output đúng họ ở mọi nhánh guard/cancel/timeout.

Plugin giữ: đọc param riêng, chốt mục tiêu lượt này, và sinh một tick an toàn. Toàn bộ state của base là private; plugin truy cập qua accessor const.

Luồng runtime

configure(name, ctx, nh)     # 1 lần. Kiểm cổng bắt buộc, đọc timeout, gọi onConfigure(nh).
        │
start(goal, now)             # mỗi lượt. Validate goal, mốc thời gian, gọi onStart(goal) -> bool.
        │
        ├── kNone     : update() -> làm việc / đếm giờ ─────► {status, elapsed, message}
        ├── kVelocity : loop { update(now) } ───────────────► {status, Twist, progress, remaining}
        └── kPath     : update() -> sinh path ──────────────► {status, Path}
        │
cancel()                     # tick kế tiếp gọi onCancel() -> stop output + kCancelled.

Quyết định thiết kế

Cổng, không phải dữ liệu, nằm trong context. PlanProvider là accessor chứ không phải con trỏ tới std::vector vì runtime dùng triple-buffer plan và xoay con trỏ giữa ba bộ đệm — một con trỏ cache lúc configure() sẽ trỏ vào bộ đệm scratch sau vài chu kỳ planner. Cùng lý do đó, con trỏ costmap phải được caller làm mới trước mỗi lượt và plugin không được cache Costmap2D* bên trong.

std::optional thay cho sentinel 0. goal.angle = 0 là yêu cầu hợp lệ ("đừng quay") và phải phân biệt được với "caller không đặt". Quy ước cũ "0 nghĩa là dùng default" khiến một góc tính từ hình học ra ~0 bị âm thầm thay bằng π/2.

Họ output nằm trong kiểu. outputKind() khai một lần và base cưỡng chế. Trước kia mỗi kết quả tự mang output_type nên nó không dùng để route được: một behavior họ path báo kVelocity ở tick đầu, còn base thì sinh Velocity(zero) cho mọi họ ở nhánh terminal.

Param do caller cấp namespace. configure() nhận robot::NodeHandle& đã scope sẵn; plugin không tự dựng NodeHandle("~/" + name) và tự nạp YAML từ disk. Nhờ vậy test chỉ cần trỏ vào cây config của mình, không phải dựng cây config production.

Không có RecoveryDirective. Từng cân nhắc thêm kênh "xin lập plan lại", nhưng state machine của move_base2 đã chuyển RECOVERING → PLANNING với start_planner = true trên cả kSucceeded lẫn kFailed — replan sau recovery đã là hành vi mặc định, thêm kênh riêng là thêm thứ không ai đọc.

kCancelled không qua ranh giới port. move_base2 không tick recovery sau khi cancel (state chuyển sang CANCELLING, nơi mọi nguồn vận tốc bị khoá). Đường onCancel() vẫn phải đúng vì nó là hàng rào cho host khác, nhưng RecoveryTick của move_base2 không cần giá trị tương ứng.

Điều kiện đóng lại phần legacy

robot_nav_core::RecoveryBehavior và robot_clear_costmap_recovery chỉ còn được tham chiếu ở hai nơi, và cả hai biến mất cùng lúc khi move_base2 thay xong move_base cũ:

Nơi	Xử lý
`move_base/src/move_base.cpp` (loader gen-1)	Bị `move_base2` thay, không sửa
`robot_clear_costmap_recovery`	Bị `ClearCostmapRecovery` gen-2 thay, không port

Khoá recovery_behaviors: trong move_base_common_params.yaml đã được gỡ: các entry cũ trỏ tên alias vào .so gen-2 trong khi loader ở đó import theo chữ ký gen-1, và Boost.DLL không kiểm kiểu qua ranh giới .so.

6.2 KiB Raw Permalink Blame History