temporary storage 7/9/2026 20:41

This commit is contained in:
2026-07-09 20:41:35 +07:00
parent e9394434ba
commit 17088a227d
12 changed files with 82 additions and 107 deletions

34
PLAN.md
View File

@@ -28,7 +28,7 @@ plugin recovery có thể trả trạng thái, velocity command hoặc path.
- Cung cấp ngữ cảnh + mục tiêu runtime:
- `RecoveryContext` (tf/costmap/global_path)
- `RecoveryGoal` (angle/distance/target_pose/params)
- Param chung `timeout` do base đọc trong `configure()` (param riêng plugin đọc `onConfigure()`).
- Param riêng plugin đọc trong `onConfigure()`; core base không giữ timeout chung.
- Cung cấp docs/test stub để plugin sau này implement đúng contract.
### 2.2. `recovery_core` không chịu trách nhiệm
@@ -120,7 +120,7 @@ public:
robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
virtual RecoveryResult runBehavior() = 0;
virtual RecoveryResult computeCommand(double dt);
virtual RecoveryResult update();
virtual RecoveryStatus status() const = 0;
protected:
@@ -131,7 +131,7 @@ protected:
Điểm khác `robot_nav_core::RecoveryBehavior`:
- Có thêm `global_path` để behavior họ path có ngữ cảnh plan hiện tại.
- `runBehavior()` trả `RecoveryResult` thay vì `void`.
-`computeCommand(dt)` cho behavior per-cycle.
-`update()` cho behavior per-cycle.
## 4. Ba Nhóm Recovery
@@ -139,7 +139,7 @@ protected:
|------|-------|--------------|--------|
| A. Path output | regen path, detour path | `runBehavior()` | `RecoveryOutputType::kPath` |
| B. No output | clear costmap, reset state | `runBehavior()` | `RecoveryOutputType::kNone` |
| C. Velocity output | rotate, backup | `computeCommand(dt)` | `RecoveryOutputType::kVelocity` |
| C. Velocity output | rotate, backup | `update()` | `RecoveryOutputType::kVelocity` |
Caller/adapter chịu trách nhiệm tiêu thụ output:
- path output: thay local path hoặc request global/local replan;
@@ -228,10 +228,10 @@ Work items:
1. [x] Implement `RecoveryResult` factories.
2. [x] Implement `RecoveryConfig::validate`.
3. [x] Implement `RecoveryConfig::fromNodeHandle`.
4. [x] Giữ default `RecoveryBehavior::computeCommand(dt)` trả `Failed()`.
4. [x] Giữ guard `RecoveryBehavior::update()` fail an toàn khi chưa start.
5. Deferred: helper chạy loop cho behavior velocity chỉ thêm khi Phase 4 integration cần:
- dùng `control_frequency`;
- tôn trọng `timeout`;
- dùng nhịp gọi từ adapter;
- giám sát ngoài core nếu cần giới hạn thời gian;
- không cấp phát/log trong loop.
6. [x] Hoàn thiện `MockBehavior`.
7. [x] Nâng `interface_contract_test.cpp` từ smoke test thành assertion test.
@@ -243,8 +243,8 @@ Acceptance:
- `RecoveryResult::Failed()` trả `status = kFailed`, `output_type = kNone`.
- `Velocity(command, status)` giữ `command`, set `output_type = kVelocity`.
- `PathOut(path, status)` giữ `path`, set `output_type = kPath`.
- Config reject `NaN`, `inf`, `control_frequency <= 0`, `timeout < 0`.
- Default `computeCommand(dt)` không sinh velocity mù.
- Config/plugin reject input không hợp lệ.
- Default `update()` không sinh velocity mù khi lifecycle sai.
- Test cover factory, config validation, default per-cycle behavior, mock lifecycle.
Verify commands:
@@ -275,12 +275,12 @@ Plugin đề xuất:
- [x] trả `RecoveryResult::Succeeded()` hoặc `Failed()`.
2. `RotateRecovery`
- [x] nhóm C, per-cycle velocity;
- [x] đọc `target_angle`, `angular_speed`, `timeout`;
- [x] dùng tích phân theo `dt` trong plugin mẫu; adapter production có thể thay bằng pose/tf;
- [x] đọc `target_angle`, `angular_speed`, `control_period`;
- [x] dùng tích phân theo `control_period` trong plugin mẫu; adapter production có thể thay bằng pose/tf;
- [x] trả zero command khi kết thúc hoặc fail.
3. `BackUpRecovery`
- [x] nhóm C, per-cycle velocity;
- [x] đọc `backup_distance`, `linear_speed`, `timeout`;
- [x] đọc `backup_distance`, `linear_speed`, `control_period`;
- [x]`require_costmap` để fail nếu thiếu local costmap trước khi trả backward velocity.
4. `RegenPathRecovery` hoặc `DetourPathRecovery`
- [x] nhóm A, path output;
@@ -344,22 +344,22 @@ Phương án:
- Adapter chịu trách nhiệm:
- load plugin;
- gọi `initialize`;
- gọi `runBehavior` hoặc loop `computeCommand`;
- gọi `runBehavior` hoặc loop `update`;
- publish velocity nếu output là `kVelocity`;
- thay path hoặc request replan nếu output là `kPath`;
- áp safety stop nếu output failed/timeout.
- áp safety stop nếu output failed hoặc giám sát ngoài core báo lỗi.
Acceptance:
- Core vẫn không publish.
- Plugin vẫn chỉ trả `RecoveryResult`.
- Adapter có safety gate trước velocity command.
- Timeout và failure path luôn trả stop command hoặc abort rõ ràng.
- Failure path luôn trả stop command hoặc abort rõ ràng.
## 8. Safety Requirements
- Behavior velocity phải trả stop command khi không chắc an toàn.
- Không publish command từ core/plugin nếu chưa qua adapter safety gate.
- `dt <= 0`, `NaN`, `inf`, missing tf/costmap/plan phải fail rõ ràng.
- Input `NaN`, `inf`, missing tf/costmap/plan phải fail rõ ràng.
- Không log spam trong control loop.
- Không parse YAML hoặc cấp phát lớn trong mỗi cycle.
- Đơn vị phải rõ:
@@ -399,7 +399,7 @@ Acceptance:
### Integration DoD
- Adapter là nơi duy nhất có side effect publish/service/path replacement.
- Safety stop rõ ràng khi failed/timeout.
- Safety stop rõ ràng khi failed hoặc adapter hủy recovery.
- Có log đủ ngữ cảnh, không spam loop.
## 10. Verification Baseline