optimal & fix file cmake

docs/SAFETY.md

@@ -1,30 +1,64 @@
-# An Toàn — recovery_core
+# An toàn — recovery_core
 
-> Cảnh báo an toàn khi dùng interface này. Đọc trước khi triển khai plugin họ vận tốc.
+> Đọ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.
 
-## recovery_core KHÔNG đảm bảo
+## Base bảo đảm
 
-- **Không collision-check tự động.** Interface không kiểm tra va chạm khi sinh vận tốc
-  (backup/spin) hay khi tạo path. Việc tránh va chạm là trách nhiệm của **plugin** (dùng
-  costmap được cấp qua `initialize`) hoặc của **caller**.
-- **Không quản lý vòng lặp thời gian thực.** Caller chịu trách nhiệm gọi `update()` đúng nhịp
-  và publish command.
-- **Không đảm bảo frame/đơn vị.** Pose lấy từ costmap/tf phải đúng frame; đơn vị phải nhất quán
-  (m, rad, s, m/s, rad/s).
-- **Guard vòng đời ở base.** `start()` trước `configure()`, hay `update()` trước `start()`, đều trả
-  `RecoveryResult::Failed()`. Sau `cancel()`, base trả stop output (Twist 0) + `kCancelled`.
+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ớ.
 
-## Nguyên tắc cho plugin
+| 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` |
 
-- Guard costmap/tf null (lấy qua `ctx()`) trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`.
-- Không cần tự guard vòng đời/cancel — base đã lo; plugin tập trung logic recovery.
-- Với họ vận tốc: khi không chắc an toàn, trả **stop command** (Twist 0), không trả vận tốc mù.
-- Kiểm tra NaN/Inf của pose/vận tốc trước khi xuất command.
-- Tôn trọng giới hạn vận tốc/gia tốc của robot (đọc qua param).
-- `BackUpRecovery` có param `require_costmap` để bắt buộc có local costmap trước khi xuất
-  vận tốc lùi. Adapter production vẫn nên có safety gate/collision check riêng trước publish.
+## Base KHÔNG bảo đảm
 
-## Trách nhiệm caller/adapter
+- **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.
 
-- Đảm bảo ngữ cảnh cho phép recovery (vd: đã dừng planner, vùng xung quanh đủ an toàn).
-- Áp timeout/giám sát ngoài để tránh recovery chạy vô hạn.
+## Quy tắc cho plugin họ vận tốc
+
+1. **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.
+2. **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.
+3. **Mất pose là dừng.** `getRobotPose()` trả `false` → trả `kFailed` + Twist 0. Không dùng pose cũ.
+4. **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.
+5. **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.
+6. **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.
+
+## Quy tắc chung cho mọi plugin
+
+- `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.
+
+## Trách nhiệm của caller
+
+- 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.