temporary storage 7/9/2026 19:41

2026-07-09 19:41:20 +07:00
parent 915cf85cc5
commit e9394434ba
17 changed files with 658 additions and 566 deletions
--- a/docs/ARCHITECTURE.md
+++ b/docs/ARCHITECTURE.md
@@ -8,34 +8,41 @@ về output** (path/vận tốc), đồng thời vẫn dùng global path/costmap

 ## Các thành phần

- `RecoveryBehavior` (interface): `initialize` (mirror nav_core) + `runBehavior` (one-shot) +
-  `computeCommand` (per-cycle) + `status`.
- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ.
- `RecoveryConfig`: param chung (control_frequency, timeout) + validate + đọc từ NodeHandle.
+- `RecoveryBehavior` (interface, template-method): API non-virtual `configure`/`start`/`update`/
+  `cancel`; plugin chỉ triển khai hook `onConfigure`/`onStart(goal)`/`onUpdate(dt)`.
+- `RecoveryContext`: gói con trỏ ngữ cảnh (tf/costmap/global_path) truyền một lần qua `configure`.
+- `RecoveryGoal`: mục tiêu RUNTIME mỗi lượt (angle/distance/target_pose/params).
+- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ +
+  rich feedback (`progress`/`remaining`/`elapsed`/`message`), thêm trạng thái `kCancelled`.
 - Plugin mẫu:
-  - `ClearCostmapRecovery`: clear layer costmap theo tên, trả no-output status.
-  - `RotateRecovery`: sinh `Twist.angular.z` theo chu kỳ tới khi đủ góc.
-  - `BackUpRecovery`: sinh `Twist.linear.x < 0` theo chu kỳ tới khi đủ khoảng lùi.
+  - `ClearCostmapRecovery`: clear layer costmap theo tên, one-shot no-output.
+  - `RotateRecovery`: quay tới `goal.angle` (rad); sinh `Twist.angular.z` mỗi cycle.
+  - `BackUpRecovery`: lùi tới `goal.distance` (m); sinh `Twist.linear.x < 0` mỗi cycle.
  - `RegenPathRecovery`: trả lại `robot_nav_msgs::Path` từ `global_path` hiện tại.

 ## Luồng runtime

 ```
-initialize(name, tf, global_path, global, local)   // 1 lần, đọc param qua NodeHandle
+configure(name, ctx)          // 1 lần: cache ctx, đọc config chung, onConfigure()
        │
-        ├── one-shot (họ A/B):  runBehavior() ──────────────► RecoveryResult{status, path|none}
+start(goal)                   // mỗi lượt: chốt mục tiêu runtime, onStart()
        │
-        └── per-cycle (họ C):   loop { computeCommand(dt) } ─► RecoveryResult{status, velocity}
-                                (caller publish command mỗi cycle tới khi status != kRunning)
+        ├── one-shot (họ A/B):  update(dt) 1 lần ─────────► RecoveryResult{status, path|none, msg}
+        │
+        └── per-cycle (họ C):   loop { update(dt) } ──────► RecoveryResult{status, velocity,
+                                (tới khi status != kRunning)             progress, remaining}
 ```

 ## Ghi chú thiết kế

- `computeCommand(dt)` lấy pose robot từ costmap/tf bên trong (nhất quán mirror nav_core),
-  không truyền pose qua tham số.
- Default `RecoveryBehavior::computeCommand(dt)` trả `RecoveryResult::Failed()` để họ A/B không
-  vô tình sinh command mù.
- `RecoveryConfig::validate()` từ chối `NaN/Inf`, `control_frequency <= 0`, `timeout < 0`.
- `RecoveryConfig::fromNodeHandle()` đọc param chung và thay giá trị invalid bằng default an toàn.
+- Template-method: base xử lý guard vòng đời (configure→start→update), `dt<=0`, `timeout`, đo
+  `elapsed`, và `cancel` một chỗ; plugin không lặp lại các guard này.
+- Mục tiêu là RUNTIME qua `RecoveryGoal` (không cố định trong config): cùng plugin phục vụ nhiều
+  yêu cầu góc/khoảng khác nhau. Field = 0 → dùng default plugin đọc ở `onConfigure()`.
+- `onUpdate(dt)` lấy pose robot từ costmap/tf bên trong; không truyền pose qua tham số.
+- Sau `cancel()`, base tự trả stop output (Twist 0) + `kCancelled`.
+- Param chung duy nhất còn lại là `timeout` (s, 0 = không giới hạn), base đọc trong `configure()`
+  từ `~/<name>/timeout`; giá trị `NaN/Inf/<0` bị thay bằng 0. Param RIÊNG của plugin đọc trong
+  `onConfigure()`.
 - Interface KHÔNG include Boost.DLL; export/import là việc của plugin/loader.
 - Plugin mẫu có dùng Boost.DLL alias, nhưng core contract vẫn không biết loader/adaptor.
--- a/docs/PLUGIN_GUIDE.md
+++ b/docs/PLUGIN_GUIDE.md
@@ -9,18 +9,38 @@ Package hiện có 4 plugin mẫu dưới `plugins/`:
 ## Bước chung

 1. Kế thừa `recovery_core::RecoveryBehavior`.
