temporary storage

PLAN.md

@@ -1,420 +1,431 @@
-# Kế Hoạch Xây Dựng Package `recovery_core` (Interface Thuần, Không Chạy ROS)
+# PLAN - `recovery_core`
 
-> `recovery_core` là **package ĐỊNH NGHĨA INTERFACE** cho các hành vi recovery — đóng đúng
-> vai trò như `robot_nav_core` cung cấp `RecoveryBehavior`, nhưng **không chạy ROS** và
-> biểu đạt hợp đồng qua `robot_geometry_msgs` / `robot_nav_msgs` (header-only).
->
-> Bản thân package **không** chứa hành vi cụ thể (backup, spin, clear costmap, regen path).
-> Các hành vi đó là **plugin implement interface** — làm ở Phase 3 hoặc ở package khác.
+`recovery_core` là package **interface** cho recovery behavior trong navigation stack ROS-like
+T800. Package này không chứa thuật toán recovery cụ thể; nó định nghĩa contract chung để các
+plugin recovery có thể trả trạng thái, velocity command hoặc path.
 
----
+## 1. Mục Tiêu
 
-## 1. Bối Cảnh & Quyết Định Thiết Kế
+- Cung cấp base class `recovery_core::RecoveryBehavior` cho các hành vi recovery.
+- Dựa trên pattern `robot_nav_core::RecoveryBehavior`, nhưng mở rộng output để bao được 3 nhóm:
+  - recovery không output, ví dụ clear/reset costmap;
+  - recovery sinh vận tốc theo từng cycle, ví dụ rotate/backup;
+  - recovery sinh path, ví dụ regen path hoặc detour path.
+- Dùng stack ROS-like trong workspace (`robot_*`, `tf3`, `robot_costmap_2d`), không phụ thuộc
+  `roscpp` hoặc ROS master thật.
+- Giữ core sạch: plugin trả `RecoveryResult`; caller/adapter quyết định publish command, thay
+  path, clear service hoặc request replan.
 
-### 1.1. `recovery_core` là gì
+## 2. Ranh Giới Thiết Kế
 
-- Là **thư viện interface** (giống `robot_nav_core::RecoveryBehavior`), nhưng ROS-free:
-  - **Không** include `robot/robot.h`, **không** `NodeHandle`, **không** `Costmap2DROBOT`,
-    **không** `tf3::BufferCore`.
-  - Chỉ phụ thuộc kiểu dữ liệu header-only: `robot_geometry_msgs` (Twist, Pose2D, PoseStamped)
-    và `robot_nav_msgs` (Path). Các kiểu này không kéo runtime ROS.
-- Cung cấp **một base class thuần ảo** đủ tổng quát để bao 3 họ recovery (mục 1.2), cùng các
-  kiểu phụ trợ (status, result, config, context).
+### 2.1. `recovery_core` chịu trách nhiệm
 
-So với `robot_nav_core::RecoveryBehavior` (blocking `runBehavior()`, cần costmap + tf):
-`recovery_core` giữ *tinh thần interface* nhưng thay hạ tầng ROS bằng abstraction ROS-free và
-mở rộng để hành vi có thể **trả về output** (không chỉ chạy rồi thôi).
+- Định nghĩa interface `RecoveryBehavior`.
+- Định nghĩa result contract:
+  - `RecoveryStatus`
+  - `RecoveryOutputType`
+  - `RecoveryResult`
+- Cung cấp helper config chung:
+  - `RecoveryConfig::control_frequency`
+  - `RecoveryConfig::timeout`
+  - `validate()`
+  - `fromNodeHandle()`
+- Cung cấp docs/test stub để plugin sau này implement đúng contract.
 
-### 1.2. Ba họ recovery interface phải bao được
+### 2.2. `recovery_core` không chịu trách nhiệm
 
-| Họ | Ví dụ | Output đặc trưng |
-|----|-------|------------------|
-| **A. Trả v<><76> path** | tạo lại một đoạn đường thoát/né | `robot_nav_msgs::Path` |
-| **B. Không output** | clear costmap, reset state | chỉ trạng thái (SUCCEEDED/FAILED) |
-| **C. Trả về vận tốc** | rotation tại chỗ, backup (không ROS) | `robot_geometry_msgs::Twist` theo cycle |
+- Không implement recovery cụ thể như clear costmap, rotate, backup, regen path.
+- Không publish `cmd_vel`.
+- Không gọi ROS service hoặc action.
+- Không tự collision-check velocity/path output.
+- Không trực tiếp nạp plugin bằng Boost.DLL trong core interface.
+- Không thay thế trực tiếp `robot_nav_core::RecoveryBehavior` trong `move_base` nếu chưa có
+  adapter riêng.
 
-Interface phải diễn đạt cả 3 mà **không ép** một hành vi phải điền output nó không dùng.
-→ Dùng **một `RecoveryResult` hợp nhất** mang cờ *loại output* + các trường optional
-(status luôn có; `twist`, `path` chỉ hợp lệ khi cờ tương ứng bật).
+## 3. Interface Contract Đã Chốt
 
