DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

temporary storage

Browse Source
This commit is contained in:
duongtd
2026-07-09 16:50:35 +07:00
parent e4f2823b17
commit 915cf85cc5
19 changed files with 2463 additions and 326 deletions
Download Patch File Download Diff File Expand all files Collapse all files

586
CMakeLists.txt Normal file
View File

@@ -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()

663
PLAN.md
View File

@@ -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 `recovery_core` là package **interface** cho recovery behavior trong navigation stack ROS-like
> vai trò như `robot_nav_core` cung cấp `RecoveryBehavior`, nhưng **không chạy ROS** và T800. Package này không chứa thuật toán recovery cụ thể; nó định nghĩa contract chung để các
> biểu đạt hợp đồng qua `robot_geometry_msgs` / `robot_nav_msgs` (header-only). plugin recovery có thể trả trạng thái, velocity command hoặc path.
>
> 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.
--- ## 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: ### 2.1. `recovery_core` chịu trách nhiệm
- **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).
So với `robot_nav_core::RecoveryBehavior` (blocking `runBehavior()`, cần costmap + tf): - Định nghĩa interface `RecoveryBehavior`.
`recovery_core` giữ *tinh thần interface* nhưng thay hạ tầng ROS bằng abstraction ROS-free và - Định nghĩa result contract:
mở rộng để hành vi có thể **trả về output** (không chỉ chạy rồi thôi). - `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 | - Không implement recovery cụ thể như clear costmap, rotate, backup, regen path.
|----|-------|------------------| - Không publish `cmd_vel`.
| **A. Trả v<><76> path** | tạo lại một đoạn đường thoát/né | `robot_nav_msgs::Path` | - Không gọi ROS service hoặc action.
| **B. Không output** | clear costmap, reset state | chỉ trạng thái (SUCCEEDED/FAILED) | - Không tự collision-check velocity/path output.
| **C. Trả về vận tốc** | rotation tại chỗ, backup (không ROS) | `robot_geometry_msgs::Twist` theo cycle | - 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. ## 3. Interface Contract Đã Chốt
→ 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).
### 1.3. Ranh giới trách nhiệm ### 3.1. Recovery status
`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
```cpp ```cpp
namespace recovery_core { enum class RecoveryStatus
{
kIdle,
kRunning,
kSucceeded,
kFailed
};
```
// Trạng thái tiến trình 1 lượt recovery. Ý nghĩa:
enum class RecoveryStatus { kIdle, kRunning, kSucceeded, kFailed }; - `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. ### 3.2. Output type
enum class RecoveryOutputType { kNone, kVelocity, kPath };
// Kết quả hợp nhất cho cả 3 họ. Chỉ đọc trường khớp với output_type. ```cpp
struct RecoveryResult { 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; RecoveryStatus status = RecoveryStatus::kRunning;
RecoveryOutputType output_type = RecoveryOutputType::kNone; RecoveryOutputType output_type = RecoveryOutputType::kNone;
robot_geometry_msgs::Twist command; // hợp lệ khi output_type == kVelocity robot_geometry_msgs::Twist command;
robot_nav_msgs::Path path; // hợp lệ khi output_type == kPath 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 Running();
static RecoveryResult Succeeded(); static RecoveryResult Succeeded();
static RecoveryResult Failed(); static RecoveryResult Failed();
static RecoveryResult Velocity(const robot_geometry_msgs::Twist&, RecoveryStatus); static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
static RecoveryResult PathOut(const robot_nav_msgs::Path&, RecoveryStatus); 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. ### 3.4. Recovery behavior
`recovery_core` chỉ khai báo interface; caller cấp implementation cụ thể (đọc từ đâu là việc
của caller).
```cpp ```cpp
// Interface tối thiểu; mở rộng dần khi có nhu cầu thực. class RecoveryBehavior
class RecoveryContext { {
public: 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>; using Ptr = std::shared_ptr<RecoveryBehavior>;
virtual ~RecoveryBehavior() = default; virtual ~RecoveryBehavior() = default;
// 1) Nạp + validate config. Không throw; trả false + set error nếu sai. virtual void initialize(std::string name,
virtual bool configure(const RecoveryConfig& config, std::string* error) = 0; 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 RecoveryResult runBehavior() = 0;
virtual void start(const robot_geometry_msgs::Pose2D& current_pose) = 0; virtual RecoveryResult computeCommand(double dt);
// 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 RecoveryStatus status() const = 0; virtual RecoveryStatus status() const = 0;
virtual const char* name() const = 0;
protected: protected:
RecoveryBehavior() = default; RecoveryBehavior() = default;
}; };
``` ```
**Vì sao có cả `computeCommand` lẫn `runBehavior`:** Điểm khác `robot_nav_core::RecoveryBehavior`:
- Họ **C** (rotation/backup) tự nhiên là *per-cycle* → override `computeCommand`. - Có thêm `global_path` để behavior họ path có ngữ cảnh plan hiện tại.
- Họ **B** (clear costmap) tự nhiên là *one-shot* → override `runBehavior` (dùng `ctx`), - `runBehavior()` trả `RecoveryResult` thay vì `void`.
`computeCommand` chỉ trả trạng thái. - Có `computeCommand(dt)` cho behavior per-cycle.
- 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.
### 2.4. Config ## 4. Ba Nhóm Recovery
```cpp | Nhóm | Ví dụ | Method chính | Output |
struct RecoveryConfig { |------|-------|--------------|--------|
std::string type; // "back_up" | "spin" | "clear_costmap" | "regen_path" ... | A. Path output | regen path, detour path | `runBehavior()` | `RecoveryOutputType::kPath` |
double control_frequency = 20.0; // Hz, dùng cho default runBehavior loop (dt = 1/f) | B. No output | clear costmap, reset state | `runBehavior()` | `RecoveryOutputType::kNone` |
double timeout = 0.0; // s, 0 = không timeout | C. Velocity output | rotate, backup | `computeCommand(dt)` | `RecoveryOutputType::kVelocity` |
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;
};
```
> Package interface **không** biết tham số riêng của backup/spin — để plugin tự đọc từ Caller/adapter chịu trách nhiệm tiêu thụ output:
> `params`. `recovery_core` chỉ validate phần chung. - 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) ```text
**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
```
recovery_core/ recovery_core/
├── PLAN.md # tài liệu này ├── CMakeLists.txt
├── README.md # phạm vi: INTERFACE, không ROS, 3 họ recovery ├── package.xml
├── package.xml # depend: robot_geometry_msgs, robot_nav_msgs ├── README.md
├── CMakeLists.txt # catkin + standalone; lib chủ yếu header, .cpp cho helper ├── PLAN.md
├── include/ ├── include/recovery_core/
│ └── recovery_core/ │ ├── recovery_behavior.h
│ ├── recovery_types.h # enum Status, OutputType; struct RecoveryResult │ ├── recovery_config.h
│ ├── recovery_config.h # struct RecoveryConfig + validate() │ └── recovery_types.h
│ ├── recovery_context.h # interface RecoveryContext (ROS-free)
│ └── recovery_behavior.h # base class RecoveryBehavior (interface chính)
├── src/ ├── src/
│ ├── recovery_types.cpp # (stub) factory helper Running()/Failed()/... │ ├── recovery_behavior.cpp
│ ├── recovery_config.cpp # (stub) validate() │ ├── recovery_config.cpp
│ └── recovery_behavior.cpp # (stub) default runBehavior() loop │ └── recovery_types.cpp
├── test/ ├── test/
│ ├── CMakeLists.txt # khai báo test target (tuỳ chọn) │ ├── CMakeLists.txt
│ ├── interface_contract_test.cpp # (stub) dùng 1 MockBehavior kiểm vòng đời │ ├── interface_contract_test.cpp
│ └── mock_behavior.h # (stub) implement tối thiểu để test interface │ └── mock_behavior.h
├── examples/ ├── examples/
│ └── minimal_plugin.cpp # (stub) ví dụ 1 plugin per-cycle bé xíu, không ROS │ └── minimal_recovery.cpp
└── docs/ └── docs/
├── ARCHITECTURE.md # sơ đồ interface + luồng runBehavior/computeCommand ├── ARCHITECTURE.md
├── PLUGIN_GUIDE.md # hướng dẫn viết plugin cho từng họ A/B/C ├── PLUGIN_GUIDE.md
└── SAFETY.md # cảnh báo: không collision-check trong core └── 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 Runtime/build dependencies:
(`<robot_geometry_msgs/Twist.h>`, `<robot_geometry_msgs/Pose2D.h>`, - `robot_costmap_2d`
`<robot_nav_msgs/Path.h>` khi cần), khai báo đầy đủ chữ ký + Doxygen public API, thân hàm - `robot_cpp`
non-inline để trong `.cpp`. - `robot_time`
- **Source**: thân tối thiểu `// TODO(phase-2): implement` + `return {}` để compile. - `robot_geometry_msgs`
- **`package.xml`**: format 2, `<depend>robot_geometry_msgs</depend>`, - `robot_nav_msgs`
`<depend>robot_nav_msgs</depend>`, buildtool catkin. **Không** depend costmap/tf/robot_cpp. - `tf3`
- **`CMakeLists.txt`**: khung catkin+standalone của `robot_clear_costmap_recovery` nhưng chỉ - `Boost system thread`
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ế độ.
### 3.3. Checklist Phase 1 Không phụ thuộc:
- `robot_nav_core`
- `roscpp`
- `pluginlib`
- [ ] Tạo đủ cây thư mục 3.1. ## 7. Roadmap
- [ ] 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.
--- ### 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 Mục tiêu:
> phải viết hành vi cụ thể. Cụ thể: - 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/` Kết quả hiện tại:
`Velocity/PathOut`, đảm bảo set đúng `output_type` + `status`. Bất biến: khi - Catkin output: `devel/lib/librecovery_core.so`.
`output_type==kNone` thì `command`/`path` để mặc định (không rác). - Catkin test binary: `devel/lib/recovery_core/recovery_core_interface_test`.
2. **`recovery_config::validate`**: kiểm `control_frequency > 0`, `timeout >= 0`, hữu hạn; - Standalone output: `/tmp/recovery_core_phase1_build/librecovery_core.a`.
`type` không rỗng. Trả `false` + message rõ. Không throw. - Standalone test binary: `/tmp/recovery_core_phase1_build/test/recovery_core_interface_test`.
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`.
### 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ạng thái: **done**.
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.
### 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. Work items:
- [ ] `RecoveryConfig::validate` từ chối mọi input sai với message rõ. 1. [x] Implement `RecoveryResult` factories.
- [ ] default `runBehavior` đúng: loop, timeout, guard ctx/pose; không alloc trong loop. 2. [x] Implement `RecoveryConfig::validate`.
- [ ] `MockBehavior`/`MockContext` + contract test pass. 3. [x] Implement `RecoveryConfig::fromNodeHandle`.
- [ ] `docs/PLUGIN_GUIDE.md` mô tả cách 1 plugin họ A/B/C override method nào. 4. [x] Giữ default `RecoveryBehavior::computeCommand(dt)` trả `Failed()`.
- [ ] **Kiểm chứng:** build + test pass. 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 ```bash
họ ít nhất một ví dụ, + cơ chế nạp theo string. 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): Trạng thái: **done**.
- 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.
> Các plugin này có thể đặt trong `recovery_core/plugins/` hoặc package riêng — **cần chốt** Mục tiêu:
> (mục 6). Dù đặt đâu, chúng chỉ được include header của `recovery_core`, không thêm dependency - Viết plugin recovery thật implement `recovery_core::RecoveryBehavior`.
> ROS vào package interface. - 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/ Boost.DLL convention:
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:
```cpp ```cpp
// spin_recovery.cpp class RotateRecovery : public recovery_core::RecoveryBehavior
#include <recovery_core/recovery_behavior.h> {
#include <boost/dll/alias.hpp> public:
static recovery_core::RecoveryBehavior::Ptr create()
namespace recovery_plugins { {
return std::make_shared<RotateRecovery>();
class SpinRecovery : public recovery_core::RecoveryBehavior { /* override ... */
public:
static recovery_core::RecoveryBehavior::Ptr create() { // factory không tham số
return std::make_shared<SpinRecovery>();
} }
}; };
} // namespace recovery_plugins BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, rotate_recovery)
// Tên alias thứ 2 ("spin_recovery") chính là `type` trong YAML recovery_behaviors.
BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)
``` ```
Quy ước (giống `ClearCostmapRecovery` → alias `ClearCostmapRecovery`): Loader side:
- 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)
```cpp ```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()>( auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations); path_so, type, boost::dll::load_mode::append_decorations);
recovery_core::RecoveryBehavior::Ptr behavior = loader(); // tạo instance
std::string err; recovery_core::RecoveryBehavior::Ptr behavior = loader();
behavior->configure(config, &err); // rồi mới cấu hình 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 Acceptance:
> `docs/PLUGIN_GUIDE.md` sẽ ghi mẫu để người dùng plugin biết cách nạp. - [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`; ```bash
`system` theo pattern package cũ). Link `${Boost_LIBRARIES}` và `${CMAKE_DL_LIBS}` (dl). catkin_make --pkg recovery_core
- `set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON)` — bắt buộc cho .so. ./devel/lib/recovery_core/recovery_core_interface_test
- Build mỗi plugin thành **shared library** riêng; tên library + symbol khớp `type` YAML. ./devel/lib/recovery_core/recovery_core_plugin_loader_test
- Install `.so` vào nơi `findLibraryPath` tìm được (theo convention `PluginLoaderHelper`). cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase3_build
- **Không** cần `plugins.xml` (workspace này dùng Boost.DLL thuần, không pluginlib). 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 Trạng thái: **pending**.
`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.
### 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 Phương án:
cho plugin cần costmap/tf; plugin thuần spin/backup có thể ở `recovery_core/plugins/`). - Viết adapter riêng nếu cần tương thích `robot_nav_core::RecoveryBehavior`.
- [ ] Mỗi họ A/B/C có ≥ 1 plugin mẫu implement interface, build ra **shared library** riêng. - Adapter chịu trách nhiệm:
- [ ] Mỗi plugin có `static Ptr create()` + `BOOST_DLL_ALIAS(...::create, <type>)`; tên alias = - load plugin;
`type` YAML; trả `std::shared_ptr`. - gọi `initialize`;
- [ ] CMake plugin: `find_package(Boost COMPONENTS system filesystem)`, link `${CMAKE_DL_LIBS}`, - gọi `runBehavior` hoặc loop `computeCommand`;
`POSITION_INDEPENDENT_CODE ON`, install `.so` đúng nơi loader tìm. - publish velocity nếu output là `kVelocity`;
- [ ] Test end-to-end (không ROS): `import_alias<recovery_core::RecoveryBehavior::Ptr()>` nạp - thay path hoặc request replan nếu output là `kPath`;
`.so` mẫu → `configure` → `runBehavior`/`computeCommand` với MockContext → kiểm output - áp safety stop nếu output failed/timeout.
đú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.
--- 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` - Behavior velocity phải trả stop command khi không chắc an toàn.
(đơ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.** - Không publish command từ core/plugin nếu chưa qua adapter safety gate.
- **`RecoveryContext` rộng tới đâu:** tối thiểu chỉ `getRobotPose`. Họ B (clear costmap) và họ - `dt <= 0`, `NaN`, `inf`, missing tf/costmap/plan phải fail rõ ràng.
A (regen path cần costmap) sẽ cần hook thêm — nên để ở **context mở rộng của adapter**, giữ - Không log spam trong control loop.
context lõi nhỏ. **Cần chốt mức tối thiểu.** - Không parse YAML hoặc cấp phát lớn trong mỗi cycle.
- **Vị trí plugin:** trong `recovery_core/plugins/` hay package riêng. Đề xuất: package riêng - Đơn vị phải rõ:
cho plugin cần costmap/tf; plugin thuần (spin/backup) có thể ở `recovery_core/plugins/`. - distance: meter;
- **Cơ chế nạp = Boost.DLL (đã chốt):** theo đúng convention workspace (`import_alias`, không - angle: radian;
pluginlib). Ràng buộc kéo theo: `Ptr = std::shared_ptr`; factory `static Ptr create()` không - time: second;
tham số + `BOOST_DLL_ALIAS`; cấu hình qua `configure()` sau khi tạo. Core interface KHÔNG - linear velocity: m/s;
phụ thuộc Boost.DLL — chỉ plugin và loader. - angular velocity: rad/s.
- **`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.
--- ## 9. Definition Of Done
## 7. Tóm Tắt 3 Phase ### Package DoD
| Phase | Kết quả | Kiểm chứng | - `package.xml` hợp lệ.
|-------|---------|-----------| - Catkin build pass.
| 1 | Khung package interface: 4 header + stub, package.xml, CMake build lib rỗng | `cmake` + `make` OK | - Standalone CMake build pass.
| 2 | Hoàn thiện phần chung của interface: types/config/validate + default `runBehavior`, contract test qua Mock | build + test pass | - Header install/export đúng.
| 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 | - 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.

55
README.md Normal file
View File

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

41
docs/ARCHITECTURE.md Normal file
View File

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

78
docs/PLUGIN_GUIDE.md Normal file
View File

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

30
docs/SAFETY.md Normal file
View File

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

100
include/recovery_core/recovery_behavior.h Normal file
View File

@@ -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_

53
include/recovery_core/recovery_config.h Normal file
View File

@@ -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_

75
include/recovery_core/recovery_types.h Normal file
View File

@@ -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_

42
package.xml Normal file
View File

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

153
plugins/back_up_recovery.cpp Normal file
View File

@@ -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)

235
plugins/clear_costmap_recovery.cpp Normal file
View File

@@ -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)

84
plugins/regen_path_recovery.cpp Normal file
View File

@@ -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)

152
plugins/rotate_recovery.cpp Normal file
View File

@@ -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)

19
src/recovery_behavior.cpp Normal file
View File

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

102
src/recovery_config.cpp Normal file
View File

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

55
src/recovery_types.cpp Normal file
View File

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

26
test/CMakeLists.txt Normal file
View File

@@ -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>\"
)

240
test/plugin_loader_contract_test.cpp Normal file
View File

@@ -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;
}