# Viết một recovery behavior mới

## 1. Chọn họ output

Quyết định đầu tiên và không đổi được về sau: `outputKind()`.

| Họ | Khi nào | Cổng bắt buộc trong context |
|---|---|---|
| `kNone` | Behavior không lái robot (đợi, xoá costmap, gọi thiết bị ngoài) | — |
| `kVelocity` | Behavior tự lái từng cycle | `PoseProvider` + `CollisionChecker` |
| `kPath` | Behavior sinh ra đường đi mới | `PlanProvider` |

Base kiểm cổng theo họ ngay ở `configure()`, nên một behavior họ velocity thiếu collision checker sẽ
**không nạp được**, thay vì phát hiện lúc đang lái.

## 2. Khung plugin

```cpp
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>

#include <boost/dll/alias.hpp>
#include <robot/robot.h>

namespace recovery_plugins
{
namespace
{
constexpr double kDefaultLimit = 1.0;   // [m] đơn vị ghi ngay tại chỗ khai báo
}

class MyRecovery final : public recovery_core::RecoveryBehavior
{
public:
  MyRecovery() = default;

  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
  {
    return std::make_shared<MyRecovery>();
  }

  recovery_core::RecoveryOutputType outputKind() const override
  {
    return recovery_core::RecoveryOutputType::kVelocity;
  }

protected:
  // nh ĐÃ được caller scope vào namespace param của instance này — đọc khoá phẳng.
  bool onConfigure(robot::NodeHandle& nh) override
  {
    nh.param("limit", limit_, kDefaultLimit);
    if (!std::isfinite(limit_) || limit_ <= 0.0)
    {
      robot::log_warning("[recovery_core] '%s': limit=%.3f không hợp lệ; dùng %.3f.",
                         name().c_str(), limit_, kDefaultLimit);
      limit_ = kDefaultLimit;
    }
    return true;   // false = không chạy được; registry bỏ behavior này và log đích danh
  }

  // Chốt mục tiêu lượt này + kiểm điều kiện an toàn để khởi động. KHÔNG sinh tick ở đây.
  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
    if (!ctx().pose->getRobotPose(start_pose_))
      return false;                        // không biết robot ở đâu -> từ chối khởi động

    target_ = goal.distance.value_or(limit_);
    return true;
  }

  // dt là thời gian THẬT tính từ tick trước; tick đầu ngay sau start() có dt = 0.
  recovery_core::RecoveryResult onUpdate(const robot::Time& now, double dt) override
  {
    robot_geometry_msgs::PoseStamped pose;
    if (!ctx().pose->getRobotPose(pose))
      return stopResult(recovery_core::RecoveryStatus::kFailed).withMessage("mất pose robot");

    const double done = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
    if (done >= target_)
      return stopResult(recovery_core::RecoveryStatus::kSucceeded).withProgress(1.0, 0.0);

    robot_geometry_msgs::Twist cmd;
    // ... tính cmd, nhớ ramp theo acc_lim và kiểm collision ở pose dự đoán ...
    return recovery_core::RecoveryResult::Velocity(cmd, recovery_core::RecoveryStatus::kRunning)
        .withProgress(done / target_, target_ - done);
  }

  // Tuỳ chọn: cơ hội giảm tốc thay vì nhảy thẳng về 0.
  recovery_core::RecoveryResult onCancel() override
  {
    return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("cancelled");
  }

private:
  double limit_ = kDefaultLimit;                    ///< [m]
  double target_ = kDefaultLimit;                   ///< [m]
  double start_yaw_ = 0.0;                          ///< [rad]
  robot_geometry_msgs::PoseStamped start_pose_;
};

}  // namespace recovery_plugins

BOOST_DLL_ALIAS(recovery_plugins::MyRecovery::create, MyRecovery)
```

Những gì **không** phải viết: guard `configured_`/`started_`, kiểm cancel, đo `elapsed`, ép
`timeout`, đặt `output_type` cho stop output, kiểm NaN của lệnh vận tốc. Base làm hết — và giữ được
vì toàn bộ state của nó là `private`.

## 3. Đăng ký build

```cmake
add_recovery_core_plugin(
  recovery_core_my_recovery
  plugins/my_recovery.cpp
)
```

Thêm tên target vào `catkin_package(LIBRARIES ...)` để consumer khác dùng lại được.

## 4. Khai trong YAML

```yaml
recovery:
  behaviors:
    - {name: my_instance, type: MyRecovery}   # thứ tự trong danh sách CHÍNH LÀ thứ tự thử
  my_instance:
    limit: 0.5                                # [m]
    timeout: 10.0                             # [s] base đọc; 0 = không giới hạn

MyRecovery:
  library_path: librecovery_core_my_recovery  # THIẾU KHOÁ NÀY LÀ LỖI PHỔ BIẾN NHẤT
```

Tên alias trong `BOOST_DLL_ALIAS` phải khớp `type`. Namespace param của instance là `<ns>/<name>` —
registry dựng `NodeHandle` đó và truyền vào `configure()`; plugin **không** tự đi tìm config trên
disk. Đó là lý do test chỉ cần trỏ vào cây config của mình là chạy được.

Một plugin có thể có **nhiều instance** với tham số khác nhau — bộ mặc định dùng
`ClearCostmapRecovery` hai lần (`conservative_reset` và `aggressive_reset`).

## 5. Test

Đặt trong `test/`, thêm tên vào danh sách `RECOVERY_CORE_TESTS` của `CMakeLists.txt` (dùng
`catkin_add_gtest`, nên `ctest -R recovery_core` bắt được).

Dùng `recovery_test::VelocityRig` (costmap giả + pose giả + collision checker giả + đồng hồ giả) và
nạp plugin **qua `RecoveryRegistry`** để đi đúng đường Boost.DLL mà runtime dùng — không link thẳng
`.so` vào test.

Tối thiểu phải phủ:

- từ chối khởi động khi điều kiện an toàn không thoả;
- mất pose giữa chừng → `kFailed` + lệnh dừng;
- loop chạy chậm (dt gấp 5–20 lần nhịp thường) vẫn dừng đúng chỗ;
- cancel → lệnh dừng;
- param ngoài dải → dùng default, không nhận giá trị sai.

Cuối cùng, kiểm rằng test **fail được**: gỡ `.so` khỏi `devel/lib` rồi chạy lại. Phải đỏ. Bộ test cũ
của gói này in `[PASS]` khi không nạp được plugin nào — đó là thứ phải tránh.