# recovery_core

Interface tick-based cho **recovery behavior** của navigation stack, kèm bộ behavior mặc định.

## Phạm vi

- Định nghĩa contract `recovery_core::RecoveryBehavior` — vòng đời hướng-goal, mỗi control cycle
  một lời gọi, behavior có thể **phát vận tốc**.
- Cung cấp `RecoveryRegistry` nạp behavior theo YAML qua **Boost.DLL** (`library_path`), giữ đúng
  vòng đời `.so`.
- Kèm **bộ recovery mặc định**: wait, clear costmap, rotate, back up.
- Không chạy roscpp/ROS master; dùng lớp ROS-like `robot_*` (`robot_costmap_2d`, `tf3`, `robot_cpp`,
  `robot_time`, `robot_geometry_msgs`, `robot_nav_msgs`).

## Vòng đời

```cpp
recovery_core::RecoveryContext ctx;
ctx.pose      = &pose_provider;       // bắt buộc cho họ velocity
ctx.collision = &collision_checker;   // bắt buộc cho họ velocity
ctx.local_costmap = local_costmap;    // non-owning, làm mới trước mỗi lượt

robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
registry.loadFromConfig(nh, "recovery", ctx);

auto* behavior = registry.at(0);
recovery_core::RecoveryGoal goal;
goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;

if (behavior->start(goal, clock.now()))          // trả bool; KHÔNG sinh tick
{
  for (;;)
  {
    const auto r = behavior->update(clock.now()); // dt do base đo THẬT
    if (const auto* cmd = r.velocity())           // nullptr nếu behavior không thuộc họ velocity
      publish(*cmd);
    if (r.terminal())
      break;
  }
}
```

API công khai `configure` / `start` / `update` / `cancel` là **non-virtual**: base giữ toàn bộ bất
biến, plugin chỉ triển khai hook `onConfigure` / `onStart` / `onUpdate` / `onCancel`.

## Ba họ output

Mỗi behavior khai **một lần** qua `outputKind()`; base cưỡng chế mọi kết quả trả về phải khớp, nên
caller route theo `output_type` được mà không cần tin lời hứa trong tài liệu.

| Họ | Behavior mặc định | Output | Cổng bắt buộc trong context |
|----|-------------------|--------|------------------------------|
| `kNone` | `WaitRecovery`, `ClearCostmapRecovery` | không | — (clear costmap cần con trỏ costmap) |
| `kVelocity` | `RotateRecovery`, `BackUpRecovery` | `robot_geometry_msgs::Twist` mỗi cycle | `PoseProvider` + `CollisionChecker` |
| `kPath` | (chưa có) | `robot_nav_msgs::Path` | `PlanProvider` |

Đọc dữ liệu qua `result.velocity()` / `result.pathOut()` — trả `nullptr` nếu họ không khớp, nên
không thể đọc nhầm trường của họ khác.

## Bộ recovery mặc định

Thứ tự trong `recovery_behaviors_params.yaml` **chính là hành vi**: caller thử từ đầu danh sách,
hỏng thì sang cái kế tiếp. Sắp từ nhẹ tới nặng.

| # | Instance | Plugin | Làm gì |
|---|----------|--------|--------|
| 0 | `wait` | `WaitRecovery` | Đứng yên `wait_duration` giây. An toàn nhất — không di chuyển, không cần pose. Với AMR trong kho, phần lớn tình huống chặn đường là vật cản động và cách này giải quyết được đa số |
| 1 | `conservative_reset` | `ClearCostmapRecovery` | Xoá vật cản đã tích trong vùng `reset_distance` quanh robot |
| 2 | `rotate` | `RotateRecovery` | Quay tại chỗ (mặc định đủ 2π) cho costmap quan sát lại xung quanh. Quét footprint qua **toàn bộ cung** trước khi quay |
| 3 | `aggressive_reset` | `ClearCostmapRecovery` | Ngược lại: giữ vùng gần, xoá tất cả phần còn lại |
| 4 | `back_up` | `BackUpRecovery` | Lùi `backup_distance` mét. Xếp **cuối** vì lùi là hướng robot thường không có sensor |

Không có behavior "chỉ xin lập plan lại": state machine của `move_base2` đã tự lập plan lại sau mọi
lượt recovery kết thúc, nên một behavior như vậy chỉ chiếm chỗ mà không làm gì.

## Bất biến an toàn

Base bảo đảm, không phụ thuộc plugin có nhớ hay không:

- **Tiến độ đo bằng pose thật.** `update(now)` cấp `dt` đo từ đồng hồ thật, không phải chu kỳ cấu
  hình. Plugin họ velocity đo quãng đi bằng hình chiếu delta pose, nên control loop chạy chậm không
  làm robot đi quá quãng, và bánh trượt không bị báo nhầm là hoàn thành.
- **Không có pose thì dừng.** `PoseProvider::getRobotPose()` trả `false` → `kFailed` + Twist 0.
- **Không lái mù.** Họ velocity bắt buộc có `CollisionChecker`; thiếu thì `configure()` trả `false`.
- **NaN/Inf không ra được cmd_vel.** Lệnh không hữu hạn bị đổi thành lệnh dừng + `kFailed`.
- **`elapsed` và `timeout`.** Base đo và ép; quá hạn là `kFailed` + stop output.
- **Stop output đúng họ.** Họ velocity nhận Twist 0 tường minh (caller đang lấy cmd_vel từ đó); họ
  khác nhận `kNone` — base không bịa output vận tốc cho behavior không lái.

Chi tiết trong [docs/SAFETY.md](docs/SAFETY.md).

## Cấu trúc

```
include/recovery_core/   recovery_context.h, recovery_types.h, recovery_behavior.h,
                         recovery_registry.h, recovery_math.h, adapters/
src/                     base lifecycle + types + registry
adapters/                CostmapPoseProvider, CostmapCollisionChecker (nối vào costmap thật)
plugins/                 wait, clear_costmap, rotate, back_up — bộ mặc định, mỗi cái một .so
test/                    9 bộ test GTest + cây config riêng
docs/                    ARCHITECTURE.md, SAFETY.md, PLUGIN_GUIDE.md
```

## Build và test

```bash
catkin_make --pkg recovery_core

# Đăng ký với ctest, nên chạy được cả hai đường:
cd build && ctest -R recovery_core --output-on-failure

# hoặc gọi trực tiếp
./devel/lib/recovery_core/backup_safety_test
```

Tham số vận hành: `pnkx_nav_core/config/recovery_behaviors_params.yaml`.
Bản dùng cho test: `test/config/recovery_behaviors_params.yaml` — **đừng** sửa tham số vận hành ở
đây.

Viết plugin mới: [docs/PLUGIN_GUIDE.md](docs/PLUGIN_GUIDE.md).