-### 1.3. Ranh giới trách nhiệm
-
-`recovery_core` **CHỈ** cung cấp:
-- Base class `RecoveryBehavior` (thuần ảo) + vòng đời chuẩn.
-- Kiểu hợp đồng: `RecoveryStatus`, `RecoveryOutputType`, `RecoveryResult`, `RecoveryConfig`,
-  `RecoveryContext` (abstraction ROS-free thay cho tf/costmap/publisher).
-- Không thuật toán hành vi cụ thể, không collision-check, không I/O, không vòng lặp thời gian.
-
-Caller / plugin chịu trách nhiệm: cấp pose (qua `RecoveryContext`), tiêu thụ output
-(Twist → cmd_vel, Path → planner), đảm bảo an toàn.
-
-### 1.4. Vị trí & chuẩn
-
-- Đặt tại `.../Navigations/Libraries/recovery_core` (cùng cấp `robot_clear_costmap_recovery`).
-- C++17, namespace `recovery_core`, guard `RECOVERY_CORE_<FILE>_H_`.
-- Identifiers/comments tiếng Anh; tài liệu `.md` tiếng Việt.
-- Build catkin + standalone (theo pattern `robot_clear_costmap_recovery`).
-- Phần lớn là **header-only** (interface) — `.cpp` chỉ cho helper/validate không inline.
-
----
-
-## 2. Thiết Kế Interface (bản chốt ở Phase 2, nháp ở đây)
-
-### 2.1. Kiểu hợp đồng
+### 3.1. Recovery status
 
 ```cpp
-namespace recovery_core {
+enum class RecoveryStatus
+{
+  kIdle,
+  kRunning,
+  kSucceeded,
+  kFailed
+};
+```
 
-// Trạng thái tiến trình 1 lượt recovery.
-enum class RecoveryStatus { kIdle, kRunning, kSucceeded, kFailed };
+Ý nghĩa:
+- `kIdle`: đã tạo/initialize nhưng chưa chạy.
+- `kRunning`: behavior cần được gọi tiếp.
+- `kSucceeded`: behavior hoàn thành.
+- `kFailed`: behavior lỗi hoặc không thể tiếp tục an toàn.
 
-// Loại output mà hành vi sinh ra ở cycle hiện tại.
-enum class RecoveryOutputType { kNone, kVelocity, kPath };
+### 3.2. Output type
 
