Đị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

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.

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

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.