temporary storage

CMakeLists.txt

@@ -0,0 +1,586 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(recovery_core VERSION 0.1.0 LANGUAGES CXX)
+
+# ========================================================
+# Build mode detection
+# ========================================================
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building recovery_core with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building recovery_core with Standalone CMake")
+endif()
+
+
+# ========================================================
+# C++ Standard
+# ========================================================
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+
+# ========================================================
+# Common dependencies
+# ========================================================
+find_package(Boost REQUIRED COMPONENTS
+  system
+  thread
+  filesystem
+)
+
+find_package(Threads REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+
+# ========================================================
+# Standalone configuration
+# ========================================================
+if(NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # Runtime search path configuration
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
+
+
+  # ------------------------------------------------------
+  # Navigation source tree
+  # ------------------------------------------------------
+  set(PNKX_NAV_CORE_SRC_DIR
+    "${CMAKE_CURRENT_SOURCE_DIR}/../../.."
+  )
+
+  set(WORKSPACE_DEVEL_LIB_DIR
+    "${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../../devel/lib"
+  )
+
+
+  # ------------------------------------------------------
+  # Collect standalone package include directories
+  # ------------------------------------------------------
+  file(GLOB STANDALONE_PACKAGE_INCLUDE_DIRS
+    LIST_DIRECTORIES true
+
+    ${PNKX_NAV_CORE_SRC_DIR}/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/include
+    ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/*/include
+  )
+
+
+  # robot_costmap_2d headers may expose PCL dependencies
+  find_package(PCL QUIET COMPONENTS
+    common
+    io
+  )
+
+
+  set(STANDALONE_INCLUDE_DIRS
+    ${STANDALONE_PACKAGE_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+
+    /opt/ros/noetic/include
+    /usr/local/include
+  )
+
+
+  if(PCL_FOUND)
+    add_definitions(${PCL_DEFINITIONS})
+  endif()
+
+
+  # ------------------------------------------------------
+  # Standalone libraries
+  # ------------------------------------------------------
+  set(PACKAGES_DIR
+    robot_costmap_2d
+    robot_cpp
+    robot_time
+    robot_xmlrpcpp
+  )
+
+
+  # ------------------------------------------------------
+  # TF3
+  # ------------------------------------------------------
+  find_library(TF3_LIBRARY
+    NAMES tf3
+
+    PATHS
+      /usr/lib
+      /usr/local/lib
+      /usr/lib/x86_64-linux-gnu
+  )
+
+
+  # ------------------------------------------------------
+  # Library search paths
+  # ------------------------------------------------------
+  if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
+    link_directories(${WORKSPACE_DEVEL_LIB_DIR})
+  endif()
+
+  link_directories(
+    /usr/local/lib
+  )
+
+
+# ========================================================
+# Catkin configuration
+# ========================================================
+else()
+
+  find_package(catkin REQUIRED COMPONENTS
+    robot_costmap_2d
+    robot_cpp
+    robot_time
+    robot_geometry_msgs
+    robot_nav_msgs
+    robot_xmlrpcpp
+  )
+
+
+  find_library(TF3_LIBRARY
+    NAMES tf3
+
+    PATHS
+      /usr/lib
+      /usr/local/lib
+      /usr/lib/x86_64-linux-gnu
+  )
+
+
+  catkin_package(
+    INCLUDE_DIRS
+      include
+
+    LIBRARIES
+      recovery_core
+      recovery_core_clear_costmap_recovery
+      recovery_core_rotate_recovery
+      recovery_core_back_up_recovery
+      recovery_core_regen_path_recovery
+
+    CATKIN_DEPENDS
+      robot_costmap_2d
+      robot_cpp
+      robot_time
+      robot_geometry_msgs
+      robot_nav_msgs
+      robot_xmlrpcpp
+
+    DEPENDS
+      Boost
+  )
+
+
+  include_directories(
+    include
+
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
+  )
+
+endif()
+
+
+# ========================================================
+# Core library
+# ========================================================
+add_library(recovery_core SHARED
+  src/recovery_types.cpp
+  src/recovery_config.cpp
+  src/recovery_behavior.cpp
+)
+
+
+# ========================================================
+# Core library - Catkin
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+
+  add_dependencies(
+    recovery_core
+
+    ${${PROJECT_NAME}_EXPORTED_TARGETS}
+    ${catkin_EXPORTED_TARGETS}
+  )
+
+
+  target_include_directories(recovery_core
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+
+      ${Boost_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
+  )
+
+
+  target_link_libraries(recovery_core
+    PUBLIC
+      ${catkin_LIBRARIES}
+
+    PRIVATE
+      Boost::boost
+      Boost::system
+      Boost::thread
+      Boost::filesystem
+
+      yaml-cpp
+
+      Threads::Threads
+
+      ${CMAKE_DL_LIBS}
+
+      ${TF3_LIBRARY}
+  )
+
+
+# ========================================================
+# Core library - Standalone
+# ========================================================
+else()
+
+  target_include_directories(recovery_core
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+
+    PRIVATE
+      ${STANDALONE_INCLUDE_DIRS}
+      ${Boost_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
+  )
+
+
+  target_link_libraries(recovery_core
+    PUBLIC
+      ${PACKAGES_DIR}
+
+    PRIVATE
+      Boost::boost
+      Boost::system
+      Boost::thread
+      Boost::filesystem
+
+      yaml-cpp
+
+      Threads::Threads
+
+      ${CMAKE_DL_LIBS}
+
+      ${TF3_LIBRARY}
+  )
+
+
+  set_target_properties(recovery_core PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY
+      ${CMAKE_BINARY_DIR}
+
+    BUILD_RPATH
+      "${CMAKE_BINARY_DIR}"
+
+    INSTALL_RPATH
+      "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+
+# ========================================================
+# Recovery plugin helper
+# ========================================================
+set(RECOVERY_CORE_PLUGIN_TARGETS)
+
+
+function(add_recovery_core_plugin target source)
+
+  add_library(${target} SHARED
+    ${source}
+  )
+
+
+  add_dependencies(
+    ${target}
+    recovery_core
+  )
+
+
+  # ======================================================
+  # Catkin plugin
+  # ======================================================
+  if(BUILDING_WITH_CATKIN)
+
+    add_dependencies(
+      ${target}
+
+      ${${PROJECT_NAME}_EXPORTED_TARGETS}
+      ${catkin_EXPORTED_TARGETS}
+    )
+
+
+    target_include_directories(${target}
+      PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+    )
+
+
+    target_link_libraries(${target}
+      PUBLIC
+        recovery_core
+
+      PRIVATE
+        ${catkin_LIBRARIES}
+
+        Boost::boost
+        Boost::system
+        Boost::thread
+        Boost::filesystem
+
+        yaml-cpp
+
+        Threads::Threads
+
+        ${CMAKE_DL_LIBS}
+
+        ${TF3_LIBRARY}
+    )
+
+
+  # ======================================================
+  # Standalone plugin
+  # ======================================================
+  else()
+
+    target_include_directories(${target}
+      PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+
+      PRIVATE
+        ${STANDALONE_INCLUDE_DIRS}
+        ${TF3_INCLUDE_DIR}
+    )
+
+
+    target_link_libraries(${target}
+      PUBLIC
+        recovery_core
+
+      PRIVATE
+        Boost::boost
+        Boost::system
+        Boost::thread
+        Boost::filesystem
+
+        yaml-cpp
+
+        Threads::Threads
+
+        ${CMAKE_DL_LIBS}
+
+        ${TF3_LIBRARY}
+    )
+
+
+    set_target_properties(${target} PROPERTIES
+      LIBRARY_OUTPUT_DIRECTORY
+        ${CMAKE_BINARY_DIR}
+
+      BUILD_RPATH
+        "${CMAKE_BINARY_DIR}"
+
+      INSTALL_RPATH
+        "${CMAKE_INSTALL_PREFIX}/lib"
+    )
+
+  endif()
+
+
+  set_target_properties(${target} PROPERTIES
+    POSITION_INDEPENDENT_CODE ON
+  )
+
+
+  list(APPEND
+    RECOVERY_CORE_PLUGIN_TARGETS
+    ${target}
+  )
+
+
+  set(
+    RECOVERY_CORE_PLUGIN_TARGETS
+    ${RECOVERY_CORE_PLUGIN_TARGETS}
+    PARENT_SCOPE
+  )
+
+endfunction()
+
+
+# ========================================================
+# Recovery plugins
+# ========================================================
+add_recovery_core_plugin(
+  recovery_core_clear_costmap_recovery
+  plugins/clear_costmap_recovery.cpp
+)
+
+add_recovery_core_plugin(
+  recovery_core_rotate_recovery
+  plugins/rotate_recovery.cpp
+)
+
+add_recovery_core_plugin(
+  recovery_core_back_up_recovery
+  plugins/back_up_recovery.cpp
+)
+
+add_recovery_core_plugin(
+  recovery_core_regen_path_recovery
+  plugins/regen_path_recovery.cpp
+)
+
+
+# ========================================================
+# Install - Catkin
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+
+  install(
+    TARGETS
+      recovery_core
+      ${RECOVERY_CORE_PLUGIN_TARGETS}
+
+    ARCHIVE DESTINATION
+      ${CATKIN_PACKAGE_LIB_DESTINATION}
+
+    LIBRARY DESTINATION
+      ${CATKIN_PACKAGE_LIB_DESTINATION}
+
+    RUNTIME DESTINATION
+      ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+
+  install(
+    DIRECTORY
+      include/${PROJECT_NAME}/
+
+    DESTINATION
+      ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+
+    FILES_MATCHING
+      PATTERN "*.h"
+      PATTERN "*.hpp"
+
+    PATTERN ".svn" EXCLUDE
+  )
+
+
+# ========================================================
+# Install - Standalone
+# ========================================================
+else()
+
+  install(
+    TARGETS
+      recovery_core
+      ${RECOVERY_CORE_PLUGIN_TARGETS}
+
+    EXPORT
+      ${PROJECT_NAME}-targets
+
+    ARCHIVE DESTINATION
+      lib
+
+    LIBRARY DESTINATION
+      lib
+
+    RUNTIME DESTINATION
+      bin
+  )
+
+
+  install(
+    EXPORT
+      ${PROJECT_NAME}-targets
+
+    FILE
+      ${PROJECT_NAME}-targets.cmake
+
+    NAMESPACE
+      ${PROJECT_NAME}::
+
+    DESTINATION
+      lib/cmake/${PROJECT_NAME}
+  )
+
+
+  install(
+    DIRECTORY
+      include/${PROJECT_NAME}/
+
+    DESTINATION
+      include
+
+    FILES_MATCHING
+      PATTERN "*.h"
+      PATTERN "*.hpp"
+
+    PATTERN ".svn" EXCLUDE
+  )
+
+
+  # ------------------------------------------------------
+  # Print configuration information
+  # ------------------------------------------------------
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Libraries:")
+  message(STATUS "  recovery_core")
+  message(STATUS "  recovery_core_clear_costmap_recovery")
+  message(STATUS "  recovery_core_rotate_recovery")
+  message(STATUS "  recovery_core_back_up_recovery")
+  message(STATUS "  recovery_core_regen_path_recovery")
+  message(STATUS "Dependencies:")
+  message(STATUS "  robot_costmap_2d")
+  message(STATUS "  robot_cpp")
+  message(STATUS "  robot_time")
+  message(STATUS "  robot_geometry_msgs")
+  message(STATUS "  robot_nav_msgs")
+  message(STATUS "  robot_xmlrpcpp")
+  message(STATUS "  tf3")
+  message(STATUS "  Boost")
+  message(STATUS "  yaml-cpp")
+  message(STATUS "=================================")
+
+endif()
+
+
+# ========================================================
+# Tests
+# ========================================================
+option(
+  BUILD_RECOVERY_CORE_TESTS
+  "Build recovery_core tests"
+  ON
+)
+
+
+if(BUILD_RECOVERY_CORE_TESTS)
+
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/CMakeLists.txt)
+    add_subdirectory(test)
+  endif()
+
+endif()

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 {
+class RecoveryBehavior
+{
 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).
   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:
   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 ... */
+class RotateRecovery : public recovery_core::RecoveryBehavior
+{
 public:
-  static recovery_core::RecoveryBehavior::Ptr create() {   // factory không tham số
-    return std::make_shared<SpinRecovery>();
+  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.

README.md

@@ -0,0 +1,55 @@
+# recovery_core
+
+Interface (base class) cho các hành vi **recovery** của navigation stack ROS-like T800.
+
+## Phạm vi
+
+- Là **package định nghĩa interface**, mô phỏng `robot_nav_core::RecoveryBehavior` nhưng mở
+  rộng thêm `global_path`: `initialize(name, tf, global_path, global_costmap, local_costmap)`;
+  đồng thời **tổng quát hoá output** để bao 3 họ recovery.
+- **Không chạy roscpp/ROS master thật**; dùng bình thường lớp ROS-like `robot_*`
+  (`robot_costmap_2d`, `tf3`, `robot_cpp`, `robot_time`, `robot_geometry_msgs`,
+  `robot_nav_msgs`).
+- Core interface không publish trực tiếp. Package hiện có thêm các plugin mẫu build thành
+  `.so` riêng và nạp qua **Boost.DLL** để kiểm chứng contract end-to-end.
+
+## Ba họ recovery
+
+| Họ | Ví dụ | Output | Method chính |
+|----|-------|--------|--------------|
+| A | regen path (đường thoát) | `robot_nav_msgs::Path` | `runBehavior()` |
+| B | clear costmap | không có (chỉ status) | `runBehavior()` |
+| C | rotation / backup | `robot_geometry_msgs::Twist` mỗi cycle | `computeCommand(dt)` |
+
+Cả 3 chia sẻ một `RecoveryResult` hợp nhất mang cờ `output_type`.
+
+## Cấu trúc
+
+```
+include/recovery_core/   recovery_types.h, recovery_config.h, recovery_behavior.h
+src/                     phần chung của contract (types/config/default behavior)
+plugins/                 clear_costmap, rotate, backup, regen_path plugin mẫu
+test/                    contract test + Boost.DLL loader test
+examples/                minimal_recovery.cpp
+docs/                    ARCHITECTURE.md, PLUGIN_GUIDE.md, SAFETY.md
+```
+
+## Build
+
+Hỗ trợ **catkin** và **standalone CMake** (như `robot_clear_costmap_recovery`).
+
+```bash
+# catkin (trong workspace)
+catkin_make --pkg recovery_core
+
+# standalone
+mkdir build && cd build && cmake .. && make
+```
+
+## Trạng thái
+
+- [x] Phase 1 — khung package (interface + stub, build lib rỗng).
+- [x] Phase 2 — triển khai phần chung (types/config/validate + default computeCommand).
+- [x] Phase 3 — plugin mẫu cho 3 họ + export/import Boost.DLL.
+
+Xem [PLAN.md](PLAN.md) để biết chi tiết từng phase.

docs/ARCHITECTURE.md

@@ -0,0 +1,41 @@
+# Kiến Trúc recovery_core
+
+## Vị trí trong stack
+
+`recovery_core` nằm ở tầng `Navigations/Libraries`, cạnh `robot_clear_costmap_recovery`. Nó
+giữ vai trò interface tương tự `robot_nav_core::RecoveryBehavior` cho các recovery cần **trả
+về output** (path/vận tốc), đồng thời vẫn dùng global path/costmap/tf như bản gốc.
+
+## Các thành phần
+
+- `RecoveryBehavior` (interface): `initialize` (mirror nav_core) + `runBehavior` (one-shot) +
+  `computeCommand` (per-cycle) + `status`.
+- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ.
+- `RecoveryConfig`: param chung (control_frequency, timeout) + validate + đọc từ NodeHandle.
+- Plugin mẫu:
+  - `ClearCostmapRecovery`: clear layer costmap theo tên, trả no-output status.
+  - `RotateRecovery`: sinh `Twist.angular.z` theo chu kỳ tới khi đủ góc.
+  - `BackUpRecovery`: sinh `Twist.linear.x < 0` theo chu kỳ tới khi đủ khoảng lùi.
+  - `RegenPathRecovery`: trả lại `robot_nav_msgs::Path` từ `global_path` hiện tại.
+
+## Luồng runtime
+
+```
+initialize(name, tf, global_path, global, local)   // 1 lần, đọc param qua NodeHandle
+        │
+        ├── one-shot (họ A/B):  runBehavior() ──────────────► RecoveryResult{status, path|none}
+        │
+        └── per-cycle (họ C):   loop { computeCommand(dt) } ─► RecoveryResult{status, velocity}
+                                (caller publish command mỗi cycle tới khi status != kRunning)
+```
+
+## Ghi chú thiết kế
+
+- `computeCommand(dt)` lấy pose robot từ costmap/tf bên trong (nhất quán mirror nav_core),
+  không truyền pose qua tham số.
+- Default `RecoveryBehavior::computeCommand(dt)` trả `RecoveryResult::Failed()` để họ A/B không
+  vô tình sinh command mù.
+- `RecoveryConfig::validate()` từ chối `NaN/Inf`, `control_frequency <= 0`, `timeout < 0`.
+- `RecoveryConfig::fromNodeHandle()` đọc param chung và thay giá trị invalid bằng default an toàn.
+- Interface KHÔNG include Boost.DLL; export/import là việc của plugin/loader.
+- Plugin mẫu có dùng Boost.DLL alias, nhưng core contract vẫn không biết loader/adaptor.

docs/PLUGIN_GUIDE.md

@@ -0,0 +1,78 @@
+# Hướng Dẫn Viết Plugin recovery_core
+
+Package hiện có 4 plugin mẫu dưới `plugins/`:
+- `clear_costmap_recovery` — nhóm B, one-shot, no output.
+- `rotate_recovery` — nhóm C, per-cycle velocity.
+- `back_up_recovery` — nhóm C, per-cycle velocity.
+- `regen_path_recovery` — nhóm A, path output.
+
+## Bước chung
+
+1. Kế thừa `recovery_core::RecoveryBehavior`.
+2. Override `initialize()` — đọc param riêng qua `robot::NodeHandle("~/" + name)`, cache
+   tf/global_path/costmap.
+3. Override method theo họ (xem dưới) + `status()`.
+4. Thêm factory `static Ptr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`.
+
+## Override theo họ
+
+| Họ | Override | Trả về |
+|----|----------|--------|
+| A. path | `runBehavior()` | `RecoveryResult::PathOut(path, kSucceeded)` |
+| B. none | `runBehavior()` | `RecoveryResult::Succeeded()` / `Failed()` |
+| C. velocity | `computeCommand(dt)` | `RecoveryResult::Velocity(twist, kRunning|kSucceeded)` |
+
+## Export bằng Boost.DLL (bắt buộc cho plugin)
+
+```cpp
+#include <recovery_core/recovery_behavior.h>
+#include <boost/dll/alias.hpp>
+
+namespace recovery_plugins {
+class SpinRecovery : public recovery_core::RecoveryBehavior {
+ public:
+  static recovery_core::RecoveryBehavior::Ptr create() {
+    return std::make_shared<SpinRecovery>();
+  }
+  // override initialize()/computeCommand()/runBehavior()/status()...
+};
+}  // namespace recovery_plugins
+
+// alias = `type` dùng trong YAML recovery_behaviors.
+BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)
+```
+
+## Nạp phía loader (adapter/caller — không nằm trong recovery_core)
+
+```cpp
+#include <boost/dll/import.hpp>
+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();
+behavior->initialize(name, tf, global_path, global_costmap, local_costmap);
+```
+
+Lưu ý: adapter/test phải giữ handle `.so` sống lâu hơn object plugin. Nếu library bị unload trong
+khi object plugin còn tồn tại, virtual call qua vtable của plugin có thể crash.
+
+## CMake cho plugin
+
+- `find_package(Boost REQUIRED COMPONENTS system filesystem)`
+- link `${Boost_LIBRARIES}`, `${CMAKE_DL_LIBS}`, `recovery_core`
+- `set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON)`
+- build shared library, tên library + symbol khớp `type`; install `.so` nơi loader tìm.
+
+## Test plugin
+
+```bash
+catkin_make --pkg recovery_core
+./devel/lib/recovery_core/recovery_core_plugin_loader_test
+```
+
+Standalone:
+
+```bash
+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_plugin_loader_test
+```

docs/SAFETY.md

@@ -0,0 +1,30 @@
+# An Toàn — recovery_core
+
+> Cảnh báo an toàn khi dùng interface này. Đọc trước khi triển khai plugin họ vận tốc.
+
+## recovery_core KHÔNG đảm bảo
+
+- **Không collision-check tự động.** Interface không kiểm tra va chạm khi sinh vận tốc
+  (backup/spin) hay khi tạo path. Việc tránh va chạm là trách nhiệm của **plugin** (dùng
+  costmap được cấp qua `initialize`) hoặc của **caller**.
+- **Không quản lý vòng lặp thời gian thực.** Caller chịu trách nhiệm gọi `computeCommand(dt)`
+  đúng nhịp và publish command.
+- **Không đảm bảo frame/đơn vị.** Pose lấy từ costmap/tf phải đúng frame; đơn vị phải nhất quán
+  (m, rad, s, m/s, rad/s).
+- **Default per-cycle fail an toàn.** Behavior không override `computeCommand(dt)` sẽ nhận
+  `RecoveryResult::Failed()` thay vì velocity mặc định.
+
+## Nguyên tắc cho plugin
+
+- Guard `initialized_` và costmap/tf null trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`.
+- Guard `dt <= 0`, `NaN`, `Inf` trước khi tính velocity.
+- Với họ vận tốc: khi không chắc an toàn, trả **stop command** (Twist 0), không trả vận tốc mù.
+- Kiểm tra NaN/Inf của pose/vận tốc trước khi xuất command.
+- Tôn trọng giới hạn vận tốc/gia tốc của robot (đọc qua param).
+- `BackUpRecovery` có param `require_costmap` để bắt buộc có local costmap trước khi xuất
+  vận tốc lùi. Adapter production vẫn nên có safety gate/collision check riêng trước publish.
+
+## Trách nhiệm caller/adapter
+
+- Đảm bảo ngữ cảnh cho phép recovery (vd: đã dừng planner, vùng xung quanh đủ an toàn).
+- Áp timeout/giám sát ngoài để tránh recovery chạy vô hạn.

include/recovery_core/recovery_behavior.h

@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — interface (base class) cho recovery behaviors.
+ *
+ * Mô phỏng robot_nav_core::RecoveryBehavior (giữ chữ ký initialize với tf + costmap),
+ * nhưng tổng quát hoá output để bao 3 họ recovery (path / none / velocity).
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_BEHAVIOR_H_
+#define RECOVERY_CORE_RECOVERY_BEHAVIOR_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <tf3/buffer_core.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+
+#include <recovery_core/recovery_types.h>
+
+namespace recovery_core
+{
+
+/**
+ * @class RecoveryBehavior
+ * @brief Interface cho mọi hành vi recovery không chạy roscpp/ROS master thật
+ *        (dùng lớp ROS-like robot_*).
+ *
+ * Ba họ hành vi và method chính tương ứng:
+ *  - Họ A (trả path)      : override runBehavior()   -> RecoveryResult::PathOut(...)
+ *  - Họ B (không output)  : override runBehavior()   -> RecoveryResult::Succeeded()/Failed()
+ *  - Họ C (trả vận tốc)   : override computeCommand() -> RecoveryResult::Velocity(...)
+ *
+ * Vòng đời: initialize() một lần -> runBehavior() (one-shot) hoặc lặp computeCommand()
+ * (per-cycle) -> status(). Guard initialized_/costmap trước khi thao tác.
+ */
+class RecoveryBehavior
+{
+public:
+  /// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace
+  /// (boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(...)).
+  using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>;
+
+  virtual ~RecoveryBehavior() = default;
+
+  /**
+   * @brief Khởi tạo — mở rộng chữ ký robot_nav_core::RecoveryBehavior::initialize (thêm
+   *        global_path). Chỉ chạy một lần; đọc param qua robot::NodeHandle("~/" + name);
+   *        cache tf/costmap/global_path (không sở hữu).
+   * @param name           Tên instance (dùng cho namespace param + log).
+   * @param tf             Transform buffer (không sở hữu).
+   * @param global_path    Đường đi toàn cục hiện tại (không sở hữu) — họ A regen path tham
+   *                       chiếu để tạo lại/né; có thể null nếu chưa có plan.
+   * @param global_costmap Costmap toàn cục (không sở hữu).
+   * @param local_costmap  Costmap cục bộ (không sở hữu).
+   */
+  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;
+
+  /**
+   * @brief Chạy hành vi kiểu ONE-SHOT (họ A regen path, họ B clear costmap).
+   *        Trả kết quả cuối kèm output (path hoặc none). Guard chưa initialize/costmap null
+   *        -> RecoveryResult::Failed().
+   */
+  virtual RecoveryResult runBehavior() = 0;
+
+  /**
+   * @brief Sinh command kiểu PER-CYCLE (họ C rotation/backup). Lấy pose robot từ costmap/tf
+   *        bên trong; caller lặp gọi mỗi control cycle và publish command.
+   * @param dt Khoảng thời gian control cycle (s), > 0.
+   * @return RecoveryResult (thường output_type == kVelocity).
+   *
+   * Mặc định: coi như không hỗ trợ per-cycle và trả Failed() — họ A/B không cần override.
+   */
+  virtual RecoveryResult computeCommand(double dt);
+
+  /**
+   * @brief Trạng thái hiện tại của lượt recovery.
+   */
+  virtual RecoveryStatus status() const = 0;
+
+  virtual std::string getNameRecoveryBehavior() const
+  {
+    return name_;
+  }
+
+protected:
+  RecoveryBehavior() = default;
+  std::string name_;
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_BEHAVIOR_H_

include/recovery_core/recovery_config.h

@@ -0,0 +1,53 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — config chung cho recovery behaviors.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_CONFIG_H_
+#define RECOVERY_CORE_RECOVERY_CONFIG_H_
+
+#include <string>
+
+// Forward declare để không kéo <robot/robot.h> vào header interface.
+namespace robot { class NodeHandle; }
+
+namespace recovery_core
+{
+
+/**
+ * @struct RecoveryConfig
+ * @brief Gói các tham số CHUNG cho vòng đời recovery.
+ *
+ * Tham số RIÊNG của từng hành vi (vd: spin_target_angle, backup_distance) do plugin tự đọc
+ * qua robot::NodeHandle trong initialize(); struct này chỉ giữ phần chung.
+ *
+ * Đơn vị:
+ *  - control_frequency : Hz  (tần số gọi computeCommand khi chạy per-cycle)
+ *  - timeout           : s   (0 = không giới hạn thời gian)
+ */
+struct RecoveryConfig
+{
+  double control_frequency = 20.0;  ///< Hz, > 0.
+  double timeout = 0.0;             ///< s, >= 0; 0 nghĩa là không timeout.
+
+  /**
+   * @brief Kiểm tra hợp lệ các tham số.
+   * @param error [out] Nếu != nullptr và invalid, ghi thông điệp lỗi.
+   * @return true nếu hợp lệ.
+   */
+  bool validate(std::string* error) const;
+
+  /**
+   * @brief Đọc config từ NodeHandle (có default, có validate + log cảnh báo nếu sai).
+   * @param nh NodeHandle đã trỏ tới namespace của behavior.
+   * @return RecoveryConfig đã điền (giá trị sai được thay bằng default).
+   */
+  static RecoveryConfig fromNodeHandle(robot::NodeHandle& nh);
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_CONFIG_H_

include/recovery_core/recovery_types.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — kiểu hợp đồng output cho recovery behaviors.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_TYPES_H_
+#define RECOVERY_CORE_RECOVERY_TYPES_H_
+
+#include <robot_geometry_msgs/Twist.h>
+#include <robot_nav_msgs/Path.h>
+
+namespace recovery_core
+{
+
+/**
+ * @enum RecoveryStatus
+ * @brief Trạng thái tiến trình của một lượt recovery.
+ */
+enum class RecoveryStatus
+{
+  kIdle,       ///< Chưa bắt đầu (sau initialize/reset).
+  kRunning,    ///< Đang thực thi, cần tiếp tục gọi.
+  kSucceeded,  ///< Hoàn thành thành công.
+  kFailed      ///< Lỗi/không thể thực thi an toàn (trả stop output).
+};
+
+/**
+ * @enum RecoveryOutputType
+ * @brief Loại output hành vi sinh ra ở kết quả hiện tại.
+ *        Quyết định trường nào trong RecoveryResult là hợp lệ để đọc.
+ */
+enum class RecoveryOutputType
+{
+  kNone,      ///< Không output (vd: clear costmap) — chỉ đọc status.
+  kVelocity,  ///< Output là command (Twist) — họ rotation/backup.
+  kPath       ///< Output là path — họ regen path.
+};
+
+/**
+ * @struct RecoveryResult
+ * @brief Kết quả hợp nhất cho cả 3 họ recovery.
+ *
+ * Bất biến: chỉ đọc trường khớp với @ref output_type.
+ *  - kNone     : bỏ qua command/path.
+ *  - kVelocity : dùng command; path để mặc định.
+ *  - kPath     : dùng path; command để mặc định.
+ */
+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.
+
+  /// @brief Đang chạy, không output.
+  static RecoveryResult Running();
+  /// @brief Thành công, không output.
+  static RecoveryResult Succeeded();
+  /// @brief Thất bại, không output (caller nên dừng an toàn).
+  static RecoveryResult Failed();
+  /// @brief Output vận tốc kèm status (kRunning hoặc kSucceeded).
+  static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
+                                 RecoveryStatus status);
+  /// @brief Output path kèm status.
+  static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
+                                RecoveryStatus status);
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_TYPES_H_

package.xml

@@ -0,0 +1,42 @@
+<package>
+  <name>recovery_core</name>
+  <version>0.1.0</version>
+  <description>
+    recovery_core định nghĩa interface (base class) cho các hành vi recovery của navigation
+    stack ROS-like T800. Mô phỏng robot_nav_core::RecoveryBehavior (giữ chữ ký initialize với
+    tf + costmap), nhưng tổng quát hoá output để bao 3 họ recovery: trả về path
+    (robot_nav_msgs::Path), không trả output (clear costmap), và trả về vận tốc
+    (robot_geometry_msgs::Twist) theo từng control cycle.
+
+    Không chạy roscpp/ROS master thật; dùng lớp ROS-like robot_* (robot_costmap_2d, tf3,
+    robot_cpp, robot_time). Core library không publish trực tiếp; các behavior mẫu build thành
+    plugin Boost.DLL riêng để adapter/caller nạp và tiêu thụ RecoveryResult.
+  </description>
+  <author>T800 Robotics</author>
+  <maintainer email="tdd@gmail.com">T800 Robotics</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>robot_time</build_depend>
+  <run_depend>robot_time</run_depend>
+
+  <build_depend>tf3</build_depend>
+  <run_depend>tf3</run_depend>
+
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+  
+  <build_depend>robot_xmlrpcpp</build_depend>
+  <run_depend>robot_xmlrpcpp</run_depend>
+
+</package>

plugins/back_up_recovery.cpp

@@ -0,0 +1,153 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — per-cycle backup recovery plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+#include <recovery_core/recovery_config.h>
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include <boost/dll/alias.hpp>
+#include <robot/robot.h>
+
+namespace recovery_plugins
+{
+namespace
+{
+robot_geometry_msgs::Twist zeroTwist()
+{
+  return robot_geometry_msgs::Twist();
+}
+
+bool validCycle(double dt)
+{
+  return std::isfinite(dt) && dt > 0.0;
+}
+}  // namespace
+
+class BackUpRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  BackUpRecovery() = default;
+
+  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) override
+  {
+    if (initialized_)
+    {
+      robot::log_error("[recovery_core] BackUpRecovery '%s' initialized twice; ignoring.",
+                       name_.c_str());
+      return;
+    }
+
+    name_ = std::move(name);
+    tf_ = tf;
+    global_path_ = global_path;
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    robot::NodeHandle private_nh("~/" + name_);
+    config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
+    private_nh.param("backup_distance", backup_distance_, 0.5);
+    private_nh.param("linear_speed", linear_speed_, 0.1);
+    private_nh.param("require_costmap", require_costmap_, false);
+
+    if (!std::isfinite(backup_distance_) || backup_distance_ <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
+                         name_.c_str());
+      backup_distance_ = 0.5;
+    }
+    if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
+                         name_.c_str());
+      linear_speed_ = 0.1;
+    }
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
+                             recovery_core::RecoveryStatus::kFailed;
+    return initialized_ ? recovery_core::RecoveryResult::Running() :
+                          recovery_core::RecoveryResult::Failed();
+  }
+
+  recovery_core::RecoveryResult computeCommand(double dt) override
+  {
+    if (!initialized_ || !validCycle(dt) || (require_costmap_ && local_costmap_ == nullptr))
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(),  );
+    }
+
+    elapsed_ += dt;
+    if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    if (traveled_distance_ >= backup_distance_)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    robot_geometry_msgs::Twist command;
+    command.linear.x = -std::abs(linear_speed_);
+    traveled_distance_ = std::min(backup_distance_, traveled_distance_ + std::abs(command.linear.x) * dt);
+
+    if (traveled_distance_ >= backup_distance_)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    status_ = recovery_core::RecoveryStatus::kRunning;
+    return recovery_core::RecoveryResult::Velocity(command, status_);
+  }
+
+  recovery_core::RecoveryStatus status() const override
+  {
+    return status_;
+  }
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<BackUpRecovery>();
+  }
+
+private:
+  tf3::BufferCore* tf_ = nullptr;
+  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
+  recovery_core::RecoveryConfig config_;
+  bool initialized_ = false;
+  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+
+  double backup_distance_ = 0.5;
+  double linear_speed_ = 0.1;
+  bool require_costmap_ = false;
+  double elapsed_ = 0.0;
+  double traveled_distance_ = 0.0;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::BackUpRecovery::create, BackUpRecovery)

plugins/clear_costmap_recovery.cpp

@@ -0,0 +1,235 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — no-output clear costmap plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <algorithm>
+#include <cmath>
+#include <set>
+#include <string>
+#include <vector>
+
+#include <boost/dll/alias.hpp>
+#include <boost/pointer_cast.hpp>
+#include <boost/thread/locks.hpp>
+#include <robot/robot.h>
+#include <robot_costmap_2d/costmap_layer.h>
+
+namespace recovery_plugins
+{
+namespace
+{
+std::string leafName(std::string name)
+{
+  const std::string::size_type slash = name.rfind('/');
+  if (slash != std::string::npos)
+  {
+    name = name.substr(slash + 1);
+  }
+  return name;
+}
+
+bool isValidResetDistance(double value)
+{
+  return std::isfinite(value) && value > 0.0;
+}
+}  // namespace
+
+class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  ClearCostmapRecovery() = default;
+
+  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) override
+  {
+    if (initialized_)
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' initialized twice; ignoring.",
+                       name_.c_str());
+      return;
+    }
+
+    name_ = std::move(name);
+    tf_ = tf;
+    global_path_ = global_path;
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    robot::NodeHandle private_nh("~/" + name_);
+    private_nh.param("reset_distance", reset_distance_, 3.0);
+    private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
+    private_nh.param("force_updating", force_updating_, false);
+    private_nh.param("affected_maps", affected_maps_, std::string("both"));
+
+    if (!isValidResetDistance(reset_distance_))
+    {
+      robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.",
+                         name_.c_str());
+      reset_distance_ = 3.0;
+    }
+
+    if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
+    {
+      robot::log_warning("[recovery_core] Invalid affected_maps '%s' for '%s'; using 'both'.",
+                         affected_maps_.c_str(), name_.c_str());
+      affected_maps_ = "both";
+    }
+
+    std::vector<std::string> clearable_layers_default;
+    clearable_layers_default.emplace_back("obstacles");
+    std::vector<std::string> clearable_layers;
+    private_nh.param("layer_names", clearable_layers, clearable_layers_default);
+    clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    if (!initialized_)
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Failed();
+    }
+
+    bool ok = true;
+    if (affected_maps_ == "global" || affected_maps_ == "both")
+    {
+      ok = clear(global_costmap_) && ok;
+      if (ok && force_updating_ && global_costmap_ != nullptr)
+      {
+        global_costmap_->updateMap();
+      }
+    }
+
+    if (affected_maps_ == "local" || affected_maps_ == "both")
+    {
+      ok = clear(local_costmap_) && ok;
+      if (ok && force_updating_ && local_costmap_ != nullptr)
+      {
+        local_costmap_->updateMap();
+      }
+    }
+
+    status_ = ok ? recovery_core::RecoveryStatus::kSucceeded :
+                   recovery_core::RecoveryStatus::kFailed;
+    return ok ? recovery_core::RecoveryResult::Succeeded() :
+                recovery_core::RecoveryResult::Failed();
+  }
+
+  recovery_core::RecoveryStatus status() const override
+  {
+    return status_;
+  }
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<ClearCostmapRecovery>();
+  }
+
+private:
+  bool clear(robot_costmap_2d::Costmap2DROBOT* costmap)
+  {
+    if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap.",
+                       name_.c_str());
+      return false;
+    }
+
+    robot_geometry_msgs::PoseStamped pose;
+    if (!costmap->getRobotPose(pose))
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' cannot get robot pose.",
+                       name_.c_str());
+      return false;
+    }
+
+    std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>* plugins =
+        costmap->getLayeredCostmap()->getPlugins();
+    if (plugins == nullptr)
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap layers.",
+                       name_.c_str());
+      return false;
+    }
+
+    bool touched_layer = false;
+    for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
+    {
+      if (!plugin)
+      {
+        continue;
+      }
+
+      const std::string name = leafName(plugin->getName());
+      if (clearable_layers_.count(name) == 0)
+      {
+        continue;
+      }
+
+      if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
+      {
+        robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.",
+                           name.c_str());
+        continue;
+      }
+
+      clearMap(boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin),
+               pose.pose.position.x, pose.pose.position.y);
+      touched_layer = true;
+    }
+
+    return touched_layer;
+  }
+
+  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap,
+                double pose_x, double pose_y)
+  {
+    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
+    const double start_point_x = pose_x - reset_distance_ / 2.0;
+    const double start_point_y = pose_y - reset_distance_ / 2.0;
+    const double end_point_x = start_point_x + reset_distance_;
+    const double end_point_y = start_point_y + reset_distance_;
+
+    int start_x = 0;
+    int start_y = 0;
+    int end_x = 0;
+    int end_y = 0;
+    costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+    costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
+
+    costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
+    costmap->addExtraBounds(costmap->getOriginX(), costmap->getOriginY(),
+                            costmap->getOriginX() + costmap->getSizeInMetersX(),
+                            costmap->getOriginY() + costmap->getSizeInMetersY());
+  }
+
+  tf3::BufferCore* tf_ = nullptr;
+  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
+  bool initialized_ = false;
+  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+
+  bool force_updating_ = false;
+  double reset_distance_ = 3.0;
+  bool invert_area_to_clear_ = false;
+  std::string affected_maps_ = "both";
+  std::set<std::string> clearable_layers_;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::ClearCostmapRecovery::create, ClearCostmapRecovery)

plugins/regen_path_recovery.cpp

@@ -0,0 +1,84 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — path output recovery plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <string>
+#include <vector>
+
+#include <boost/dll/alias.hpp>
+#include <robot/robot.h>
+
+namespace recovery_plugins
+{
+
+class RegenPathRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  RegenPathRecovery() = default;
+
+  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) override
+  {
+    if (initialized_)
+    {
+      robot::log_error("[recovery_core] RegenPathRecovery '%s' initialized twice; ignoring.",
+                       name_.c_str());
+      return;
+    }
+
+    name_ = std::move(name);
+    tf_ = tf;
+    global_path_ = global_path;
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    if (!initialized_ || global_path_ == nullptr || global_path_->empty())
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Failed();
+    }
+
+    robot_nav_msgs::Path path;
+    path.poses = *global_path_;
+
+    status_ = recovery_core::RecoveryStatus::kSucceeded;
+    return recovery_core::RecoveryResult::PathOut(path, status_);
+  }
+
+  recovery_core::RecoveryStatus status() const override
+  {
+    return status_;
+  }
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<RegenPathRecovery>();
+  }
+
+private:
+  tf3::BufferCore* tf_ = nullptr;
+  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
+  bool initialized_ = false;
+  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)

plugins/rotate_recovery.cpp

@@ -0,0 +1,152 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — per-cycle rotate recovery plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+#include <recovery_core/recovery_config.h>
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include <boost/dll/alias.hpp>
+#include <robot/robot.h>
+
+namespace recovery_plugins
+{
+namespace
+{
+robot_geometry_msgs::Twist zeroTwist()
+{
+  return robot_geometry_msgs::Twist();
+}
+
+bool validCycle(double dt)
+{
+  return std::isfinite(dt) && dt > 0.0;
+}
+}  // namespace
+
+class RotateRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  RotateRecovery() = default;
+
+  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) override
+  {
+    if (initialized_)
+    {
+      robot::log_error("[recovery_core] RotateRecovery '%s' initialized twice; ignoring.",
+                       name_.c_str());
+      return;
+    }
+
+    name_ = std::move(name);
+    tf_ = tf;
+    global_path_ = global_path;
+    global_costmap_ = global_costmap;
+    local_costmap_ = local_costmap;
+
+    robot::NodeHandle private_nh("~/" + name_);
+    config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
+    private_nh.param("target_angle", target_angle_, 1.57079632679);
+    private_nh.param("angular_speed", angular_speed_, 0.4);
+
+    if (!std::isfinite(target_angle_) || std::abs(target_angle_) <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
+                         name_.c_str());
+      target_angle_ = 1.57079632679;
+    }
+    if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
+                         name_.c_str());
+      angular_speed_ = 0.4;
+    }
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
+                             recovery_core::RecoveryStatus::kFailed;
+    return initialized_ ? recovery_core::RecoveryResult::Running() :
+                          recovery_core::RecoveryResult::Failed();
+  }
+
+  recovery_core::RecoveryResult computeCommand(double dt) override
+  {
+    if (!initialized_ || !validCycle(dt))
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    elapsed_ += dt;
+    if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    const double target = std::abs(target_angle_);
+    if (rotated_angle_ >= target)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    robot_geometry_msgs::Twist command;
+    command.angular.z = std::copysign(angular_speed_, target_angle_);
+    rotated_angle_ = std::min(target, rotated_angle_ + std::abs(command.angular.z) * dt);
+
+    if (rotated_angle_ >= target)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    status_ = recovery_core::RecoveryStatus::kRunning;
+    return recovery_core::RecoveryResult::Velocity(command, status_);
+  }
+
+  recovery_core::RecoveryStatus status() const override
+  {
+    return status_;
+  }
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<RotateRecovery>();
+  }
+
+private:
+  tf3::BufferCore* tf_ = nullptr;
+  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
+  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
+  recovery_core::RecoveryConfig config_;
+  bool initialized_ = false;
+  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+
+  double target_angle_ = 1.57079632679;
+  double angular_speed_ = 0.4;
+  double elapsed_ = 0.0;
+  double rotated_angle_ = 0.0;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, RotateRecovery)

src/recovery_behavior.cpp

@@ -0,0 +1,19 @@
+/*********************************************************************
+ * recovery_core — default impl cho RecoveryBehavior.
+ *
+ * Chỉ chứa default computeCommand() (họ A/B không override). initialize()/runBehavior()/
+ * status() là thuần ảo, do plugin cụ thể triển khai.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <recovery_core/recovery_behavior.h>
+
+namespace recovery_core
+{
+
+RecoveryResult RecoveryBehavior::computeCommand(double /*dt*/)
+{
+  return RecoveryResult::Failed();
+}
+
+}  // namespace recovery_core

src/recovery_config.cpp

@@ -0,0 +1,102 @@
+/*********************************************************************
+ * recovery_core — validate + đọc RecoveryConfig.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <recovery_core/recovery_config.h>
+
+#include <cmath>
+#include <string>
+
+#include <robot/robot.h>
+
+namespace recovery_core
+{
+namespace
+{
+
+constexpr double kDefaultControlFrequency = 20.0;
+constexpr double kDefaultTimeout = 0.0;
+
+void appendError(std::string* error, const std::string& message)
+{
+  if (error == nullptr)
+  {
+    return;
+  }
+
+  if (!error->empty())
+  {
+    *error += "; ";
+  }
+  *error += message;
+}
+
+bool invalidControlFrequency(double value)
+{
+  return !std::isfinite(value) || value <= 0.0;
+}
+
+bool invalidTimeout(double value)
+{
+  return !std::isfinite(value) || value < 0.0;
+}
+
+}  // namespace
+
+bool RecoveryConfig::validate(std::string* error) const
+{
+  if (error != nullptr)
+  {
+    error->clear();
+  }
+
+  bool valid = true;
+  if (invalidControlFrequency(control_frequency))
+  {
+    appendError(error, "control_frequency must be finite and > 0 Hz");
+    valid = false;
+  }
+
+  if (invalidTimeout(timeout))
+  {
+    appendError(error, "timeout must be finite and >= 0 s");
+    valid = false;
+  }
+
+  return valid;
+}
+
+RecoveryConfig RecoveryConfig::fromNodeHandle(robot::NodeHandle& nh)
+{
+  RecoveryConfig config;
+  nh.param("control_frequency", config.control_frequency, kDefaultControlFrequency);
+  nh.param("timeout", config.timeout, kDefaultTimeout);
+
+  std::string error;
+  if (config.validate(&error))
+  {
+    return config;
+  }
+
+  robot::log_warning("[recovery_core] Invalid common recovery config: %s. "
+                     "Replacing invalid values with defaults.",
+                     error.c_str());
+
+  if (invalidControlFrequency(config.control_frequency))
+  {
+    config.control_frequency = kDefaultControlFrequency;
+  }
+  if (invalidTimeout(config.timeout))
+  {
+    config.timeout = kDefaultTimeout;
+  }
+
+  if (!config.validate(nullptr))
+  {
+    return RecoveryConfig{};
+  }
+  return config;
+}
+
+}  // namespace recovery_core

src/recovery_types.cpp

@@ -0,0 +1,55 @@
+/*********************************************************************
+ * recovery_core — factory cho RecoveryResult.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <recovery_core/recovery_types.h>
+
+namespace recovery_core
+{
+
+RecoveryResult RecoveryResult::Running()
+{
+  RecoveryResult result;
+  result.status = RecoveryStatus::kRunning;
+  result.output_type = RecoveryOutputType::kNone;
+  return result;
+}
+
+RecoveryResult RecoveryResult::Succeeded()
+{
+  RecoveryResult result;
+  result.status = RecoveryStatus::kSucceeded;
+  result.output_type = RecoveryOutputType::kNone;
+  return result;
+}
+
+RecoveryResult RecoveryResult::Failed()
+{
+  RecoveryResult result;
+  result.status = RecoveryStatus::kFailed;
+  result.output_type = RecoveryOutputType::kNone;
+  return result;
+}
+
+RecoveryResult RecoveryResult::Velocity(const robot_geometry_msgs::Twist& command,
+                                        RecoveryStatus status)
+{
+  RecoveryResult result;
+  result.status = status;
+  result.output_type = RecoveryOutputType::kVelocity;
+  result.command = command;
+  return result;
+}
+
+RecoveryResult RecoveryResult::PathOut(const robot_nav_msgs::Path& path,
+                                       RecoveryStatus status)
+{
+  RecoveryResult result;
+  result.status = status;
+  result.output_type = RecoveryOutputType::kPath;
+  result.path = path;
+  return result;
+}
+
+}  // namespace recovery_core

test/CMakeLists.txt

@@ -0,0 +1,26 @@
+# Test target cho recovery_core contract.
+# Build khi cấu hình standalone và khi catkin bật test target của package.
+
+add_executable(recovery_core_plugin_loader_test plugin_loader_contract_test.cpp)
+
+target_include_directories(recovery_core_plugin_loader_test
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${STANDALONE_INCLUDE_DIRS}
+)
+
+target_link_libraries(recovery_core_plugin_loader_test
+  PRIVATE
+    recovery_core
+    yaml-cpp
+    ${Boost_LIBRARIES}
+    ${CMAKE_THREAD_LIBS_INIT}
+    ${CMAKE_DL_LIBS}
+)
+
+add_dependencies(recovery_core_plugin_loader_test ${RECOVERY_CORE_PLUGIN_TARGETS})
+
+target_compile_definitions(recovery_core_plugin_loader_test
+  PRIVATE
+    RECOVERY_CORE_PLUGIN_DIR=\"$<TARGET_FILE_DIR:recovery_core_rotate_recovery>\"
+)

test/plugin_loader_contract_test.cpp

@@ -0,0 +1,240 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — Boost.DLL plugin contract test.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <robot/robot.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
+
+#include <boost/dll/import.hpp>
+#include <boost/dll/shared_library.hpp>
+#include <boost/filesystem/path.hpp>
+
+namespace
+{
+using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr();
+
+struct PluginConfig
+{
+    std::string name;
+    std::string type;
+};
+std::vector<boost::dll::shared_library> libraries_;
+std::vector<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr> creators_;
+std::vector<std::string> name_plugins_;
+
+std::vector<robot_geometry_msgs::PoseStamped> global_path_;
+
+void expect(bool condition, const std::string& message)
+{
+  if (!condition)
+  {
+    std::cerr << "[FAIL] " << message << std::endl;
+    std::exit(1);
+  }
+}
+
+std::vector<PluginConfig> getListRecoveryPlugins()
+{
+
+std::vector<PluginConfig> my_list;
+
+robot::NodeHandle priv_nh;
+
+if (priv_nh.hasParam("recovery_behaviors"))
+{
+    YAML::Node my_plugins = priv_nh.getParamValue("recovery_behaviors");
+
+    if (my_plugins.IsDefined() && my_plugins.IsSequence())
+    {
+       std::set<std::string> name_plugins;
+        for (std::size_t i = 0; i < my_plugins.size(); ++i)
+        {
+            YAML::Node plugin_i = my_plugins[i];
+
+            // 1. Phải là map
+            if (!plugin_i.IsMap())
+            {
+                std::cerr<< "Recovery plugin at index " << i << " must be a map." << std::endl;
+                continue;
+            }
+
+            // 2. Phải có name và type
+            if (!plugin_i["name"].IsDefined() || !plugin_i["type"].IsDefined())
+            {
+                std::cerr << "Recovery plugin at index " << i << " must have 'name' and 'type'." << std::endl;
+                continue;
+            }
+
+            PluginConfig p;
+
+            try
+            {
+                p.name = plugin_i["name"].as<std::string>();
+                p.type = plugin_i["type"].as<std::string>();
+            }
+            catch (const YAML::Exception& e)
+            {
+                std::cerr << "Invalid recovery plugin at index " << i << ": " << e.what() << std::endl;
+                continue;
+            }
+
+            // 3. Kiểm tra duplicate name
+            const auto result = name_plugins.insert(p.name);
+
+            if (!result.second)
+            {
+                std::cerr << "A recovery plugin with name '" << p.name << "' already exists." << std::endl;
+                continue;
+            }
+
+            // 4. Chỉ thêm sau khi validate hoàn toàn
+            my_list.push_back(p);
+            name_plugins_.push_back(p.name);
+
+            robot::log_warning("Load plugin: name: %s, type: %s", p.name.c_str(), p.type.c_str());
+        }
+    }
+    else
+    {
+        std::cerr << "'recovery_behaviors' must be a sequence." << std::endl;
+    }
+}
+
+return my_list;
+}
+
+void testLoadPlugins()
+{
+       std::vector<PluginConfig> my_list_plugin = getListRecoveryPlugins();
+       if(my_list_plugin.empty())
+       {
+              robot::log_error("No recovery plugins found in configuration.");
+              return;
+       }
+       for(const auto& plugin : my_list_plugin)
+       {
+              robot::PluginLoaderHelper loader;
+              std::string path_file_so = loader.findLibraryPath(plugin.type);
+              if(path_file_so == "")
+              {
+                     robot::log_error("Cannot find library for recovery behavior type '%s'", plugin.type.c_str());
+                     return;
+              }
+              robot::log_info("Loading recovery behavior type '%s' from '%s'", plugin.type.c_str(), path_file_so.c_str());
+              try
+              {
+                     // 1. Load library vào local handle.
+                     boost::dll::shared_library library(path_file_so);
+
+                     // 2. Lấy factory alias.
+                     auto& factory = library.get_alias<Factory>(plugin.type);
+
+                     // 3. Factory tạo behavior object.
+                     recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = factory();
+
+                     if (!behavior)
+                     {
+                            robot::log_error("Factory returned nullptr for '%s'", plugin.type.c_str());
+                            return;
+                     }
+
+                     // 4. Chuyển quyền giữ library vào storage sống lâu dài.
+                     libraries_.push_back(std::move(library));
+                     behavior->initialize(plugin.name, nullptr, &global_path_, nullptr, nullptr);
+                     creators_.push_back(behavior);
+              }
+              catch (const std::exception& e)
+              {
+                     robot::log_error("Failed to load recovery behavior '%s': %s", plugin.type.c_str(), e.what());
+                     return;
+              }
+       //     expect(static_cast<bool>(behavior), "Failed to load plugin: " + plugin.name + " of type: " + plugin.type);
+       }
+}
+
+void testRotatePlugin()
+{
+for(const auto& behavior : creators_)
+ {
+     if(behavior->getNameRecoveryBehavior() == "rotation_rc")
+     {
+         const recovery_core::RecoveryResult first = behavior->computeCommand(0.1);
+
+         expect(first.status == recovery_core::RecoveryStatus::kRunning,
+                "rotate first cycle must be running");
+         expect(first.output_type == recovery_core::RecoveryOutputType::kVelocity,
+                "rotate must return velocity output");
+         expect(first.command.angular.z > 0.0, "rotate must command positive angular.z by default");
+
+         recovery_core::RecoveryResult last = first;
+         for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
+         {
+           last = behavior->computeCommand(0.1);
+         }
+
+         expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
+                "rotate must finish within bounded cycles");
+         expect(last.output_type == recovery_core::RecoveryOutputType::kVelocity,
+                "rotate final result must still be a velocity output");
+         expect(std::abs(last.command.angular.z) < 1e-9,
+                "rotate must return a zero angular command when complete");
+     }
+ }
+}
+
+void testRegenPathPlugin()
+{
+robot_geometry_msgs::PoseStamped pose1;
+pose1.pose.position.x = 1.0;
+robot_geometry_msgs::PoseStamped pose2;
+pose2.pose.position.x = 2.0;
+global_path_.push_back(pose1);
+global_path_.push_back(pose2);
+
+
+ for(const auto& behavior : creators_)
+ {
+     if(behavior->getNameRecoveryBehavior() == "regen_path_rc")
+     {
+         const recovery_core::RecoveryResult result = behavior->runBehavior();
+
+         expect(result.status == recovery_core::RecoveryStatus::kSucceeded,
+                "regen path must succeed with a non-empty global path");
+         expect(result.output_type == recovery_core::RecoveryOutputType::kPath,
+                "regen path must return path output");
+         expect(result.path.poses.size() == global_path_.size(),
+                "regen path must preserve the global path size");
+       global_path_ = result.path.poses;
+     }
+ }
+}
+
+}  // namespace
+
+int main()
+{
+  testLoadPlugins();
+  if(creators_.empty()) return 0;
+//   for(auto& behavior : creators_)
+//   {
+//     std::cout<<behavior->getNameRecoveryBehavior()<<std::endl;
+//   }
+
+  testRegenPathPlugin();
+  testRotatePlugin();
+
+  std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
+  return 0;
+}