65 lines
4.5 KiB
Markdown
65 lines
4.5 KiB
Markdown
# 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` |
|
|
|
|
## Base KHÔNG bảo đảm
|
|
|
|
- **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.
|
|
|
|
## 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.
|