# PLAN - `recovery_core`

`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

- 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.

## 2. Ranh Giới Thiết Kế

### 2.1. `recovery_core` chịu trách nhiệm

- Định nghĩa interface `RecoveryBehavior`.
- Định nghĩa result contract:
  - `RecoveryStatus`
  - `RecoveryOutputType`
  - `RecoveryResult`
- Cung cấp ngữ cảnh + mục tiêu runtime:
  - `RecoveryContext` (tf/costmap/global_path)
  - `RecoveryGoal` (angle/distance/target_pose/params)
- 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

- 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.

## 3. Interface Contract Đã Chốt

### 3.1. Recovery status

```cpp
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.

### 3.2. Output type

```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;
  robot_nav_msgs::Path path;

  static RecoveryResult Running();
  static RecoveryResult Succeeded();
  static RecoveryResult Failed();
  static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
                                 RecoveryStatus status);
  static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
                                RecoveryStatus status);
};
```

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`.

### 3.4. Recovery behavior

```cpp
class RecoveryBehavior
{
public:
  using Ptr = std::shared_ptr<RecoveryBehavior>;

  virtual ~RecoveryBehavior() = default;

  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;

  virtual RecoveryResult runBehavior() = 0;
  virtual RecoveryResult update();
  virtual RecoveryStatus status() const = 0;

protected:
  RecoveryBehavior() = default;
};
```

Đ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ó `update()` cho behavior per-cycle.

## 4. Ba Nhóm Recovery

| 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 | `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;
- 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

```text
recovery_core/
├── CMakeLists.txt
├── package.xml
├── README.md
├── PLAN.md
├── include/recovery_core/
│   ├── recovery_behavior.h
│   └── recovery_types.h
├── src/
│   ├── recovery_behavior.cpp
│   └── recovery_types.cpp
├── plugins/
│   ├── clear_costmap_recovery.cpp
│   ├── rotate_recovery.cpp
│   ├── back_up_recovery.cpp
│   └── regen_path_recovery.cpp
├── test/
│   ├── CMakeLists.txt
│   └── plugin_loader_contract_test.cpp
└── docs/
    ├── ARCHITECTURE.md
    ├── PLUGIN_GUIDE.md
    └── SAFETY.md
```

## 6. Dependencies

Runtime/build dependencies:
- `robot_costmap_2d`
- `robot_cpp`
- `robot_time`
- `robot_geometry_msgs`
- `robot_nav_msgs`
- `tf3`
- `Boost system thread`

Không phụ thuộc:
- `robot_nav_core`
- `roscpp`
- `pluginlib`

## 7. Roadmap

### Phase 1 - Package Skeleton

Trạng thái: **done**.

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`.

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.

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`.

### Phase 2 - Core Contract Implementation

Trạng thái: **done**.

Mục tiêu:
- Hoàn thiện phần logic chung của interface, chưa viết recovery cụ thể.

Work items:
1. [x] Implement `RecoveryResult` factories.
2. [x] Implement `RecoveryConfig::validate`.
3. [x] Implement `RecoveryConfig::fromNodeHandle`.
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 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.
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/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:

```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
```

### Phase 3 - Plugin Implementations

Trạng thái: **done**.

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.

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`, `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`, `control_period`;
   - [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.

Boost.DLL convention:

```cpp
class RotateRecovery : public recovery_core::RecoveryBehavior
{
public:
  static recovery_core::RecoveryBehavior::Ptr create()
  {
    return std::make_shared<RotateRecovery>();
  }
};

BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, rotate_recovery)
```

Loader side:

```cpp
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
    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);
```

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.

Verify commands:

```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
```

### Phase 4 - Adapter / Integration

Trạng thái: **pending**.

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.

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 `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 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.
- 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.
- 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õ:
  - distance: meter;
  - angle: radian;
  - time: second;
  - linear velocity: m/s;
  - angular velocity: rad/s.

## 9. Definition Of Done

### Package DoD

- `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 hoặc adapter hủy recovery.
- 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.