From 915cf85cc5185245654208c9dacdd15b7a6bf478 Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 9 Jul 2026 16:50:35 +0700 Subject: [PATCH] temporary storage --- CMakeLists.txt | 586 +++++++++++++++++++ PLAN.md | 663 +++++++++++----------- README.md | 55 ++ docs/ARCHITECTURE.md | 41 ++ docs/PLUGIN_GUIDE.md | 78 +++ docs/SAFETY.md | 30 + include/recovery_core/recovery_behavior.h | 100 ++++ include/recovery_core/recovery_config.h | 53 ++ include/recovery_core/recovery_types.h | 75 +++ package.xml | 42 ++ plugins/back_up_recovery.cpp | 153 +++++ plugins/clear_costmap_recovery.cpp | 235 ++++++++ plugins/regen_path_recovery.cpp | 84 +++ plugins/rotate_recovery.cpp | 152 +++++ src/recovery_behavior.cpp | 19 + src/recovery_config.cpp | 102 ++++ src/recovery_types.cpp | 55 ++ test/CMakeLists.txt | 26 + test/plugin_loader_contract_test.cpp | 240 ++++++++ 19 files changed, 2463 insertions(+), 326 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 docs/ARCHITECTURE.md create mode 100644 docs/PLUGIN_GUIDE.md create mode 100644 docs/SAFETY.md create mode 100644 include/recovery_core/recovery_behavior.h create mode 100644 include/recovery_core/recovery_config.h create mode 100644 include/recovery_core/recovery_types.h create mode 100644 package.xml create mode 100644 plugins/back_up_recovery.cpp create mode 100644 plugins/clear_costmap_recovery.cpp create mode 100644 plugins/regen_path_recovery.cpp create mode 100644 plugins/rotate_recovery.cpp create mode 100644 src/recovery_behavior.cpp create mode 100644 src/recovery_config.cpp create mode 100644 src/recovery_types.cpp create mode 100644 test/CMakeLists.txt create mode 100644 test/plugin_loader_contract_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..41b3c38 --- /dev/null +++ b/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 + $ + $ + + ${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 + $ + $ + + 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 + $ + $ + ) + + + 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 + $ + $ + + 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() \ No newline at end of file diff --git a/PLAN.md b/PLAN.md index 7d88590..d16fdbb 100644 --- a/PLAN.md +++ b/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�� 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__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 { - 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(...) - // (xem move_base.cpp loadRecoveryBehaviors + robot_nav_core::RecoveryBehavior::Ptr). +class RecoveryBehavior +{ +public: using Ptr = std::shared_ptr; + 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* 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: +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 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 - (``, ``, - `` 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, `robot_geometry_msgs`, - `robot_nav_msgs`, 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` để khớp -chữ ký `import_alias`. - -#### 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 -#include - -namespace recovery_plugins { - -class SpinRecovery : public recovery_core::RecoveryBehavior { /* override ... */ - public: - static recovery_core::RecoveryBehavior::Ptr create() { // factory không tham số - return std::make_shared(); +class RotateRecovery : public recovery_core::RecoveryBehavior +{ +public: + static recovery_core::RecoveryBehavior::Ptr create() + { + return std::make_shared(); } }; -} // 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 -// path .so lấy qua PluginLoaderHelper::findLibraryPath(type) như move_base đang làm. auto loader = boost::dll::import_alias( - 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( 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, )`; 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` 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..3feaeaf --- /dev/null +++ b/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. diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md new file mode 100644 index 0000000..f289f0e --- /dev/null +++ b/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. diff --git a/docs/PLUGIN_GUIDE.md b/docs/PLUGIN_GUIDE.md new file mode 100644 index 0000000..02dae0a --- /dev/null +++ b/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 +#include + +namespace recovery_plugins { +class SpinRecovery : public recovery_core::RecoveryBehavior { + public: + static recovery_core::RecoveryBehavior::Ptr create() { + return std::make_shared(); + } + // 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 +auto loader = boost::dll::import_alias( + 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( 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 +``` diff --git a/docs/SAFETY.md b/docs/SAFETY.md new file mode 100644 index 0000000..c0de7b3 --- /dev/null +++ b/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. diff --git a/include/recovery_core/recovery_behavior.h b/include/recovery_core/recovery_behavior.h new file mode 100644 index 0000000..f6458f8 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +#include + +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(...)). + using RecoveryBehaviorPtr = std::shared_ptr; + + 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* 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_ diff --git a/include/recovery_core/recovery_config.h b/include/recovery_core/recovery_config.h new file mode 100644 index 0000000..720535d --- /dev/null +++ b/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 + +// Forward declare để không kéo 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_ diff --git a/include/recovery_core/recovery_types.h b/include/recovery_core/recovery_types.h new file mode 100644 index 0000000..1a0071f --- /dev/null +++ b/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 +#include + +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_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..a3587c4 --- /dev/null +++ b/package.xml @@ -0,0 +1,42 @@ + + recovery_core + 0.1.0 + + 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. + + T800 Robotics + T800 Robotics + BSD + + catkin + + robot_costmap_2d + robot_costmap_2d + + robot_cpp + robot_cpp + + robot_time + robot_time + + tf3 + tf3 + + robot_geometry_msgs + robot_geometry_msgs + + robot_nav_msgs + robot_nav_msgs + + robot_xmlrpcpp + robot_xmlrpcpp + + diff --git a/plugins/back_up_recovery.cpp b/plugins/back_up_recovery.cpp new file mode 100644 index 0000000..ab71e13 --- /dev/null +++ b/plugins/back_up_recovery.cpp @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * recovery_core — per-cycle backup recovery plugin. + * + * Author: DuongTD + *********************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +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* 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(); + } + +private: + tf3::BufferCore* tf_ = nullptr; + std::vector* 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) diff --git a/plugins/clear_costmap_recovery.cpp b/plugins/clear_costmap_recovery.cpp new file mode 100644 index 0000000..6c5401f --- /dev/null +++ b/plugins/clear_costmap_recovery.cpp @@ -0,0 +1,235 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * recovery_core — no-output clear costmap plugin. + * + * Author: DuongTD + *********************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +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* 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 clearable_layers_default; + clearable_layers_default.emplace_back("obstacles"); + std::vector 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(); + } + +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>* 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& plugin : *plugins) + { + if (!plugin) + { + continue; + } + + const std::string name = leafName(plugin->getName()); + if (clearable_layers_.count(name) == 0) + { + continue; + } + + if (dynamic_cast(plugin.get()) == nullptr) + { + robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.", + name.c_str()); + continue; + } + + clearMap(boost::static_pointer_cast(plugin), + pose.pose.position.x, pose.pose.position.y); + touched_layer = true; + } + + return touched_layer; + } + + void clearMap(const boost::shared_ptr& costmap, + double pose_x, double pose_y) + { + boost::unique_lock 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* 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 clearable_layers_; +}; + +} // namespace recovery_plugins + +BOOST_DLL_ALIAS(recovery_plugins::ClearCostmapRecovery::create, ClearCostmapRecovery) diff --git a/plugins/regen_path_recovery.cpp b/plugins/regen_path_recovery.cpp new file mode 100644 index 0000000..007c7ab --- /dev/null +++ b/plugins/regen_path_recovery.cpp @@ -0,0 +1,84 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * recovery_core — path output recovery plugin. + * + * Author: DuongTD + *********************************************************************/ + +#include + +#include +#include + +#include +#include + +namespace recovery_plugins +{ + +class RegenPathRecovery final : public recovery_core::RecoveryBehavior +{ +public: + RegenPathRecovery() = default; + + void initialize(std::string name, tf3::BufferCore* tf, + std::vector* 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(); + } + +private: + tf3::BufferCore* tf_ = nullptr; + std::vector* 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) diff --git a/plugins/rotate_recovery.cpp b/plugins/rotate_recovery.cpp new file mode 100644 index 0000000..022413a --- /dev/null +++ b/plugins/rotate_recovery.cpp @@ -0,0 +1,152 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * recovery_core — per-cycle rotate recovery plugin. + * + * Author: DuongTD + *********************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +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* 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(); + } + +private: + tf3::BufferCore* tf_ = nullptr; + std::vector* 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) diff --git a/src/recovery_behavior.cpp b/src/recovery_behavior.cpp new file mode 100644 index 0000000..f5b2f80 --- /dev/null +++ b/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 + +namespace recovery_core +{ + +RecoveryResult RecoveryBehavior::computeCommand(double /*dt*/) +{ + return RecoveryResult::Failed(); +} + +} // namespace recovery_core diff --git a/src/recovery_config.cpp b/src/recovery_config.cpp new file mode 100644 index 0000000..5b32bba --- /dev/null +++ b/src/recovery_config.cpp @@ -0,0 +1,102 @@ +/********************************************************************* + * recovery_core — validate + đọc RecoveryConfig. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +#include + +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 diff --git a/src/recovery_types.cpp b/src/recovery_types.cpp new file mode 100644 index 0000000..e738d1e --- /dev/null +++ b/src/recovery_types.cpp @@ -0,0 +1,55 @@ +/********************************************************************* + * recovery_core — factory cho RecoveryResult. + * + * Author: DuongTD + *********************************************************************/ +#include + +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 diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..5286014 --- /dev/null +++ b/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=\"$\" +) diff --git a/test/plugin_loader_contract_test.cpp b/test/plugin_loader_contract_test.cpp new file mode 100644 index 0000000..1043494 --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr(); + +struct PluginConfig +{ + std::string name; + std::string type; +}; +std::vector libraries_; +std::vector creators_; +std::vector name_plugins_; + +std::vector global_path_; + +void expect(bool condition, const std::string& message) +{ + if (!condition) + { + std::cerr << "[FAIL] " << message << std::endl; + std::exit(1); + } +} + +std::vector getListRecoveryPlugins() +{ + +std::vector 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 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(); + p.type = plugin_i["type"].as(); + } + 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 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(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(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<getNameRecoveryBehavior()<