-// Kết quả hợp nhất cho cả 3 họ. Chỉ đọc trường khớp với output_type.
-struct RecoveryResult {
+```cpp
+enum class RecoveryOutputType
+{
+  kNone,
+  kVelocity,
+  kPath
+};
+```
+
+Quy ước:
+- `kNone`: chỉ đọc `status`.
+- `kVelocity`: chỉ đọc `command`.
+- `kPath`: chỉ đọc `path`.
+
+### 3.3. Recovery result
+
+```cpp
+struct RecoveryResult
+{
   RecoveryStatus status = RecoveryStatus::kRunning;
   RecoveryOutputType output_type = RecoveryOutputType::kNone;
 
-  robot_geometry_msgs::Twist command;   // hợp lệ khi output_type == kVelocity
-  robot_nav_msgs::Path   path;          // hợp lệ khi output_type == kPath
+  robot_geometry_msgs::Twist command;
+  robot_nav_msgs::Path path;
 
-  // Tiện ích khởi tạo nhanh (định nghĩa trong .cpp hoặc inline):
   static RecoveryResult Running();
   static RecoveryResult Succeeded();
   static RecoveryResult Failed();
-  static RecoveryResult Velocity(const robot_geometry_msgs::Twist&, RecoveryStatus);
-  static RecoveryResult PathOut(const robot_nav_msgs::Path&, RecoveryStatus);
+  static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
+                                 RecoveryStatus status);
+  static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
+                                RecoveryStatus status);
 };
 ```
 
-### 2.2. Context ROS-free (thay tf/costmap/publisher)
+Factory phải giữ bất biến:
+- `Running/Succeeded/Failed` dùng `output_type = kNone`.
+- `Velocity(...)` dùng `output_type = kVelocity`.
+- `PathOut(...)` dùng `output_type = kPath`.
 
-`RecoveryContext` là abstraction để plugin lấy trạng thái thế giới mà **không** biết ROS.
-`recovery_core` chỉ khai báo interface; caller cấp implementation cụ thể (đọc từ đâu là việc
-của caller).
+### 3.4. Recovery behavior
 
 ```cpp
-// Interface tối thiểu; mở rộng dần khi có nhu cầu thực.
-class RecoveryContext {
- public:
-  virtual ~RecoveryContext() = default;
-  // Pose robot hiện tại trong frame quy ước (rad, m). Trả false nếu không có.
-  virtual bool getRobotPose(robot_geometry_msgs::Pose2D* out) const = 0;
-  // (tuỳ chọn, cho họ cần) truy vấn cost tại điểm; mặc định không hỗ trợ.
-  // virtual bool getCost(double x, double y, unsigned char* cost) const { return false; }
-};
-```
-
-> Ghi chú: interface base **không ép** hành vi phải dùng context (họ C rotation thuần có thể
-> chỉ cần pose truyền vào computeCommand). Context là kênh mở rộng cho họ A/B cần hỏi thế giới.
-
-### 2.3. Base class interface
-
-```cpp
-class RecoveryBehavior {
- public:
-  // shared_ptr để KHỚP cơ chế nạp Boost.DLL của workspace:
-  //   boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(...)
-  // (xem move_base.cpp loadRecoveryBehaviors + robot_nav_core::RecoveryBehavior::Ptr).
+class RecoveryBehavior
+{
+public:
   using Ptr = std::shared_ptr<RecoveryBehavior>;
+
   virtual ~RecoveryBehavior() = default;
 
-  // 1) Nạp + validate config. Không throw; trả false + set error nếu sai.
-  virtual bool configure(const RecoveryConfig& config, std::string* error) = 0;
+  virtual void initialize(std::string name,
+                          tf3::BufferCore* tf,
+                          std::vector<robot_geometry_msgs::PoseStamped>* global_path,
+                          robot_costmap_2d::Costmap2DROBOT* global_costmap,
+                          robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
 
-  // 2) Bắt đầu 1 lượt: chốt state khởi đầu, reset progress về kRunning.
-  virtual void start(const robot_geometry_msgs::Pose2D& current_pose) = 0;
-
-  // 3a) Kiểu PER-CYCLE (họ C, và họ A nếu sinh path dần): gọi mỗi control cycle.
-  //     dt (s) > 0. Guard chưa configure/start -> Failed + output kNone.
-  virtual RecoveryResult computeCommand(const robot_geometry_msgs::Pose2D& current_pose,
-                                        double dt) = 0;
-
-  // 3b) Kiểu ONE-SHOT/BLOCKING (họ B clear costmap, họ A regen path 1 lần):
-  //     chạy trọn hành vi qua context, trả kết quả cuối. Mặc định: lặp computeCommand.
-  virtual RecoveryResult runBehavior(RecoveryContext* ctx);   // có default impl
-
-  virtual void reset() = 0;
+  virtual RecoveryResult runBehavior() = 0;
+  virtual RecoveryResult computeCommand(double dt);
   virtual RecoveryStatus status() const = 0;
-  virtual const char* name() const = 0;
 
- protected:
+protected:
   RecoveryBehavior() = default;
 };
 ```
 
-**Vì sao có cả `computeCommand` lẫn `runBehavior`:**
-- Họ **C** (rotation/backup) tự nhiên là *per-cycle* → override `computeCommand`.
-- Họ **B** (clear costmap) tự nhiên là *one-shot* → override `runBehavior` (dùng `ctx`),
-  `computeCommand` chỉ trả trạng thái.
-- Họ **A** (regen path) có thể one-shot (`runBehavior` trả `Path`) hoặc per-cycle tuỳ plugin.
-- `runBehavior` có **default implementation** trong `recovery_core` (lặp `computeCommand` tới
-  khi khác `kRunning`) để plugin per-cycle không phải viết lại; plugin one-shot thì override.
+Đ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`.
+- Có `computeCommand(dt)` cho behavior per-cycle.
 
-### 2.4. Config
+## 4. Ba Nhóm Recovery
 
-```cpp
-struct RecoveryConfig {
-  std::string type;                 // "back_up" | "spin" | "clear_costmap" | "regen_path" ...
-  double control_frequency = 20.0;  // Hz, dùng cho default runBehavior loop (dt = 1/f)
-  double timeout = 0.0;             // s, 0 = không timeout
-  std::map<std::string, double> params;   // tham số riêng của từng plugin
-  // validate(): control_frequency > 0, timeout >= 0, hữu hạn. Trả false + message.
-  bool validate(std::string* error) const;
-};
-```
+| Nhóm | Ví dụ | Method chính | Output |
+|------|-------|--------------|--------|
+| 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` |
 
-> Package interface **không** biết tham số riêng của backup/spin — để plugin tự đọc từ
-> `params`. `recovery_core` chỉ validate phần chung.
+Caller/adapter chịu trách nhiệm tiêu thụ output:
+- path output: thay local path hoặc request global/local replan;
+- no output: tiếp tục navigation hoặc chuyển behavior kế tiếp;
+- velocity output: publish command ngoài core, có safety gate trước khi gửi robot.
 
----
+## 5. Package Layout
 
-## 3. PHASE 1 — Dựng Khung Package (KHÔNG code triển khai)
-
-**Mục tiêu:** tạo cây thư mục + file stub (header có guard/namespace/Doxygen, `.cpp` thân
-rỗng compile được), `package.xml`, `CMakeLists.txt` build ra lib rỗng. Package phải configure
-CMake thành công.
-
-### 3.1. Cây thư mục mục tiêu
-
-```
+```text
 recovery_core/
-├── PLAN.md                            # tài liệu này
-├── README.md                          # phạm vi: INTERFACE, không ROS, 3 họ recovery
-├── package.xml                        # depend: robot_geometry_msgs, robot_nav_msgs
-├── CMakeLists.txt                     # catkin + standalone; lib chủ yếu header, .cpp cho helper
-├── include/
-│   └── recovery_core/
-│       ├── recovery_types.h           # enum Status, OutputType; struct RecoveryResult
-│       ├── recovery_config.h          # struct RecoveryConfig + validate()
-│       ├── recovery_context.h         # interface RecoveryContext (ROS-free)
-│       └── recovery_behavior.h        # base class RecoveryBehavior (interface chính)
+├── CMakeLists.txt
+├── package.xml
+├── README.md
+├── PLAN.md
+├── include/recovery_core/
+│   ├── recovery_behavior.h
+│   ├── recovery_config.h
+│   └── recovery_types.h
 ├── src/
-│   ├── recovery_types.cpp             # (stub) factory helper Running()/Failed()/...
-│   ├── recovery_config.cpp            # (stub) validate()
-│   └── recovery_behavior.cpp          # (stub) default runBehavior() loop
+│   ├── recovery_behavior.cpp
+│   ├── recovery_config.cpp
+│   └── recovery_types.cpp
 ├── test/
-│   ├── CMakeLists.txt                 # khai báo test target (tuỳ chọn)
-│   ├── interface_contract_test.cpp    # (stub) dùng 1 MockBehavior kiểm vòng đời
-│   └── mock_behavior.h                # (stub) implement tối thiểu để test interface
+│   ├── CMakeLists.txt
+│   ├── interface_contract_test.cpp
+│   └── mock_behavior.h
 ├── examples/
-│   └── minimal_plugin.cpp             # (stub) ví dụ 1 plugin per-cycle bé xíu, không ROS
+│   └── minimal_recovery.cpp
 └── docs/
-    ├── ARCHITECTURE.md                # sơ đồ interface + luồng runBehavior/computeCommand
-    ├── PLUGIN_GUIDE.md                # hướng dẫn viết plugin cho từng họ A/B/C
-    └── SAFETY.md                      # cảnh báo: không collision-check trong core
+    ├── ARCHITECTURE.md
+    ├── PLUGIN_GUIDE.md
+    └── SAFETY.md
 ```
 
-### 3.2. Quy ước nội dung stub (Phase 1)
+## 6. Dependencies
 
-- **Header**: license/author ngắn, guard `RECOVERY_CORE_*_H_`, include tối thiểu
-  (`<robot_geometry_msgs/Twist.h>`, `<robot_geometry_msgs/Pose2D.h>`,
-  `<robot_nav_msgs/Path.h>` khi cần), khai báo đầy đủ chữ ký + Doxygen public API, thân hàm
-  non-inline để trong `.cpp`.
-- **Source**: thân tối thiểu `// TODO(phase-2): implement` + `return {}` để compile.
-- **`package.xml`**: format 2, `<depend>robot_geometry_msgs</depend>`,
-  `<depend>robot_nav_msgs</depend>`, buildtool catkin. **Không** depend costmap/tf/robot_cpp.
-- **`CMakeLists.txt`**: khung catkin+standalone của `robot_clear_costmap_recovery` nhưng chỉ
-  giữ 2 msgs depend; tạo library `recovery_core` từ các `.cpp` (kể cả khi phần lớn header-only,
-  vẫn build 1 lib nhỏ cho validate/helper). Install header + export target 2 chế độ.
+Runtime/build dependencies:
+- `robot_costmap_2d`
+- `robot_cpp`
+- `robot_time`
+- `robot_geometry_msgs`
+- `robot_nav_msgs`
+- `tf3`
+- `Boost system thread`
 
-### 3.3. Checklist Phase 1
+Không phụ thuộc:
+- `robot_nav_core`
+- `roscpp`
+- `pluginlib`
 
-- [ ] Tạo đủ cây thư mục 3.1.
-- [ ] 4 header có guard, namespace `recovery_core`, Doxygen; khai báo khớp mục 2.
-- [ ] 3 `.cpp` compile với thân stub.
-- [ ] `package.xml` chỉ depend `robot_geometry_msgs` + `robot_nav_msgs`.
-- [ ] `CMakeLists.txt` configure OK cả catkin lẫn standalone; build ra `librecovery_core` rỗng.
-- [ ] `README.md` nêu rõ: đây là INTERFACE, không ROS, 3 họ recovery, không hành vi cụ thể.
-- [ ] **Kiểm chứng:** `cmake` + `make` thành công.
+## 7. Roadmap
 
----
+### Phase 1 - Package Skeleton
 
-## 4. PHASE 2 — Triển Khai Interface (điền logic phần thuộc về core)
+Trạng thái: **done**.
 
-> Core là interface nên "triển khai" ở đây = hoàn thiện **phần chung** mà base cung cấp, KHÔNG
-> phải viết hành vi cụ thể. Cụ thể:
+Mục tiêu:
+- Tạo package skeleton.
+- Tạo 3 header interface.
+- Tạo 3 source stub compile được.
+- Tạo README/docs/example/test stub.
+- CMake hỗ trợ catkin và standalone.
+- `package.xml` đúng dependency, không kéo `robot_nav_core`.
 
-### 4.1. Nội dung triển khai
+Acceptance:
+- `catkin_make --pkg recovery_core` pass.
+- Standalone `cmake` + `make` pass.
+- Smoke test chạy được.
+- `package.xml` parse được bằng `catkin_pkg`.
+- `README.md` mô tả rõ đây là interface package, không phải behavior implementation.
 
-1. **`recovery_types`**: định nghĩa các factory `RecoveryResult::Running/Succeeded/Failed/`
-   `Velocity/PathOut`, đảm bảo set đúng `output_type` + `status`. Bất biến: khi
-   `output_type==kNone` thì `command`/`path` để mặc định (không rác).
-2. **`recovery_config::validate`**: kiểm `control_frequency > 0`, `timeout >= 0`, hữu hạn;
-   `type` không rỗng. Trả `false` + message rõ. Không throw.
-3. **`recovery_behavior.cpp` — default `runBehavior(ctx)`**:
-   - Guard `ctx == nullptr` → `Failed`.
-   - Lấy pose qua `ctx->getRobotPose`; nếu fail → `Failed`.
-   - `dt = 1 / control_frequency`; lặp `computeCommand(pose, dt)` tới khi status khác
-     `kRunning` **hoặc** vượt `timeout` (nếu > 0) → khi timeout trả `Failed`.
-   - Mỗi vòng cập nhật pose qua `ctx` (mô phỏng caller thật). Trả result cuối.
-   - **Không** cấp phát trong vòng lặp; không log spam (chỉ log khi đổi status).
-4. **Bất biến & guard chung** (document + test):
-   - Chưa `configure` thành công → mọi call trả `Failed`.
-   - `dt <= 0` hoặc pose NaN/Inf → `Failed`, output `kNone`.
+Kết quả hiện tại:
+- Catkin output: `devel/lib/librecovery_core.so`.
+- Catkin test binary: `devel/lib/recovery_core/recovery_core_interface_test`.
+- Standalone output: `/tmp/recovery_core_phase1_build/librecovery_core.a`.
+- Standalone test binary: `/tmp/recovery_core_phase1_build/test/recovery_core_interface_test`.
 
-### 4.2. Kiểm thử interface (không cần plugin thật)
+### Phase 2 - Core Contract Implementation
 
-- `test/mock_behavior.h`: `MockBehavior` per-cycle đơn giản (đếm N cycle rồi `kSucceeded`,
-  trả `kVelocity`) + `MockContext` cấp pose cố định.
-- `interface_contract_test.cpp`:
-  - `configure` sai → `false` + message.
-  - `start` → `status()==kRunning`.
-  - `computeCommand` tiến trình đúng, đạt đích → `kSucceeded`.
-  - default `runBehavior` chạy hết vòng đời qua `MockContext`, tôn trọng `timeout`.
-  - guard: chưa configure, dt<=0, ctx null, pose NaN.
+Trạng thái: **done**.
 
-### 4.3. Checklist Phase 2
+Mục tiêu:
+- Hoàn thiện phần logic chung của interface, chưa viết recovery cụ thể.
 
-- [ ] `RecoveryResult` factories set đúng cờ; có test bất biến output.
-- [ ] `RecoveryConfig::validate` từ chối mọi input sai với message rõ.
-- [ ] default `runBehavior` đúng: loop, timeout, guard ctx/pose; không alloc trong loop.
-- [ ] `MockBehavior`/`MockContext` + contract test pass.
-- [ ] `docs/PLUGIN_GUIDE.md` mô tả cách 1 plugin họ A/B/C override method nào.
-- [ ] **Kiểm chứng:** build + test pass.
+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()`.
+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`;
+   - 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.
+8. [x] Cập nhật `docs/ARCHITECTURE.md` và `docs/SAFETY.md` theo contract thật.
 
----
+Acceptance:
+- `RecoveryResult::Running()` trả `status = kRunning`, `output_type = kNone`.
+- `RecoveryResult::Succeeded()` trả `status = kSucceeded`, `output_type = kNone`.
+- `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ù.
+- Test cover factory, config validation, default per-cycle behavior, mock lifecycle.
 
-## 5. PHASE 3 — Tạo Recovery Plugin (implement interface)
+Verify commands:
 
-**Mục tiêu:** viết các hành vi cụ thể **implement** `recovery_core::RecoveryBehavior`, mỗi
-họ ít nhất một ví dụ, + cơ chế nạp theo string.
+```bash
+xmllint --noout src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml
+python3 -c "from catkin_pkg.package import parse_package; parse_package('src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml')"
+catkin_make --pkg recovery_core
+./devel/lib/recovery_core/recovery_core_interface_test
+cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase2_build
+make -C /tmp/recovery_core_phase2_build -j4
+/tmp/recovery_core_phase2_build/test/recovery_core_interface_test
+```
 
-### 5.1. Plugin mẫu cho từng họ
+### Phase 3 - Plugin Implementations
 
-- **Họ C — `SpinRecovery` / `BackUpRecovery`** (per-cycle, ROS-free):
-  - Override `computeCommand`: sinh `Twist` theo pose + dt (xoay tới góc / lùi tới khoảng
-    cách), đạt đích → `kSucceeded`. Đọc tham số từ `RecoveryConfig::params`
-    (`spin_target_angle`, `spin_speed`, `backup_distance`, `backup_speed`...).
-  - Dùng default `runBehavior` (không cần override).
-- **Họ B — `ClearCostmapRecovery`-like** (one-shot, cần context):
-  - Override `runBehavior(ctx)`: gọi hành động clear qua một context mở rộng
-    (`RecoveryContext` thêm hook clear — hoặc để ở adapter). Trả `kSucceeded`/`kFailed`,
-    output `kNone`. `computeCommand` chỉ trả trạng thái.
-  - Lưu ý: clear costmap thật cần costmap → phần đó nằm ở **adapter/caller**, không trong
-    `recovery_core`. Plugin ở đây minh hoạ contract, thao tác thật uỷ thác qua context.
-- **Họ A — `RegenPathRecovery`** (trả path):
-  - Override để trả `RecoveryResult` với `output_type == kPath`, điền `robot_nav_msgs::Path`
-    (một đoạn đường thoát hình học đơn giản, ví dụ cung lùi). one-shot hoặc per-cycle.
+Trạng thái: **done**.
 
-> Các plugin này có thể đặt trong `recovery_core/plugins/` hoặc package riêng — **cần chốt**
-> (mục 6). Dù đặt đâu, chúng chỉ được include header của `recovery_core`, không thêm dependency
-> ROS vào package interface.
+Mục tiêu:
+- Viết plugin recovery thật implement `recovery_core::RecoveryBehavior`.
+- Export bằng Boost.DLL đúng convention workspace.
+- Test nạp plugin end-to-end.
 
-### 5.2. Cơ chế nạp — **Boost.DLL** (chốt, theo đúng convention workspace)
+Plugin đề xuất:
+1. `ClearCostmapRecovery`
+   - [x] nhóm B, one-shot, no output;
+   - [x] tham khảo logic `robot_clear_costmap_recovery`;
+   - [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] 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ó `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;
+   - [x] trả `robot_nav_msgs::Path`;
+   - [x] caller/adapter quyết định thay plan hay request replan.
 
-Workspace nạp planner/recovery plugin bằng **Boost.DLL import_alias**, KHÔNG dùng pluginlib/
-class_loader. Bằng chứng trong repo:
-- Plugin export: `robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp:37,330-337`.
-- Loader import: `move_base/src/move_base.cpp:22, 2016-2026` (`loadRecoveryBehaviors`).
-
-`recovery_core` (interface) **không** include Boost.DLL. Chỉ **plugin** export symbol và
-**loader/adapter** import. Interface chỉ cần `Ptr = std::shared_ptr<RecoveryBehavior>` để khớp
-chữ ký `import_alias<recovery_core::RecoveryBehavior::Ptr()>`.
-
-#### 5.2.1. Phía PLUGIN — export factory bằng `BOOST_DLL_ALIAS`
-
-Mỗi plugin cung cấp một **factory không tham số** trả `Ptr` (shared_ptr), rồi alias ra tên
-symbol dùng làm `type` trong YAML:
+Boost.DLL convention:
 
 ```cpp
-// spin_recovery.cpp
-#include <recovery_core/recovery_behavior.h>
-#include <boost/dll/alias.hpp>
-
-namespace recovery_plugins {
-
-class SpinRecovery : public recovery_core::RecoveryBehavior { /* override ... */
- public:
-  static recovery_core::RecoveryBehavior::Ptr create() {   // factory không tham số
-    return std::make_shared<SpinRecovery>();
+class RotateRecovery : public recovery_core::RecoveryBehavior
+{
+public:
+  static recovery_core::RecoveryBehavior::Ptr create()
+  {
+    return std::make_shared<RotateRecovery>();
   }
 };
 
-}  // namespace recovery_plugins
-
-// Tên alias thứ 2 ("spin_recovery") chính là `type` trong YAML recovery_behaviors.
-BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)
+BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, rotate_recovery)
 ```
 
-Quy ước (giống `ClearCostmapRecovery` → alias `ClearCostmapRecovery`):
-- Factory là `static Ptr create()` — **không tham số** (Boost.DLL alias yêu cầu signature
-  `Ptr()`), cấu hình đi qua `configure()` sau khi tạo, không qua constructor.
-- Trả `std::shared_ptr` (KHÔNG `unique_ptr`) để khớp loader.
-- Tên alias = tên `type` mà YAML/loader sẽ dùng để tìm library + symbol.
-
-#### 5.2.2. Phía LOADER — `import_alias` (mẫu, thường nằm ở adapter/caller)
+Loader side:
 
 ```cpp
-#include <boost/dll/import.hpp>
-// path .so lấy qua PluginLoaderHelper::findLibraryPath(type) như move_base đang làm.
 auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
-    path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations);
-recovery_core::RecoveryBehavior::Ptr behavior = loader();   // tạo instance
-std::string err;
-behavior->configure(config, &err);                          // rồi mới cấu hình
+    path_so, 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` **không** viết loader này (nó thuộc caller/move_base/adapter). Nhưng
-> `docs/PLUGIN_GUIDE.md` sẽ ghi mẫu để người dùng plugin biết cách nạp.
+Acceptance:
+- [x] Mỗi plugin build ra `.so` riêng.
+- [x] Mỗi plugin export factory không tham số, trả `RecoveryBehavior::Ptr`.
+- [x] Test nạp `.so` bằng `boost::dll::import_alias`.
+- [x] Test gọi `initialize`, `runBehavior` hoặc `computeCommand`.
+- [x] Output đúng nhóm recovery.
+- [x] Không plugin nào publish trực tiếp trong core logic.
 
-#### 5.2.3. CMake cho PLUGIN (không phải cho core)
+Verify commands:
 
-- `find_package(Boost REQUIRED COMPONENTS system filesystem)` (Boost.DLL cần `filesystem`;
-  `system` theo pattern package cũ). Link `${Boost_LIBRARIES}` và `${CMAKE_DL_LIBS}` (dl).
-- `set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON)` — bắt buộc cho .so.
-- Build mỗi plugin thành **shared library** riêng; tên library + symbol khớp `type` YAML.
-- Install `.so` vào nơi `findLibraryPath` tìm được (theo convention `PluginLoaderHelper`).
-- **Không** cần `plugins.xml` (workspace này dùng Boost.DLL thuần, không pluginlib).
+```bash
+catkin_make --pkg recovery_core
+./devel/lib/recovery_core/recovery_core_interface_test
+./devel/lib/recovery_core/recovery_core_plugin_loader_test
+cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase3_build
+make -C /tmp/recovery_core_phase3_build -j4
+/tmp/recovery_core_phase3_build/test/recovery_core_interface_test
+/tmp/recovery_core_phase3_build/test/recovery_core_plugin_loader_test
+```
 
-### 5.3. Adapter ROS (tuỳ chọn, package RIÊNG)
+### Phase 4 - Adapter / Integration
 
-Nếu cần chạy trong move_base thật: package adapter bọc plugin `recovery_core`, implement
-`robot_nav_core::RecoveryBehavior` (có costmap/tf), cấp `RecoveryContext` đọc TF/costmap,
-tiêu thụ Twist/Path. Giữ `recovery_core` sạch, không đổi.
+Trạng thái: **pending**.
 
-### 5.4. Checklist Phase 3
+Mục tiêu:
+- Tích hợp `recovery_core` vào caller thật mà không làm core phụ thuộc ROS publish/service.
 
-- [ ] Chốt: plugin đặt trong `recovery_core/plugins/` hay package riêng (đề xuất: package riêng
-      cho plugin cần costmap/tf; plugin thuần spin/backup có thể ở `recovery_core/plugins/`).
-- [ ] Mỗi họ A/B/C có ≥ 1 plugin mẫu implement interface, build ra **shared library** riêng.
-- [ ] Mỗi plugin có `static Ptr create()` + `BOOST_DLL_ALIAS(...::create, <type>)`; tên alias =
-      `type` YAML; trả `std::shared_ptr`.
-- [ ] CMake plugin: `find_package(Boost COMPONENTS system filesystem)`, link `${CMAKE_DL_LIBS}`,
-      `POSITION_INDEPENDENT_CODE ON`, install `.so` đúng nơi loader tìm.
-- [ ] Test end-to-end (không ROS): `import_alias<recovery_core::RecoveryBehavior::Ptr()>` nạp
-      `.so` mẫu → `configure` → `runBehavior`/`computeCommand` với MockContext → kiểm output
-      đúng họ (Twist / none / Path).
-- [ ] Cập nhật `docs/ARCHITECTURE.md`, `docs/PLUGIN_GUIDE.md` (kèm mẫu export + import).
-- [ ] **Kiểm chứng:** build plugin `.so` + test nạp Boost.DLL pass.
+Phương án:
+- Viết adapter riêng nếu cần tương thích `robot_nav_core::RecoveryBehavior`.
+- Adapter chịu trách nhiệm:
+  - load plugin;
+  - gọi `initialize`;
+  - gọi `runBehavior` hoặc loop `computeCommand`;
+  - 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.
 
----
+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.
 
-## 6. Rủi Ro & Điểm Cần Xác Nhận
+## 8. Safety Requirements
 
-- **Kiểu output hợp nhất vs đa interface:** đề xuất một `RecoveryResult` mang cờ `output_type`
-  (đơn giản, 1 base class). Phương án khác: template/đa base theo họ — phức tạp hơn. **Cần chốt.**
-- **`RecoveryContext` rộng tới đâu:** tối thiểu chỉ `getRobotPose`. Họ B (clear costmap) và họ
-  A (regen path cần costmap) sẽ cần hook thêm — nên để ở **context mở rộng của adapter**, giữ
-  context lõi nhỏ. **Cần chốt mức tối thiểu.**
-- **Vị trí plugin:** trong `recovery_core/plugins/` hay package riêng. Đề xuất: package riêng
-  cho plugin cần costmap/tf; plugin thuần (spin/backup) có thể ở `recovery_core/plugins/`.
-- **Cơ chế nạp = Boost.DLL (đã chốt):** theo đúng convention workspace (`import_alias`, không
-  pluginlib). Ràng buộc kéo theo: `Ptr = std::shared_ptr`; factory `static Ptr create()` không
-  tham số + `BOOST_DLL_ALIAS`; cấu hình qua `configure()` sau khi tạo. Core interface KHÔNG
-  phụ thuộc Boost.DLL — chỉ plugin và loader.
-- **`robot_nav_msgs` build dep:** xác nhận có CMake config/target để `find_package` (catkin)
-  hoặc include-only (standalone). Là header msgs nên nhiều khả năng include-only.
-- **An toàn:** core không collision-check → ghi rõ `docs/SAFETY.md`; caller/adapter chịu trách
-  nhiệm an toàn khi thực thi backup/spin.
+- 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.
+- 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õ:
+  - distance: meter;
+  - angle: radian;
+  - time: second;
+  - linear velocity: m/s;
+  - angular velocity: rad/s.
 
----
+## 9. Definition Of Done
 
-## 7. Tóm Tắt 3 Phase
+### Package DoD
 
-| Phase | Kết quả | Kiểm chứng |
-|-------|---------|-----------|
-| 1 | Khung package interface: 4 header + stub, package.xml, CMake build lib rỗng | `cmake` + `make` OK |
-| 2 | Hoàn thiện phần chung của interface: types/config/validate + default `runBehavior`, contract test qua Mock | build + test pass |
-| 3 | Plugin mẫu cho 3 họ (Twist/none/Path), export `BOOST_DLL_ALIAS`, nạp qua `import_alias`, doc | build `.so` + test nạp Boost.DLL pass |
+- `package.xml` hợp lệ.
+- Catkin build pass.
+- Standalone CMake build pass.
+- Header install/export đúng.
+- Test binary chạy được.
+- README/docs mô tả đúng scope.
+
+### Interface DoD
+
+- Contract status/output rõ ràng.
+- Factory result đúng bất biến.
+- Config validate đầy đủ.
+- Default behavior fail an toàn.
+- Test cover lifecycle và output invariant.
+
+### Plugin DoD
+
+- Plugin không publish trực tiếp.
+- Plugin không sở hữu raw pointer tf/costmap/global_path.
+- Plugin guard initialized/null/invalid input.
+- Plugin export Boost.DLL alias đúng.
+- Loader test nạp được `.so`.
+
+### Integration DoD
+
+- Adapter là nơi duy nhất có side effect publish/service/path replacement.
+- Safety stop rõ ràng khi failed/timeout.
+- Có log đủ ngữ cảnh, không spam loop.
+
+## 10. Verification Baseline
+
+Phase 1 đã được kiểm chứng bằng các lệnh:
+
+```bash
+xmllint --noout src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml
+python3 -c "from catkin_pkg.package import parse_package; p=parse_package('src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml'); print(p.name, p.version)"
+catkin_make --pkg recovery_core
+./devel/lib/recovery_core/recovery_core_interface_test
+cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase1_build
+make -C /tmp/recovery_core_phase1_build -j4
+/tmp/recovery_core_phase1_build/test/recovery_core_interface_test
+```
+
+Kỳ vọng chính:
+- `catkin_make --pkg recovery_core` tạo `devel/lib/librecovery_core.so`.
+- Standalone `make` tạo `/tmp/recovery_core_phase1_build/librecovery_core.a`.
+- Contract test in `interface contract test OK`.
+
+## 11. Open Decisions
+
+- Plugin mẫu hiện nằm dưới `recovery_core/plugins/`; package riêng chỉ cần nếu muốn tách deploy.
+- Có cần adapter tương thích `robot_nav_core::RecoveryBehavior` cho `move_base` hiện tại không.
+- Có cần helper loop trong base class cho behavior velocity hay để caller tự quản loop.
+- `global_path` nên là mutable pointer như hiện tại hay chuyển sang `const std::vector<...>*`
+  nếu behavior không được phép sửa plan trực tiếp.