An toàn — recovery_core

Đọc trước khi viết plugin họ vận tốc. Recovery chạy đúng lúc robot đã ở tình huống xấu, nên default ở đây phải fail-safe chứ không fail-open.

Base bảo đảm

Những điều dưới đây đúng kể cả khi plugin viết ẩu — base cưỡng chế, không phụ thuộc plugin nhớ.

Bất biến	Cơ chế
Họ velocity không chạy được nếu thiếu pose hoặc collision checker	`configure()` trả `false` khi `ctx.pose`/`ctx.collision` null
`dt` là thời gian thật, không phải chu kỳ cấu hình	`update(now)` tính `now - lần trước`; đồng hồ đi lùi → `dt = 0`
`elapsed` luôn có mặt trên mọi kết quả	Base ghi, plugin không phải đặt
Recovery không treo vô hạn	Param `timeout` [s]; quá hạn → `kFailed` + stop output
`output_type` luôn thuộc `{outputKind(), kNone}`	Sai họ → hạ về `kNone`, xoá dữ liệu, log lỗi
NaN/Inf không ra được `cmd_vel`	Lệnh không hữu hạn → Twist 0 + `kFailed`
Guard vòng đời không cần plugin tự làm	`start` trước `configure`, `update` trước `start` → `kFailed` + stop output
Stop output đúng họ	Họ velocity nhận Twist 0 tường minh; họ khác nhận `kNone`

Không thay caller quyết định có nên recovery hay không. Ngữ cảnh cho phép recovery (đã dừng planner, không đang ở chế độ thủ công, không đang estop) là việc của caller.
Không quản lý nhịp gọi. Caller phải tick đúng chu kỳ control và publish lệnh.
Không là hàng rào vận tốc cuối cùng. Plugin clamp theo giới hạn của mình; VelocityArbiter phía ngoài vẫn phải clamp lần cuối. Hai tầng, không chồng nhau: plugin lập kế hoạch dừng/ramp, arbiter chặn giá trị vượt ngưỡng.

Không tích phân vận tốc lệnh để đo tiến độ. Dùng PoseProvider và hình chiếu delta pose (recovery_core::projectOntoHeading). Tích phân lệnh thì bánh trượt hay robot bị chặn vẫn báo đi đủ quãng — và đó là lúc nguy hiểm nhất để nói dối.
Kiểm va chạm trên pose dự đoán, trước khi phát lệnh. Không phải sau. BackUpRecovery dò pose ở cuối chu kỳ tới; RotateRecovery quét toàn bộ cung ngay tại onStart() và refuse khởi động nếu có bất kỳ góc nào bị chặn.
Mất pose là dừng. getRobotPose() trả false → trả kFailed + Twist 0. Không dùng pose cũ.
Ramp theo acc_lim_*. Tick đầu không được nhảy thẳng lên tốc độ tối đa; onCancel() cũng nên giảm tốc thay vì nhảy bậc.
Clamp mọi param có dải. angle ∈ [-2π, 2π], distance ∈ (0, *_max], tốc độ ≤ trần. Param sai thì log cảnh báo kèm giá trị rồi dùng default — không im lặng.
Ghi rõ dấu. linear.x < 0 là lùi; angular.z > 0 là ngược chiều kim đồng hồ. Param cấu hình là độ lớn, dấu do plugin đặt theo ngữ cảnh.

onUpdate() chạy trên thread phát cmd_vel. Không block quá một phần nhỏ chu kỳ control: không parse file, không cấp phát lớn, không gọi việc nặng của costmap (ClearCostmapRecovery cố ý không gọi updateMap() vì lý do này).
Không cache Costmap2D* qua các tick. Con trỏ có thể bị thay giữa hai cycle; lấy lại mỗi lần dùng.
Kiểm mã trả về của CollisionChecker bằng < 0, không so với một giá trị âm cụ thể — các hiện thực trong workspace không thống nhất mã lỗi.
configure() trả false khi cấu hình không chạy được, thay vì log warning rồi chạy tiếp bằng default. Registry sẽ bỏ behavior đó và log đích danh.

Làm mới con trỏ costmap trong RecoveryContext trước mỗi start()/update().
Tick recovery từ đúng một thread — thread sở hữu cmd_vel. Thread riêng cho recovery nghĩa là hai bộ điều khiển cùng phát vận tốc.
Chỉ gọi update() sau khi start() trả true.
Sau cancel(), hoặc tick tiếp để nhận stop output, hoặc tự khoá nguồn vận tốc về 0.