-2. Override `initialize()` — đọc param riêng qua `robot::NodeHandle("~/" + name)`, cache
-   tf/global_path/costmap.
-3. Override method theo họ (xem dưới) + `status()`.
-4. Thêm factory `static Ptr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`.
+2. Override hook `onConfigure()` (tuỳ chọn) — đọc param riêng qua `robot::NodeHandle("~/" + name)`;
+   ngữ cảnh tf/global_path/costmap lấy qua `ctx()`.
+3. Override `onStart(goal)` + `onUpdate(dt)` theo họ (xem dưới). KHÔNG override
+   `configure/start/update/cancel` — base đã lo guard vòng đời/timeout/cancel/elapsed.
+4. Thêm factory `static RecoveryBehaviorPtr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`.
+
+## Vòng đời (goal-driven)
+
+```
+configure(name, ctx)      // 1 lần: cache ctx, đọc config chung, gọi onConfigure()
+        │
+start(goal)               // mỗi lượt: chốt mục tiêu RUNTIME (angle/distance/pose), gọi onStart()
+        │
+loop update(dt)           // mỗi cycle tới khi status != kRunning; base tick elapsed + guard
+        │
+[cancel()]                // update() kế tiếp -> stop output + kCancelled
+```
+
+`RecoveryGoal` là điểm mấu chốt: cùng plugin, mỗi lượt caller đặt `goal.angle` (rad) hay
+`goal.distance` (m) khác nhau; field = 0 nghĩa là dùng default đã cấu hình. Override thêm truyền
+qua `goal.params` (vd `goal.params["angular_speed"] = 0.8`).

 ## Override theo họ

 | Họ | Override | Trả về |
 |----|----------|--------|
-| A. path | `runBehavior()` | `RecoveryResult::PathOut(path, kSucceeded)` |
-| B. none | `runBehavior()` | `RecoveryResult::Succeeded()` / `Failed()` |
-| C. velocity | `computeCommand(dt)` | `RecoveryResult::Velocity(twist, kRunning|kSucceeded)` |
+| A. path | `onUpdate()` (one-shot) | `RecoveryResult::PathOut(path, kSucceeded)` |
+| B. none | `onUpdate()` (one-shot) | `RecoveryResult::Succeeded()` / `Failed()` |
+| C. velocity | `onStart()` chốt goal + `onUpdate(dt)` mỗi cycle | `RecoveryResult::Velocity(twist, kRunning\|kSucceeded)` |
+
+Mọi kết quả nên gắn feedback qua `.withProgress(progress, remaining)` và `.withMessage(...)` để
+caller giám sát tiến độ (progress ∈ [0,1], remaining theo rad/m).

 ## Export bằng Boost.DLL (bắt buộc cho plugin)

@@ -31,10 +51,11 @@ Package hiện có 4 plugin mẫu dưới `plugins/`:
 namespace recovery_plugins {
 class SpinRecovery : public recovery_core::RecoveryBehavior {
 public:
-  static recovery_core::RecoveryBehavior::Ptr create() {
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() {
    return std::make_shared<SpinRecovery>();
  }
-  // override initialize()/computeCommand()/runBehavior()/status()...
+ protected:
+  // override onConfigure()/onStart(goal)/onUpdate(dt)...
 };
 }  // namespace recovery_plugins

@@ -46,10 +67,21 @@ BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)

 ```cpp
 #include <boost/dll/import.hpp>
-auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
+auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(
    path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations);
-recovery_core::RecoveryBehavior::Ptr behavior = loader();
-behavior->initialize(name, tf, global_path, global_costmap, local_costmap);
+recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = loader();
+
+recovery_core::RecoveryContext ctx;
+ctx.tf = tf; ctx.global_path = global_path;
+ctx.global_costmap = global_costmap; ctx.local_costmap = local_costmap;
+behavior->configure(name, ctx);
+
+recovery_core::RecoveryGoal goal;
+goal.angle = 1.57;                    // "quay 90 độ ngay lượt này"
+recovery_core::RecoveryResult r = behavior->start(goal);
+while (r.status == recovery_core::RecoveryStatus::kRunning) {
+  r = behavior->update(dt);          // publish r.command; đọc r.progress/r.remaining/r.message
+}
 ```

 Lưu ý: adapter/test phải giữ handle `.so` sống lâu hơn object plugin. Nếu library bị unload trong
--- a/docs/SAFETY.md
+++ b/docs/SAFETY.md
@@ -11,13 +11,14 @@
  đú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).
- **Default per-cycle fail an toàn.** Behavior không override `computeCommand(dt)` sẽ nhận
-  `RecoveryResult::Failed()` thay vì velocity mặc định.
+- **Guard vòng đời ở base.** `start()` trước `configure()`, hay `update()` trước `start()`, đều trả
+  `RecoveryResult::Failed()`. `dt <= 0`, `NaN`, `Inf`, và `timeout` do base chặn trước khi gọi
+  `onUpdate()`. Sau `cancel()`, base trả stop output (Twist 0) + `kCancelled`.

 ## Nguyên tắc cho plugin

- Guard `initialized_` và costmap/tf null trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`.
- Guard `dt <= 0`, `NaN`, `Inf` trước khi tính velocity.
+- 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 `dt <= 0`/`NaN`/`Inf`/`timeout` — base đã lo; chỉ 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).