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

Compare commits

base: DuongTD:89add78c7fe740b16551e7d0a217d71b81b0828f
Branches Tags
DuongTD:main
...
compare: DuongTD:30c846038635b46b97fa6b4c45f47ff46d72d559
Branches Tags
DuongTD:main

2 Commits
89add78c7f ... 30c8460386

Author SHA1 Message Date
duongtd
30c8460386 optimal & fix file cmake 2026-08-03 22:34:43 +07:00
duongtd
887bff1b97 optimal & fix file cmake 2026-08-03 22:32:40 +07:00
37 changed files with 4473 additions and 1339 deletions
Expand all files Collapse all files

101
CMakeLists.txt
View File

@@ -55,8 +55,10 @@ if(NOT BUILDING_WITH_CATKIN)
"${CMAKE_CURRENT_SOURCE_DIR}/../../.." "${CMAKE_CURRENT_SOURCE_DIR}/../../.."
) )
# Test/recovery_core -> Test -> AMR_T800 -> src -> <workspace>. Bốn cấp, không phải bảy: bảy là
# di sản từ vị trí cũ của gói và trỏ vào một thư mục không tồn tại.
set(WORKSPACE_DEVEL_LIB_DIR set(WORKSPACE_DEVEL_LIB_DIR
"${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../../devel/lib" "${CMAKE_CURRENT_SOURCE_DIR}/../../../../devel/lib"
) )
@@ -135,6 +137,8 @@ if(NOT BUILDING_WITH_CATKIN)
# ======================================================== # ========================================================
else() else()
# nav_test_harness chỉ dùng cho test (fake clock/costmap/pose/collision), nên nó KHÔNG nằm trong
# catkin_package(CATKIN_DEPENDS) — consumer của recovery_core không phải kéo theo nó.
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
robot_costmap_2d robot_costmap_2d
robot_cpp robot_cpp
@@ -142,6 +146,7 @@ else()
robot_geometry_msgs robot_geometry_msgs
robot_nav_msgs robot_nav_msgs
robot_xmlrpcpp robot_xmlrpcpp
nav_test_harness
) )
@@ -161,10 +166,10 @@ else()
LIBRARIES LIBRARIES
recovery_core recovery_core
recovery_core_wait_recovery
recovery_core_clear_costmap_recovery recovery_core_clear_costmap_recovery
recovery_core_rotate_recovery recovery_core_rotate_recovery
recovery_core_back_up_recovery recovery_core_back_up_recovery
recovery_core_regen_path_recovery
CATKIN_DEPENDS CATKIN_DEPENDS
robot_costmap_2d robot_costmap_2d
@@ -181,7 +186,11 @@ else()
include_directories( include_directories(
include include
)
# Header của dependency vào dạng SYSTEM: gói này build với -Wall -Wextra, mà robot_cpp/costmap_2d
# sinh hàng trăm warning riêng của chúng — để lẫn vào thì warning của chính gói này vô hình.
include_directories(SYSTEM
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${TF3_INCLUDE_DIR} ${TF3_INCLUDE_DIR}
@@ -196,8 +205,16 @@ endif()
add_library(recovery_core SHARED add_library(recovery_core SHARED
src/recovery_types.cpp src/recovery_types.cpp
src/recovery_behavior.cpp src/recovery_behavior.cpp
src/recovery_registry.cpp
# Adapter nối cổng của contract vào costmap thật. Nằm trong lib chính (không phải plugin) vì
# host dựng chúng trực tiếp, không nạp qua Boost.DLL.
adapters/costmap_pose_provider.cpp
adapters/costmap_collision_checker.cpp
) )
target_compile_options(recovery_core PRIVATE -Wall -Wextra)
# ======================================================== # ========================================================
# Core library - Catkin # Core library - Catkin
@@ -306,6 +323,9 @@ function(add_recovery_core_plugin target source)
) )
target_compile_options(${target} PRIVATE -Wall -Wextra)
add_dependencies( add_dependencies(
${target} ${target}
recovery_core recovery_core
@@ -427,6 +447,13 @@ endfunction()
# ======================================================== # ========================================================
# Recovery plugins # Recovery plugins
# ======================================================== # ========================================================
# Bộ recovery DEFAULT của gói. Thứ tự chạy do YAML quyết định (mục 3.4 của NAV_REFACTOR_PLAN.md),
# không phải thứ tự khai báo ở đây.
add_recovery_core_plugin(
recovery_core_wait_recovery
plugins/wait_recovery.cpp
)
add_recovery_core_plugin( add_recovery_core_plugin(
recovery_core_clear_costmap_recovery recovery_core_clear_costmap_recovery
plugins/clear_costmap_recovery.cpp plugins/clear_costmap_recovery.cpp
@@ -442,11 +469,6 @@ add_recovery_core_plugin(
plugins/back_up_recovery.cpp plugins/back_up_recovery.cpp
) )
add_recovery_core_plugin(
recovery_core_regen_path_recovery
plugins/regen_path_recovery.cpp
)
# ======================================================== # ========================================================
# Install - Catkin # Install - Catkin
@@ -547,10 +569,10 @@ else()
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Libraries:") message(STATUS "Libraries:")
message(STATUS " recovery_core") message(STATUS " recovery_core")
message(STATUS " recovery_core_wait_recovery")
message(STATUS " recovery_core_clear_costmap_recovery") message(STATUS " recovery_core_clear_costmap_recovery")
message(STATUS " recovery_core_rotate_recovery") message(STATUS " recovery_core_rotate_recovery")
message(STATUS " recovery_core_back_up_recovery") message(STATUS " recovery_core_back_up_recovery")
message(STATUS " recovery_core_regen_path_recovery")
message(STATUS "Dependencies:") message(STATUS "Dependencies:")
message(STATUS " robot_costmap_2d") message(STATUS " robot_costmap_2d")
message(STATUS " robot_cpp") message(STATUS " robot_cpp")
@@ -569,17 +591,66 @@ endif()
# ======================================================== # ========================================================
# Tests # Tests
# ======================================================== # ========================================================
option( # Đăng ký qua catkin_add_gtest để `catkin_make run_tests` / `ctest` bắt được, đồng thời vẫn chạy
BUILD_RECOVERY_CORE_TESTS # trực tiếp được binary trong devel/lib/recovery_core.
"Build recovery_core tests" if(CATKIN_ENABLE_TESTING AND BUILDING_WITH_CATKIN)
ON
if(NOT COMMAND catkin_add_gtest)
message(FATAL_ERROR "catkin_add_gtest NOT FOUND")
endif()
set(RECOVERY_CORE_TESTS
recovery_lifecycle_test
output_kind_test
goal_semantics_test
timeout_test
wait_recovery_test
backup_safety_test
pose_progress_test
rotate_safety_test
registry_test
) )
foreach(test_name ${RECOVERY_CORE_TESTS})
catkin_add_gtest(${test_name} test/${test_name}.cpp)
if(BUILD_RECOVERY_CORE_TESTS) if(TARGET ${test_name})
set_target_properties(${test_name} PROPERTIES EXCLUDE_FROM_ALL FALSE)
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/CMakeLists.txt) target_compile_options(${test_name} PRIVATE -Wall -Wextra)
add_subdirectory(test)
target_include_directories(${test_name}
PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/test
)
target_include_directories(${test_name} SYSTEM
PRIVATE
${catkin_INCLUDE_DIRS}
)
# catkin_add_gtest dùng target_link_libraries dạng plain, nên phần bổ sung cũng phải plain.
# Plugin KHÔNG được link thẳng: chúng chỉ export alias Boost.DLL và được nạp qua
# RecoveryRegistry, đúng đường mà runtime đi.
target_link_libraries(${test_name}
recovery_core
${catkin_LIBRARIES}
${Boost_LIBRARIES}
yaml-cpp
pthread
${CMAKE_DL_LIBS}
)
add_dependencies(${test_name} ${RECOVERY_CORE_PLUGIN_TARGETS})
# Đường tới cây config test và thư mục .so, để binary tự trỏ đúng chỗ khi chạy qua ctest
# (ctest không mang theo PNKX_NAV_CORE_CONFIG_DIR hay LD_LIBRARY_PATH của shell).
target_compile_definitions(${test_name} PRIVATE
RECOVERY_CORE_TEST_CONFIG_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/config"
RECOVERY_CORE_TEST_LIBRARY_DIR="${CATKIN_DEVEL_PREFIX}/lib"
)
endif() endif()
endforeach()
endif() endif()

473
PLAN.md
View File

@@ -1,430 +1,43 @@
# PLAN - `recovery_core` # Trạng thái `recovery_core`
`recovery_core` là package **interface** cho recovery behavior trong navigation stack ROS-like Nội dung PLAN.md cũ đã được gỡ. Hai lý do:
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. **Đánh số phase trùng nhưng khác nghĩa.** Nó dùng "Phase 1/2/3" cho các mốc riêng của gói, trùng
số với Phase 1–5 của `Test/NAV_REFACTOR_PLAN.md` — cùng một con số chỉ hai thứ khác hẳn nhau,
## 1. Mục Tiêu trong hai file cùng nằm dưới `Test/`, là bẫy đọc nhầm.
2. **Tick sai.** Nó đánh `[x]` cho `RecoveryConfig::validate`, `RecoveryConfig::fromNodeHandle`,
- Cung cấp base class `recovery_core::RecoveryBehavior` cho các hành vi recovery. `MockBehavior` và `interface_contract_test.cpp` — grep cả ba symbol ra 0 hit, và `test/` khi đó
- Dựa trên pattern `robot_nav_core::RecoveryBehavior`, nhưng mở rộng output để bao được 3 nhóm: chỉ có đúng một file. Một checklist nói dối còn tệ hơn không có checklist.
- recovery không output, ví dụ clear/reset costmap;
- recovery sinh vận tốc theo từng cycle, ví dụ rotate/backup; ## Nguồn chuẩn hiện tại
- 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 | Cần gì | Đọc ở đâu |
`roscpp` hoặc ROS master thật. |---|---|
- Giữ core sạch: plugin trả `RecoveryResult`; caller/adapter quyết định publish command, thay | Contract, họ output, bất biến an toàn | [`README.md`](README.md) |
path, clear service hoặc request replan. | Quyết định thiết kế và lý do | [`docs/ARCHITECTURE.md`](docs/ARCHITECTURE.md) |
| Quy tắc bắt buộc khi viết plugin họ vận tốc | [`docs/SAFETY.md`](docs/SAFETY.md) |
## 2. Ranh Giới Thiết Kế | Cách thêm behavior mới | [`docs/PLUGIN_GUIDE.md`](docs/PLUGIN_GUIDE.md) |
| Bối cảnh trong cuộc refactor navigation runtime | `Test/NAV_REFACTOR_PLAN.md`, PHASE 3 |
### 2.1. `recovery_core` chịu trách nhiệm
## Đã xong
- Định nghĩa interface `RecoveryBehavior`.
- Định nghĩa result contract: - Contract: `bool configure(name, ctx, nh)` / `bool start(goal, now)` / `update(now)` / `cancel()`;
- `RecoveryStatus` `outputKind()` khai họ output; toàn bộ state của base là `private`.
- `RecoveryOutputType` - `RecoveryContext` chỉ chứa **cổng** (`PoseProvider`, `CollisionChecker`, `PlanProvider`), không
- `RecoveryResult` chứa con trỏ tới dữ liệu động.
- Cung cấp ngữ cảnh + mục tiêu runtime: - `RecoveryGoal` dùng `std::optional` + `trigger`; bỏ quy ước sentinel "0 = dùng default".
- `RecoveryContext` (tf/costmap/global_path) - `RecoveryRegistry` nạp theo YAML qua Boost.DLL, giữ factory sống đúng vòng đời.
- `RecoveryGoal` (angle/distance/target_pose/params) - `adapters/`: `CostmapPoseProvider`, `CostmapCollisionChecker`.
- Param riêng plugin đọc trong `onConfigure()`; core base không giữ timeout chung. - Bộ mặc định 4 plugin / 5 instance: `wait`, `conservative_reset`, `rotate`, `aggressive_reset`,
- Cung cấp docs/test stub để plugin sau này implement đúng contract. `back_up`. `regen_path_recovery` bị xoá, không thay thế.
- 9 bộ test GTest đăng ký với ctest, chạy trên cây config riêng của gói.
### 2.2. `recovery_core` không chịu trách nhiệm
## Chưa xong
- Không implement recovery cụ thể như clear costmap, rotate, backup, regen path.
- Không publish `cmd_vel`. - **Họ `kPath` chưa có behavior nào.** Contract đã sẵn sàng (`PlanProvider` + `outputKind()` trả
- Không gọi ROS service hoặc action. `kPath`), nhưng một behavior sinh đường thoát thật cần thông tin từ global planner — nằm ngoài
- Không tự collision-check velocity/path output. phạm vi gói này.
- Không trực tiếp nạp plugin bằng Boost.DLL trong core interface. - **`ClearCostmapRecovery` chưa có test tự động.** Nó cần `Costmap2DROBOT` thật (TF + chuỗi layer),
- Không thay thế trực tiếp `robot_nav_core::RecoveryBehavior` trong `move_base` nếu chưa có không dựng được bằng fake của `nav_test_harness`. Hiện chỉ được kiểm bằng đọc code; test tích hợp
adapter riêng. thuộc Phase 5 (kịch bản chạy trên sim).
- **Chưa chạy trên robot thật.** Bật từng behavior một, không bật cả bộ cùng lúc.
## 3. Interface Contract Đã Chốt
### 3.1. Recovery status
```cpp
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.
### 3.2. Output type
```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;
robot_nav_msgs::Path path;
static RecoveryResult Running();
static RecoveryResult Succeeded();
static RecoveryResult Failed();
static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
RecoveryStatus status);
static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
RecoveryStatus status);
};
```
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`.
### 3.4. Recovery behavior
```cpp
class RecoveryBehavior
{
public:
using Ptr = std::shared_ptr<RecoveryBehavior>;
virtual ~RecoveryBehavior() = default;
virtual void initialize(std::string name,
tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
virtual RecoveryResult runBehavior() = 0;
virtual RecoveryResult update();
virtual RecoveryStatus status() const = 0;
protected:
RecoveryBehavior() = default;
};
```
Đ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ó `update()` cho behavior per-cycle.
## 4. Ba Nhóm Recovery
| 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 | `update()` | `RecoveryOutputType::kVelocity` |
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
```text
recovery_core/
├── CMakeLists.txt
├── package.xml
├── README.md
├── PLAN.md
├── include/recovery_core/
│ ├── recovery_behavior.h
│ └── recovery_types.h
├── src/
│ ├── recovery_behavior.cpp
│ └── recovery_types.cpp
├── plugins/
│ ├── clear_costmap_recovery.cpp
│ ├── rotate_recovery.cpp
│ ├── back_up_recovery.cpp
│ └── regen_path_recovery.cpp
├── test/
│ ├── CMakeLists.txt
│ └── plugin_loader_contract_test.cpp
└── docs/
├── ARCHITECTURE.md
├── PLUGIN_GUIDE.md
└── SAFETY.md
```
## 6. Dependencies
Runtime/build dependencies:
- `robot_costmap_2d`
- `robot_cpp`
- `robot_time`
- `robot_geometry_msgs`
- `robot_nav_msgs`
- `tf3`
- `Boost system thread`
Không phụ thuộc:
- `robot_nav_core`
- `roscpp`
- `pluginlib`
## 7. Roadmap
### Phase 1 - Package Skeleton
Trạng thái: **done**.
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`.
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.
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`.
### Phase 2 - Core Contract Implementation
Trạng thái: **done**.
Mục tiêu:
- Hoàn thiện phần logic chung của interface, chưa viết recovery cụ thể.
Work items:
1. [x] Implement `RecoveryResult` factories.
2. [x] Implement `RecoveryConfig::validate`.
3. [x] Implement `RecoveryConfig::fromNodeHandle`.
4. [x] Giữ guard `RecoveryBehavior::update()` fail an toàn khi chưa start.
5. Deferred: helper chạy loop cho behavior velocity chỉ thêm khi Phase 4 integration cần:
- dùng nhịp gọi từ adapter;
- giám sát ngoài core nếu cần giới hạn thời gian;
- 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/plugin reject input không hợp lệ.
- Default `update()` không sinh velocity mù khi lifecycle sai.
- Test cover factory, config validation, default per-cycle behavior, mock lifecycle.
Verify commands:
```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
```
### Phase 3 - Plugin Implementations
Trạng thái: **done**.
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.
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`, `control_period`;
- [x] dùng tích phân theo `control_period` 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`, `control_period`;
- [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.
Boost.DLL convention:
```cpp
class RotateRecovery : public recovery_core::RecoveryBehavior
{
public:
static recovery_core::RecoveryBehavior::Ptr create()
{
return std::make_shared<RotateRecovery>();
}
};
BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, rotate_recovery)
```
Loader side:
```cpp
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
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);
```
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.
Verify commands:
```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
```
### Phase 4 - Adapter / Integration
Trạng thái: **pending**.
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.
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 `update`;
- 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 hoặc giám sát ngoài core báo lỗi.
Acceptance:
- Core vẫn không publish.
- Plugin vẫn chỉ trả `RecoveryResult`.
- Adapter có safety gate trước velocity command.
- Failure path luôn trả stop command hoặc abort rõ ràng.
## 8. Safety Requirements
- 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.
- Input `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
### Package DoD
- `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 hoặc adapter hủy recovery.
- 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.

138
README.md
View File

@@ -1,69 +1,121 @@
# recovery_core # recovery_core
Interface (base class) cho các hành vi **recovery** của navigation stack ROS-like T800. Interface tick-based cho **recovery behavior** của navigation stack, kèm bộ behavior mặc định.
## Phạm vi ## Phạm vi
- Là **package định nghĩa interface**, mô phỏng `robot_nav_core::RecoveryBehavior` nhưng mở - Định nghĩa contract `recovery_core::RecoveryBehavior` — vòng đời hướng-goal, mỗi control cycle
rộng thêm `global_path`: `initialize(name, tf, global_path, global_costmap, local_costmap)`; một lời gọi, behavior có thể **phát vận tốc**.
đồng thời **tổng quát hoá output** để bao 3 họ recovery. - Cung cấp `RecoveryRegistry` nạp behavior theo YAML qua **Boost.DLL** (`library_path`), giữ đúng
- **Không chạy roscpp/ROS master thật**; dùng bình thường lớp ROS-like `robot_*` vòng đời `.so`.
(`robot_costmap_2d`, `tf3`, `robot_cpp`, `robot_time`, `robot_geometry_msgs`, - Kèm **bộ recovery mặc định**: wait, clear costmap, rotate, back up.
`robot_nav_msgs`). - Không chạy roscpp/ROS master; dùng lớp ROS-like `robot_*` (`robot_costmap_2d`, `tf3`, `robot_cpp`,
- Core interface không publish trực tiếp. Package hiện có thêm các plugin mẫu build thành `robot_time`, `robot_geometry_msgs`, `robot_nav_msgs`).
`.so` riêng và nạp qua **Boost.DLL** để kiểm chứng contract end-to-end.
## Vòng đời hướng-goal ## Vòng đời
```cpp ```cpp
behavior->configure(name, ctx); // 1 lần: ctx = {tf, global_path, global/local costmap} recovery_core::RecoveryContext ctx;
RecoveryGoal goal; goal.angle = 1.57; // mục tiêu RUNTIME: "quay 90 độ ngay lượt này" ctx.pose = &pose_provider; // bắt buộc cho họ velocity
RecoveryResult r = behavior->start(goal); ctx.collision = &collision_checker; // bắt buộc cho họ velocity
while (r.status == RecoveryStatus::kRunning) ctx.local_costmap = local_costmap; // non-owning, làm mới trước mỗi lượt
r = behavior->update(); // publish r.command; đọc r.progress / r.remaining / r.message
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
registry.loadFromConfig(nh, "recovery", ctx);
auto* behavior = registry.at(0);
recovery_core::RecoveryGoal goal;
goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;
if (behavior->start(goal, clock.now())) // trả bool; KHÔNG sinh tick
{
for (;;)
{
const auto r = behavior->update(clock.now()); // dt do base đo THẬT
if (const auto* cmd = r.velocity()) // nullptr nếu behavior không thuộc họ velocity
publish(*cmd);
if (r.terminal())
break;
}
}
``` ```
API công khai `configure/start/update/cancel` là non-virtual (base lo guard vòng đời/cancel); API công khai `configure` / `start` / `update` / `cancel` là **non-virtual**: base giữ toàn bộ bất
plugin chỉ override hook `onConfigure()/onStart(goal)/onUpdate()`. biến, plugin chỉ triển khai hook `onConfigure` / `onStart` / `onUpdate` / `onCancel`.
## Ba họ recovery ## Ba họ output
| Họ | Ví dụ | Output | Hook chính | Mỗi behavior khai **một lần** qua `outputKind()`; base cưỡng chế mọi kết quả trả về phải khớp, nên
|----|-------|--------|-----------| caller route theo `output_type` được mà không cần tin lời hứa trong tài liệu.
| A | regen path (đường thoát) | `robot_nav_msgs::Path` | `onUpdate()` one-shot |
| B | clear costmap | không có (chỉ status) | `onUpdate()` one-shot |
| C | rotation / backup | `robot_geometry_msgs::Twist` mỗi cycle | `onStart(goal)` + `onUpdate()` |
Cả 3 chia sẻ một `RecoveryResult` hợp nhất mang cờ `output_type` + rich feedback | Họ | Behavior mặc định | Output | Cổng bắt buộc trong context |
(`progress`/`remaining`/`elapsed`/`message`). |----|-------------------|--------|------------------------------|
| `kNone` | `WaitRecovery`, `ClearCostmapRecovery` | không | — (clear costmap cần con trỏ costmap) |
| `kVelocity` | `RotateRecovery`, `BackUpRecovery` | `robot_geometry_msgs::Twist` mỗi cycle | `PoseProvider` + `CollisionChecker` |
| `kPath` | (chưa có) | `robot_nav_msgs::Path` | `PlanProvider` |
Đọc dữ liệu qua `result.velocity()` / `result.pathOut()` — trả `nullptr` nếu họ không khớp, nên
không thể đọc nhầm trường của họ khác.
## Bộ recovery mặc định
Thứ tự trong `recovery_behaviors_params.yaml` **chính là hành vi**: caller thử từ đầu danh sách,
hỏng thì sang cái kế tiếp. Sắp từ nhẹ tới nặng.
| # | Instance | Plugin | Làm gì |
|---|----------|--------|--------|
| 0 | `wait` | `WaitRecovery` | Đứng yên `wait_duration` giây. An toàn nhất — không di chuyển, không cần pose. Với AMR trong kho, phần lớn tình huống chặn đường là vật cản động và cách này giải quyết được đa số |
| 1 | `conservative_reset` | `ClearCostmapRecovery` | Xoá vật cản đã tích trong vùng `reset_distance` quanh robot |
| 2 | `rotate` | `RotateRecovery` | Quay tại chỗ (mặc định đủ 2π) cho costmap quan sát lại xung quanh. Quét footprint qua **toàn bộ cung** trước khi quay |
| 3 | `aggressive_reset` | `ClearCostmapRecovery` | Ngược lại: giữ vùng gần, xoá tất cả phần còn lại |
| 4 | `back_up` | `BackUpRecovery` | Lùi `backup_distance` mét. Xếp **cuối** vì lùi là hướng robot thường không có sensor |
Không có behavior "chỉ xin lập plan lại": state machine của `move_base2` đã tự lập plan lại sau mọi
lượt recovery kết thúc, nên một behavior như vậy chỉ chiếm chỗ mà không làm gì.
## Bất biến an toàn
Base bảo đảm, không phụ thuộc plugin có nhớ hay không:
- **Tiến độ đo bằng pose thật.** `update(now)` cấp `dt` đo từ đồng hồ thật, không phải chu kỳ cấu
hình. Plugin họ velocity đo quãng đi bằng hình chiếu delta pose, nên control loop chạy chậm không
làm robot đi quá quãng, và bánh trượt không bị báo nhầm là hoàn thành.
- **Không có pose thì dừng.** `PoseProvider::getRobotPose()` trả `false` → `kFailed` + Twist 0.
- **Không lái mù.** Họ velocity bắt buộc có `CollisionChecker`; thiếu thì `configure()` trả `false`.
- **NaN/Inf không ra được cmd_vel.** Lệnh không hữu hạn bị đổi thành lệnh dừng + `kFailed`.
- **`elapsed` và `timeout`.** Base đo và ép; quá hạn là `kFailed` + stop output.
- **Stop output đúng họ.** Họ velocity nhận Twist 0 tường minh (caller đang lấy cmd_vel từ đó); họ
khác nhận `kNone` — base không bịa output vận tốc cho behavior không lái.
Chi tiết trong [docs/SAFETY.md](docs/SAFETY.md).
## Cấu trúc ## Cấu trúc
``` ```
include/recovery_core/ recovery_types.h, recovery_behavior.h include/recovery_core/ recovery_context.h, recovery_types.h, recovery_behavior.h,
src/ phần chung của contract (types + base lifecycle) recovery_registry.h, recovery_math.h, adapters/
plugins/ clear_costmap, rotate, backup, regen_path plugin mẫu src/ base lifecycle + types + registry
test/ contract test + Boost.DLL loader test adapters/ CostmapPoseProvider, CostmapCollisionChecker (nối vào costmap thật)
examples/ minimal_recovery.cpp plugins/ wait, clear_costmap, rotate, back_up — bộ mặc định, mỗi cái một .so
docs/ ARCHITECTURE.md, PLUGIN_GUIDE.md, SAFETY.md test/ 9 bộ test GTest + cây config riêng
docs/ ARCHITECTURE.md, SAFETY.md, PLUGIN_GUIDE.md
``` ```
## Build ## Build và test
Hỗ trợ **catkin** và **standalone CMake** (như `robot_clear_costmap_recovery`).
```bash ```bash
# catkin (trong workspace)
catkin_make --pkg recovery_core catkin_make --pkg recovery_core
# standalone # Đăng ký với ctest, nên chạy được cả hai đường:
mkdir build && cd build && cmake .. && make cd build && ctest -R recovery_core --output-on-failure
# hoặc gọi trực tiếp
./devel/lib/recovery_core/backup_safety_test
``` ```
## Trạng thái Tham số vận hành: `pnkx_nav_core/config/recovery_behaviors_params.yaml`.
Bản dùng cho test: `test/config/recovery_behaviors_params.yaml` — **đừng** sửa tham số vận hành ở
đây.
- [x] Phase 1 — khung package (interface + stub, build lib rỗng). Viết plugin mới: [docs/PLUGIN_GUIDE.md](docs/PLUGIN_GUIDE.md).
- [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.

124
adapters/costmap_collision_checker.cpp Normal file
View File

@@ -0,0 +1,124 @@
/*********************************************************************
* recovery_core — CollisionChecker dựng trên Costmap2DROBOT.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/adapters/costmap_collision_checker.h>
#include <algorithm>
#include <cmath>
#include <utility>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/footprint.h>
namespace recovery_core
{
void CostmapCollisionChecker::setFootprintOverride(
std::vector<robot_geometry_msgs::Point> footprint)
{
footprint_override_ = std::move(footprint);
}
void CostmapCollisionChecker::setSampleStep(double step_m)
{
if (std::isfinite(step_m) && step_m > 0.0)
{
sample_step_ = step_m;
}
}
double CostmapCollisionChecker::pointCost(const robot_costmap_2d::Costmap2DROBOT& costmap,
double wx, double wy) const
{
// Lấy lại con trỏ lưới MỖI LẦN: LayeredCostmap có thể thay Costmap2D bên dưới giữa hai cycle.
const robot_costmap_2d::Costmap2D* grid = costmap.getCostmap();
if (grid == nullptr)
{
return kOutsideMap;
}
unsigned int mx = 0;
unsigned int my = 0;
if (!grid->worldToMap(wx, wy, mx, my))
{
return kOutsideMap;
}
const unsigned char cost = grid->getCost(mx, my);
if (cost == robot_costmap_2d::NO_INFORMATION)
{
return kUnknown;
}
// INSCRIBED_INFLATED_OBSTACLE nghĩa là tâm robot đặt ở đây thì đã chạm vật cản — chặn như lethal.
if (cost == robot_costmap_2d::LETHAL_OBSTACLE ||
cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
{
return kLethal;
}
return static_cast<double>(cost);
}
double CostmapCollisionChecker::lineCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double x0,
double y0, double x1, double y1) const
{
const double length = std::hypot(x1 - x0, y1 - y0);
const int steps = std::max(1, static_cast<int>(std::ceil(length / sample_step_)));
double worst = 0.0;
for (int i = 0; i <= steps; ++i)
{
const double t = static_cast<double>(i) / static_cast<double>(steps);
const double cost = pointCost(costmap, x0 + t * (x1 - x0), y0 + t * (y1 - y0));
if (cost < 0.0)
{
return cost; // Bất kỳ mã lỗi nào cũng chặn ngay, không đi tiếp trên đoạn này.
}
worst = std::max(worst, cost);
}
return worst;
}
double CostmapCollisionChecker::footprintCost(double x, double y, double theta) const
{
if (costmap_ == nullptr)
{
// Không có costmap thì không khẳng định được chỗ này đi được — trả về "chặn" chứ không phải 0.
return kOutsideMap;
}
const std::vector<robot_geometry_msgs::Point>& spec =
footprint_override_.empty() ? costmap_->getRobotFootprint() : footprint_override_;
if (spec.size() < 3)
{
// Footprint suy biến -> coi robot là một điểm, giống CostmapModel.
return pointCost(*costmap_, x, y);
}
std::vector<robot_geometry_msgs::Point> oriented;
robot_costmap_2d::transformFootprint(x, y, theta, spec, oriented);
double worst = 0.0;
for (std::size_t i = 0; i < oriented.size(); ++i)
{
const std::size_t j = (i + 1) % oriented.size();
const double cost =
lineCost(*costmap_, oriented[i].x, oriented[i].y, oriented[j].x, oriented[j].y);
if (cost < 0.0)
{
return cost;
}
worst = std::max(worst, cost);
}
return worst;
}
} // namespace recovery_core

25
adapters/costmap_pose_provider.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*********************************************************************
* recovery_core — PoseProvider dựng trên Costmap2DROBOT.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/adapters/costmap_pose_provider.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
namespace recovery_core
{
bool CostmapPoseProvider::getRobotPose(robot_geometry_msgs::PoseStamped& pose) const
{
if (costmap_ == nullptr)
{
return false;
}
// getRobotPose() đã kiểm transform_tolerance bên trong; trả false nghĩa là TF thiếu hoặc quá hạn.
// Không ghi vào `pose` khi thất bại — bên gọi phải dừng an toàn chứ không dùng pose cũ.
return costmap_->getRobotPose(pose);
}
} // namespace recovery_core

124
docs/ARCHITECTURE.md
View File

@@ -1,46 +1,106 @@
# Kiến Trúc recovery_core # Kiến trúc recovery_core
## Vị trí trong stack ## Vị trí
`recovery_core` nằm ở tầng `Navigations/Libraries`, cạnh `robot_clear_costmap_recovery`. Nó Gói nằm ở `Test/recovery_core`. Nó định nghĩa interface recovery **duy nhất** của workspace
giữ vai trò interface tương tự `robot_nav_core::RecoveryBehavior` cho các recovery cần **trả (`CLAUDE.md`); bản legacy `robot_nav_core::RecoveryBehavior` đã dừng phát triển và không được port
về output** (path/vận tốc), đồng thời vẫn dùng global path/costmap/tf như bản gốc. sang.
## Các thành phần Consumer đầu tiên là `RecoveryRunner` của `move_base2` — nó include thẳng
`recovery_core/recovery_behavior.h`, nên contract được compiler kiểm đầy đủ ở một nơi.
- `RecoveryBehavior` (interface, template-method): API non-virtual `configure`/`start`/`update`/ ## Vì sao tick-based
`cancel`; plugin chỉ triển khai hook `onConfigure`/`onStart(goal)`/`onUpdate()`.
- `RecoveryContext`: gói con trỏ ngữ cảnh (tf/costmap/global_path) truyền một lần qua `configure`. Recovery thế hệ trước chạy blocking bên trong một lời gọi và không trả gì. Hệ quả: không cancel
- `RecoveryGoal`: mục tiêu RUNTIME mỗi lượt (angle/distance/target_pose/params). được giữa chừng, không báo tiến độ, và **không phát được vận tốc** — nên mọi behavior cần robot cử
- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ + động phải tự quay vòng lặp bên trong, tranh quyền phát `cmd_vel` với controller.
rich feedback (`progress`/`remaining`/`elapsed`/`message`), thêm trạng thái `kCancelled`.
- Plugin mẫu: Bản này trả kết quả từng cycle. Ràng buộc kéo theo: recovery phải được tick từ **đúng thread đang
- `ClearCostmapRecovery`: clear layer costmap theo tên, one-shot no-output. sở hữu cmd_vel**, không được có thread riêng.
- `RotateRecovery`: quay tới `goal.angle` (rad); sinh `Twist.angular.z` mỗi cycle.
- `BackUpRecovery`: lùi tới `goal.distance` (m); sinh `Twist.linear.x < 0` mỗi cycle. ## Thành phần
- `RegenPathRecovery`: trả lại `robot_nav_msgs::Path` từ `global_path` hiện tại.
| Thành phần | Vai trò |
|---|---|
| `RecoveryBehavior` | Base template-method. Giữ toàn bộ bất biến; plugin chỉ triển khai hook |
| `RecoveryContext` | Các **cổng** môi trường: `PoseProvider`, `CollisionChecker`, `PlanProvider`, con trỏ costmap |
| `RecoveryGoal` | Mục tiêu runtime mỗi lượt: `trigger`, `angle`, `distance`, `target_pose`, `params` |
| `RecoveryResult` | Kết quả một tick: status, output theo họ, progress/remaining/elapsed/message |
| `RecoveryRegistry` | Danh sách behavior **có thứ tự**, nạp từ YAML qua Boost.DLL |
| `adapters/` | `CostmapPoseProvider`, `CostmapCollisionChecker` — nối cổng vào costmap thật |
| `plugins/` | Bộ mặc định: wait, clear costmap, rotate, back up |
## Ai giữ bất biến gì
Đây là điểm khác quan trọng nhất so với bản trước, nơi state là `protected` và plugin ghi thẳng
được vào `status_`.
**Base giữ:**
- guard vòng đời: `start` sau `configure`, `update` sau `start`, `configure` chỉ một lần;
- cổng bắt buộc theo `outputKind()` — họ velocity không có `PoseProvider`/`CollisionChecker` thì
`configure()` trả `false` ngay, chứ không để plugin phát hiện lúc đang lái;
- validate goal (NaN/Inf, `distance > 0`) trước khi plugin nhìn thấy;
- đo `dt` và `elapsed` bằng đồng hồ **thật**, ép `timeout`;
- cưỡng chế `output_type ∈ {outputKind(), kNone}`;
- chặn NaN/Inf trong lệnh vận tốc;
- sinh **stop output đúng họ** ở mọi nhánh guard/cancel/timeout.
**Plugin giữ:** đọc param riêng, chốt mục tiêu lượt này, và sinh một tick an toàn. Toàn bộ state của
base là `private`; plugin truy cập qua accessor `const`.
## Luồng runtime ## Luồng runtime
``` ```
configure(name, ctx) // 1 lần: cache ctx, đọc config chung, onConfigure() configure(name, ctx, nh) # 1 lần. Kiểm cổng bắt buộc, đọc timeout, gọi onConfigure(nh).
│ │
start(goal) // mỗi lượt: chốt mục tiêu runtime, onStart() start(goal, now) # mỗi lượt. Validate goal, mốc thời gian, gọi onStart(goal) -> bool.
│ │
├── one-shot (họ A/B): update() 1 lần ───────────► RecoveryResult{status, path|none, msg} ├── kNone : update() -> làm việc / đếm giờ ─────► {status, elapsed, message}
├── kVelocity : loop { update(now) } ───────────────► {status, Twist, progress, remaining}
└── kPath : update() -> sinh path ──────────────► {status, Path}
│ │
└── per-cycle (họ C): loop { update() } ────────► RecoveryResult{status, velocity, cancel() # tick kế tiếp gọi onCancel() -> stop output + kCancelled.
(tới khi status != kRunning) progress, remaining}
``` ```
## Ghi chú thiết kế ## Quyết định thiết kế
- Template-method: base xử lý guard vòng đời (configure→start→update) và `cancel` một chỗ; **Cổng, không phải dữ liệu, nằm trong context.** `PlanProvider` là accessor chứ không phải con trỏ
plugin không lặp lại các guard này. tới `std::vector` vì runtime dùng triple-buffer plan và **xoay con trỏ** giữa ba bộ đệm — một con
- Mục tiêu là RUNTIME qua `RecoveryGoal` (không cố định trong config): cùng plugin phục vụ nhiều trỏ cache lúc `configure()` sẽ trỏ vào bộ đệm scratch sau vài chu kỳ planner. Cùng lý do đó, con trỏ
yêu cầu góc/khoảng khác nhau. Field = 0 → dùng default plugin đọc ở `onConfigure()`. costmap phải được caller làm mới trước mỗi lượt và plugin không được cache `Costmap2D*` bên trong.
- `onUpdate()` lấy pose robot từ costmap/tf bên trong; không truyền pose qua tham số.
- Sau `cancel()`, base tự trả stop output (Twist 0) + `kCancelled`. **`std::optional` thay cho sentinel 0.** `goal.angle = 0` là yêu cầu hợp lệ ("đừng quay") và phải
- Param RIÊNG của plugin đọc trong `onConfigure()`. phân biệt được với "caller không đặt". Quy ước cũ "0 nghĩa là dùng default" khiến một góc tính từ
- Interface KHÔNG include Boost.DLL; export/import là việc của plugin/loader. hình học ra ~0 bị âm thầm thay bằng π/2.
- Plugin mẫu có dùng Boost.DLL alias, nhưng core contract vẫn không biết loader/adaptor.
**Họ output nằm trong kiểu.** `outputKind()` khai một lần và base cưỡng chế. Trước kia mỗi kết quả
tự mang `output_type` nên nó không dùng để route được: một behavior họ path báo `kVelocity` ở tick
đầu, còn base thì sinh `Velocity(zero)` cho mọi họ ở nhánh terminal.
**Param do caller cấp namespace.** `configure()` nhận `robot::NodeHandle&` đã scope sẵn; plugin
không tự dựng `NodeHandle("~/" + name)` và tự nạp YAML từ disk. Nhờ vậy test chỉ cần trỏ vào cây
config của mình, không phải dựng cây config production.
**Không có `RecoveryDirective`.** Từng cân nhắc thêm kênh "xin lập plan lại", nhưng state machine
của `move_base2` đã chuyển `RECOVERING → PLANNING` với `start_planner = true` trên **cả**
`kSucceeded` lẫn `kFailed` — replan sau recovery đã là hành vi mặc định, thêm kênh riêng là thêm thứ
không ai đọc.
**`kCancelled` không qua ranh giới port.** `move_base2` không tick recovery sau khi cancel (state
chuyển sang `CANCELLING`, nơi mọi nguồn vận tốc bị khoá). Đường `onCancel()` vẫn phải đúng vì nó là
hàng rào cho host khác, nhưng `RecoveryTick` của `move_base2` không cần giá trị tương ứng.
## Điều kiện đóng lại phần legacy
`robot_nav_core::RecoveryBehavior` và `robot_clear_costmap_recovery` chỉ còn được tham chiếu ở hai
nơi, và cả hai biến mất cùng lúc khi `move_base2` thay xong `move_base` cũ:
| Nơi | Xử lý |
|---|---|
| `move_base/src/move_base.cpp` (loader gen-1) | Bị `move_base2` thay, không sửa |
| `robot_clear_costmap_recovery` | Bị `ClearCostmapRecovery` gen-2 thay, không port |
Khoá `recovery_behaviors:` trong `move_base_common_params.yaml` đã được gỡ: các entry cũ trỏ tên
alias vào `.so` gen-2 trong khi loader ở đó import theo chữ ký gen-1, và Boost.DLL không kiểm kiểu
qua ranh giới `.so`.

218
docs/PLUGIN_GUIDE.md
View File

@@ -1,110 +1,160 @@
# Hướng Dẫn Viết Plugin recovery_core # Viết một recovery behavior mới
Package hiện có 4 plugin mẫu dưới `plugins/`: ## 1. Chọn họ output
- `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 Quyết định đầu tiên và không đổi được về sau: `outputKind()`.
1. Kế thừa `recovery_core::RecoveryBehavior`. | Họ | Khi nào | Cổng bắt buộc trong context |
2. Override hook `onConfigure()` (tuỳ chọn) — đọc param riêng qua `robot::NodeHandle("~/" + name)`; |---|---|---|
ngữ cảnh tf/global_path/costmap lấy qua `ctx()`. | `kNone` | Behavior không lái robot (đợi, xoá costmap, gọi thiết bị ngoài) | — |
3. Override `onStart(goal)` + `onUpdate()` theo họ (xem dưới). KHÔNG override | `kVelocity` | Behavior tự lái từng cycle | `PoseProvider` + `CollisionChecker` |
`configure/start/update/cancel` — base đã lo guard vòng đời/cancel. | `kPath` | Behavior sinh ra đường đi mới | `PlanProvider` |
4. Thêm factory `static RecoveryBehaviorPtr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`.
## Vòng đời (goal-driven) Base kiểm cổng theo họ ngay ở `configure()`, nên một behavior họ velocity thiếu collision checker sẽ
**không nạp được**, thay vì phát hiện lúc đang lái.
``` ## 2. Khung plugin
configure(name, ctx) // 1 lần: cache ctx, đọc config chung, gọi onConfigure()
│
start(goal) // mỗi lượt: chốt mục tiêu RUNTIME (angle/distance/pose), gọi onStart()
│
loop update() // mỗi cycle tới khi status != kRunning; base guard vòng đời/cancel
│
[cancel()] // update() kế tiếp -> stop output + kCancelled
```
`RecoveryGoal` là điểm mấu chốt: cùng plugin, mỗi lượt caller đặt `goal.angle` (rad) hay
`goal.distance` (m) khác nhau; field = 0 nghĩa là dùng default đã cấu hình. Override thêm truyền
qua `goal.params` (vd `goal.params["angular_speed"] = 0.8`).
## Override theo họ
| Họ | Override | Trả về |
|----|----------|--------|
| A. path | `onUpdate()` (one-shot) | `RecoveryResult::PathOut(path, kSucceeded)` |
| B. none | `onUpdate()` (one-shot) | `RecoveryResult::Succeeded()` / `Failed()` |
| C. velocity | `onStart()` chốt goal + `onUpdate()` mỗi cycle | `RecoveryResult::Velocity(twist, kRunning\|kSucceeded)` |
Mọi kết quả nên gắn feedback qua `.withProgress(progress, remaining)` và `.withMessage(...)` để
caller giám sát tiến độ (progress ∈ [0,1], remaining theo rad/m).
## Export bằng Boost.DLL (bắt buộc cho plugin)
```cpp ```cpp
#include <recovery_core/recovery_behavior.h> #include <recovery_core/recovery_behavior.h>
#include <boost/dll/alias.hpp> #include <recovery_core/recovery_math.h>
namespace recovery_plugins { #include <boost/dll/alias.hpp>
class SpinRecovery : public recovery_core::RecoveryBehavior { #include <robot/robot.h>
public:
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { namespace recovery_plugins
return std::make_shared<SpinRecovery>(); {
namespace
{
constexpr double kDefaultLimit = 1.0; // [m] đơn vị ghi ngay tại chỗ khai báo
} }
class MyRecovery final : public recovery_core::RecoveryBehavior
{
public:
MyRecovery() = default;
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<MyRecovery>();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kVelocity;
}
protected: protected:
// override onConfigure()/onStart(goal)/onUpdate()... // nh ĐÃ được caller scope vào namespace param của instance này — đọc khoá phẳng.
bool onConfigure(robot::NodeHandle& nh) override
{
nh.param("limit", limit_, kDefaultLimit);
if (!std::isfinite(limit_) || limit_ <= 0.0)
{
robot::log_warning("[recovery_core] '%s': limit=%.3f không hợp lệ; dùng %.3f.",
name().c_str(), limit_, kDefaultLimit);
limit_ = kDefaultLimit;
}
return true; // false = không chạy được; registry bỏ behavior này và log đích danh
}
// Chốt mục tiêu lượt này + kiểm điều kiện an toàn để khởi động. KHÔNG sinh tick ở đây.
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
if (!ctx().pose->getRobotPose(start_pose_))
return false; // không biết robot ở đâu -> từ chối khởi động
target_ = goal.distance.value_or(limit_);
return true;
}
// dt là thời gian THẬT tính từ tick trước; tick đầu ngay sau start() có dt = 0.
recovery_core::RecoveryResult onUpdate(const robot::Time& now, double dt) override
{
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
return stopResult(recovery_core::RecoveryStatus::kFailed).withMessage("mất pose robot");
const double done = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
if (done >= target_)
return stopResult(recovery_core::RecoveryStatus::kSucceeded).withProgress(1.0, 0.0);
robot_geometry_msgs::Twist cmd;
// ... tính cmd, nhớ ramp theo acc_lim và kiểm collision ở pose dự đoán ...
return recovery_core::RecoveryResult::Velocity(cmd, recovery_core::RecoveryStatus::kRunning)
.withProgress(done / target_, target_ - done);
}
// Tuỳ chọn: cơ hội giảm tốc thay vì nhảy thẳng về 0.
recovery_core::RecoveryResult onCancel() override
{
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("cancelled");
}
private:
double limit_ = kDefaultLimit; ///< [m]
double target_ = kDefaultLimit; ///< [m]
double start_yaw_ = 0.0; ///< [rad]
robot_geometry_msgs::PoseStamped start_pose_;
}; };
} // namespace recovery_plugins } // namespace recovery_plugins
// alias = `type` dùng trong YAML recovery_behaviors. BOOST_DLL_ALIAS(recovery_plugins::MyRecovery::create, MyRecovery)
BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)
``` ```
## Nạp phía loader (adapter/caller — không nằm trong recovery_core) Những gì **không** phải viết: guard `configured_`/`started_`, kiểm cancel, đo `elapsed`, ép
`timeout`, đặt `output_type` cho stop output, kiểm NaN của lệnh vận tốc. Base làm hết — và giữ được
vì toàn bộ state của nó là `private`.
```cpp ## 3. Đăng ký build
#include <boost/dll/import.hpp>
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(
path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations);
recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = loader();
recovery_core::RecoveryContext ctx; ```cmake
ctx.tf = tf; ctx.global_path = global_path; add_recovery_core_plugin(
ctx.global_costmap = global_costmap; ctx.local_costmap = local_costmap; recovery_core_my_recovery
behavior->configure(name, ctx); plugins/my_recovery.cpp
)
recovery_core::RecoveryGoal goal;
goal.angle = 1.57; // "quay 90 độ ngay lượt này"
recovery_core::RecoveryResult r = behavior->start(goal);
while (r.status == recovery_core::RecoveryStatus::kRunning) {
r = behavior->update(); // publish r.command; đọc r.progress/r.remaining/r.message
}
``` ```
Lưu ý: adapter/test phải giữ handle `.so` sống lâu hơn object plugin. Nếu library bị unload trong Thêm tên target vào `catkin_package(LIBRARIES ...)` để consumer khác dùng lại được.
khi object plugin còn tồn tại, virtual call qua vtable của plugin có thể crash.
## CMake cho plugin ## 4. Khai trong YAML
- `find_package(Boost REQUIRED COMPONENTS system filesystem)` ```yaml
- link `${Boost_LIBRARIES}`, `${CMAKE_DL_LIBS}`, `recovery_core` recovery:
- `set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON)` behaviors:
- build shared library, tên library + symbol khớp `type`; install `.so` nơi loader tìm. - {name: my_instance, type: MyRecovery} # thứ tự trong danh sách CHÍNH LÀ thứ tự thử
my_instance:
limit: 0.5 # [m]
timeout: 10.0 # [s] base đọc; 0 = không giới hạn
## Test plugin MyRecovery:
library_path: librecovery_core_my_recovery # THIẾU KHOÁ NÀY LÀ LỖI PHỔ BIẾN NHẤT
```bash
catkin_make --pkg recovery_core
./devel/lib/recovery_core/recovery_core_plugin_loader_test
``` ```
Standalone: Tên alias trong `BOOST_DLL_ALIAS` phải khớp `type`. Namespace param của instance là `<ns>/<name>` —
registry dựng `NodeHandle` đó và truyền vào `configure()`; plugin **không** tự đi tìm config trên
disk. Đó là lý do test chỉ cần trỏ vào cây config của mình là chạy được.
```bash Một plugin có thể có **nhiều instance** với tham số khác nhau — bộ mặc định dùng
cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase3_build `ClearCostmapRecovery` hai lần (`conservative_reset` và `aggressive_reset`).
make -C /tmp/recovery_core_phase3_build -j4
/tmp/recovery_core_phase3_build/test/recovery_core_plugin_loader_test ## 5. Test
```
Đặt trong `test/`, thêm tên vào danh sách `RECOVERY_CORE_TESTS` của `CMakeLists.txt` (dùng
`catkin_add_gtest`, nên `ctest -R recovery_core` bắt được).
Dùng `recovery_test::VelocityRig` (costmap giả + pose giả + collision checker giả + đồng hồ giả) và
nạp plugin **qua `RecoveryRegistry`** để đi đúng đường Boost.DLL mà runtime dùng — không link thẳng
`.so` vào test.
Tối thiểu phải phủ:
- từ chối khởi động khi điều kiện an toàn không thoả;
- mất pose giữa chừng → `kFailed` + lệnh dừng;
- loop chạy chậm (dt gấp 5–20 lần nhịp thường) vẫn dừng đúng chỗ;
- cancel → lệnh dừng;
- param ngoài dải → dùng default, không nhận giá trị sai.
Cuối cùng, kiểm rằng test **fail được**: gỡ `.so` khỏi `devel/lib` rồi chạy lại. Phải đỏ. Bộ test cũ
của gói này in `[PASS]` khi không nạp được plugin nào — đó là thứ phải tránh.

80
docs/SAFETY.md
View File

@@ -1,30 +1,64 @@
# An Toàn — recovery_core # 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. > Đọc trước khi viết plugin họ vận tốc. Recovery chạy đúng lúc robot đã ở tình huống xấu, nên
> default ở đây phải fail-safe chứ không fail-open.
## recovery_core KHÔNG đảm bảo ## Base bảo đảm
- **Không collision-check tự động.** Interface không kiểm tra va chạm khi sinh vận tốc Những điều dưới đây đúng kể cả khi plugin viết ẩu — base cưỡng chế, không phụ thuộc plugin nhớ.
(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 `update()` đú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).
- **Guard vòng đời ở base.** `start()` trước `configure()`, hay `update()` trước `start()`, đều trả
`RecoveryResult::Failed()`. Sau `cancel()`, base trả stop output (Twist 0) + `kCancelled`.
## Nguyên tắc cho plugin | Bất biến | Cơ chế |
|---|---|
| Họ velocity không chạy được nếu thiếu pose hoặc collision checker | `configure()` trả `false` khi `ctx.pose`/`ctx.collision` null |
| `dt` là thời gian **thật**, không phải chu kỳ cấu hình | `update(now)` tính `now - lần trước`; đồng hồ đi lùi → `dt = 0` |
| `elapsed` luôn có mặt trên mọi kết quả | Base ghi, plugin không phải đặt |
| Recovery không treo vô hạn | Param `timeout` [s]; quá hạn → `kFailed` + stop output |
| `output_type` luôn thuộc `{outputKind(), kNone}` | Sai họ → hạ về `kNone`, xoá dữ liệu, log lỗi |
| NaN/Inf không ra được `cmd_vel` | Lệnh không hữu hạn → Twist 0 + `kFailed` |
| Guard vòng đời không cần plugin tự làm | `start` trước `configure`, `update` trước `start` → `kFailed` + stop output |
| Stop output đúng họ | Họ velocity nhận **Twist 0 tường minh**; họ khác nhận `kNone` |
- Guard costmap/tf null (lấy qua `ctx()`) trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`. ## Base KHÔNG bảo đảm
- Không cần tự guard vòng đời/cancel — base đã lo; plugin tập trung logic recovery.
- 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 - **Không thay caller quyết định có nên recovery hay không.** Ngữ cảnh cho phép recovery (đã dừng
planner, không đang ở chế độ thủ công, không đang estop) là việc của caller.
- **Không quản lý nhịp gọi.** Caller phải tick đúng chu kỳ control và publish lệnh.
- **Không là hàng rào vận tốc cuối cùng.** Plugin clamp theo giới hạn của mình; `VelocityArbiter`
phía ngoài vẫn phải clamp lần cuối. Hai tầng, không chồng nhau: plugin lập **kế hoạch** dừng/ramp,
arbiter **chặn** giá trị vượt ngưỡng.
- Đảm bảo ngữ cảnh cho phép recovery (vd: đã dừng planner, vùng xung quanh đủ an toàn). ## Quy tắc cho plugin họ vận tốc
- Áp timeout/giám sát ngoài để tránh recovery chạy vô hạn.
1. **Không tích phân vận tốc lệnh để đo tiến độ.** Dùng `PoseProvider` và hình chiếu delta pose
(`recovery_core::projectOntoHeading`). Tích phân lệnh thì bánh trượt hay robot bị chặn vẫn báo đi
đủ quãng — và đó là lúc nguy hiểm nhất để nói dối.
2. **Kiểm va chạm trên pose *dự đoán*, trước khi phát lệnh.** Không phải sau. `BackUpRecovery` dò
pose ở cuối chu kỳ tới; `RotateRecovery` quét toàn bộ cung ngay tại `onStart()` và refuse khởi
động nếu có bất kỳ góc nào bị chặn.
3. **Mất pose là dừng.** `getRobotPose()` trả `false` → trả `kFailed` + Twist 0. Không dùng pose cũ.
4. **Ramp theo `acc_lim_*`.** Tick đầu không được nhảy thẳng lên tốc độ tối đa; `onCancel()` cũng
nên giảm tốc thay vì nhảy bậc.
5. **Clamp mọi param có dải.** `angle` ∈ [-2π, 2π], `distance` ∈ (0, `*_max`], tốc độ ≤ trần. Param
sai thì log cảnh báo **kèm giá trị** rồi dùng default — không im lặng.
6. **Ghi rõ dấu.** `linear.x < 0` là lùi; `angular.z > 0` là ngược chiều kim đồng hồ. Param cấu hình
là **độ lớn**, dấu do plugin đặt theo ngữ cảnh.
## Quy tắc chung cho mọi plugin
- `onUpdate()` chạy trên thread phát `cmd_vel`. **Không block quá một phần nhỏ chu kỳ control**:
không parse file, không cấp phát lớn, không gọi việc nặng của costmap (`ClearCostmapRecovery` cố
ý không gọi `updateMap()` vì lý do này).
- **Không cache `Costmap2D*`** qua các tick. Con trỏ có thể bị thay giữa hai cycle; lấy lại mỗi lần
dùng.
- Kiểm mã trả về của `CollisionChecker` bằng `< 0`, **không** so với một giá trị âm cụ thể — các
hiện thực trong workspace không thống nhất mã lỗi.
- `configure()` trả `false` khi cấu hình không chạy được, thay vì log warning rồi chạy tiếp bằng
default. Registry sẽ bỏ behavior đó và log đích danh.
## Trách nhiệm của caller
- Làm mới con trỏ costmap trong `RecoveryContext` trước mỗi `start()`/`update()`.
- Tick recovery từ **đúng một thread** — thread sở hữu `cmd_vel`. Thread riêng cho recovery nghĩa là
hai bộ điều khiển cùng phát vận tốc.
- Chỉ gọi `update()` sau khi `start()` trả `true`.
- Sau `cancel()`, hoặc tick tiếp để nhận stop output, hoặc tự khoá nguồn vận tốc về 0.

92
include/recovery_core/adapters/costmap_collision_checker.h Normal file
View File

@@ -0,0 +1,92 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — CollisionChecker dựng trên Costmap2DROBOT.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_
#define RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_
#include <vector>
#include <robot_geometry_msgs/Point.h>
#include <recovery_core/recovery_context.h>
namespace robot_costmap_2d { class Costmap2DROBOT; }
namespace recovery_core
{
/**
* @class CostmapCollisionChecker
* @brief Cost của footprint tại một pose giả định, quét trên costmap hiện hành.
*
* Mã trả về khớp **chính xác** `nav_test_harness::FakeCollisionChecker` để code tiêu thụ không đổi
* cách đọc giữa test và runtime:
* - `-1` một điểm của footprint nằm ngoài bản đồ;
* - `-2` một điểm rơi vào `LETHAL_OBSTACLE` hoặc `INSCRIBED_INFLATED_OBSTACLE`;
* - `-3` một điểm rơi vào `NO_INFORMATION` (chưa biết → coi là không đi được);
* - `>= 0` cost lớn nhất dọc biên footprint.
*
* @note Cố ý **không** bọc `robot_base_local_planner::CostmapModel`: lớp đó cache một tham chiếu
* costmap trong constructor (đúng thứ CLAUDE.md cấm), và mã lỗi của nó ngược với harness
* (-1 lethal / -3 ngoài bản đồ), nên bọc nó sẽ làm runtime và test bất đồng đúng ở chỗ khó
* thấy nhất.
*
* @warning Chỉ quét **biên** đa giác footprint, không quét lòng trong — giống `CostmapModel`. Ở
* runtime inflation layer làm vật cản nở ra nên biên luôn chạm; trong test thì kịch bản
* phải tự đặt vật cản chạm biên.
*
* Con trỏ costmap **non-owning**; lớp này không cache `Costmap2D*` bên trong mà lấy lại mỗi lần gọi
* — costmap có thể bị thay giữa hai cycle, và cache chính là nguyên nhân lỗi double-free đã ghi
* nhận trong workspace.
*/
class CostmapCollisionChecker final : public CollisionChecker
{
public:
/// Mã lỗi, khớp `nav_test_harness::FakeCollisionChecker`.
static constexpr double kOutsideMap = -1.0;
static constexpr double kLethal = -2.0;
static constexpr double kUnknown = -3.0;
explicit CostmapCollisionChecker(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
: costmap_(costmap)
{
}
/// @brief Trỏ lại vào costmap hiện hành. Không sở hữu.
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
{
costmap_ = costmap;
}
/**
* @brief Ép dùng footprint này thay cho footprint của costmap.
* @param footprint Đa giác trong frame robot [m]. Rỗng = quay lại dùng footprint của costmap.
*/
void setFootprintOverride(std::vector<robot_geometry_msgs::Point> footprint);
/// @brief Bước lấy mẫu dọc cạnh footprint [m]. Bỏ qua giá trị <= 0.
void setSampleStep(double step_m);
double footprintCost(double x, double y, double theta) const override;
private:
/// @return cost tại một điểm toàn cục, hoặc mã lỗi âm.
double pointCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double wx, double wy) const;
/// @return cost lớn nhất dọc đoạn thẳng, hoặc mã lỗi âm đầu tiên gặp phải.
double lineCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double x0, double y0, double x1,
double y1) const;
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; ///< non-owning
std::vector<robot_geometry_msgs::Point> footprint_override_;
double sample_step_ = 0.025; ///< [m]
};
} // namespace recovery_core
#endif // RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_

54
include/recovery_core/adapters/costmap_pose_provider.h Normal file
View File

@@ -0,0 +1,54 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — PoseProvider dựng trên Costmap2DROBOT.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_
#define RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_
#include <recovery_core/recovery_context.h>
namespace robot_costmap_2d { class Costmap2DROBOT; }
namespace recovery_core
{
/**
* @class CostmapPoseProvider
* @brief Lấy pose robot qua `Costmap2DROBOT::getRobotPose` (đã gồm tra TF + transform_tolerance).
*
* Con trỏ costmap là **non-owning** và có thể bị thay giữa hai lượt recovery — dùng @ref setCostmap
* để caller làm mới trước mỗi `start()`/`update()` thay vì giữ một bản cache trong context.
*/
class CostmapPoseProvider final : public PoseProvider
{
public:
explicit CostmapPoseProvider(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
: costmap_(costmap)
{
}
/// @brief Trỏ lại vào costmap hiện hành. Không sở hữu.
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
{
costmap_ = costmap;
}
/**
* @copydoc PoseProvider::getRobotPose
*
* Trả false khi chưa có costmap hoặc TF không cho ra pose — hai trường hợp đều nghĩa là "không
* biết robot ở đâu", và plugin phải dừng an toàn.
*/
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override;
private:
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; ///< non-owning
};
} // namespace recovery_core
#endif // RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_

177
include/recovery_core/recovery_behavior.h
View File

@@ -4,10 +4,8 @@
* *
* recovery_core — interface (base class) cho recovery behaviors. * recovery_core — interface (base class) cho recovery behaviors.
* *
* Vòng đời hợp nhất, hướng-goal cho cả 3 họ recovery (path / none / velocity): * Vòng đời tick-based, hướng-goal:
* configure(name, ctx) -> start(goal) -> lặp update() -> [cancel()] * configure(name, ctx, nh) -> start(goal, now) -> lặp update(now) -> [cancel()]
* Base lo phần chung (guard init/cancel, đẩy feedback); plugin chỉ
* override các hook nhỏ onConfigure()/onStart()/onUpdate().
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
@@ -16,12 +14,11 @@
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector>
#include <tf3/buffer_core.h> #include <robot/node_handle.h>
#include <robot_costmap_2d/costmap_2d_robot.h> #include <robot/time.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_types.h> #include <recovery_core/recovery_types.h>
namespace recovery_core namespace recovery_core
@@ -29,60 +26,82 @@ namespace recovery_core
/** /**
* @class RecoveryBehavior * @class RecoveryBehavior
* @brief Interface hướng-goal cho mọi hành vi recovery (không chạy roscpp/ROS master thật). * @brief Interface hướng-goal, tick-based cho mọi hành vi recovery.
* *
* Thiết kế theo template-method: API công khai (configure/start/update/cancel) là NON-VIRTUAL * Template-method: API công khai (`configure`/`start`/`update`/`cancel`) là NON-VIRTUAL và base giữ
* và do base xử lý phần lặp lại; plugin chỉ triển khai các hook protected: * toàn bộ bất biến; plugin chỉ triển khai hook protected. Khác bản trước ở chỗ base **thật sự** giữ
* - onConfigure() : đọc param riêng từ ctx()/NodeHandle (tuỳ chọn). * được bất biến — mọi state là `private`, plugin không ghi vào được.
* - onStart(goal) : chốt mục tiêu lượt này (rad/m/pose), reset trạng thái tiến độ.
* - onUpdate() : một "tick"; họ one-shot (path/clear) hoàn tất ngay lần đầu.
* *
* Ba họ hành vi: * Base chịu trách nhiệm:
* - Họ A (trả path) : onUpdate() trả RecoveryResult::PathOut(...), kSucceeded ngay. * - guard vòng đời (`start` sau `configure`, `update` sau `start`);
* - Họ B (không output) : onUpdate() làm việc rồi trả Succeeded()/Failed() ngay. * - kiểm ngữ cảnh bắt buộc theo @ref outputKind (họ velocity phải có pose + collision checker);
* - Họ C (trả vận tốc) : onUpdate() sinh Twist mỗi cycle tới khi đạt goal -> kSucceeded. * - validate goal (NaN/Inf) trước khi giao cho plugin;
* - đo `elapsed` và ép `timeout`;
* - cưỡng chế `output_type` của mọi kết quả thuộc `{outputKind(), kNone}`;
* - sinh **stop output đúng họ** khi guard/cancel/timeout — họ velocity nhận Twist 0 tường minh,
* họ khác nhận `kNone` (base không bịa ra output vận tốc cho behavior không lái).
* *
* Bất biến: start() chỉ hợp lệ sau configure(); update() chỉ chạy sau start(). Vi phạm -> * Plugin chịu trách nhiệm: đọc param riêng, chốt mục tiêu lượt này, và sinh một tick an toàn.
* RecoveryResult::Failed(). Base tự trả stop output + kCancelled sau khi cancel(). *
* @note Không thread-safe. Recovery được tick từ đúng thread sở hữu cmd_vel; hai thread cùng phát
* vận tốc là hai bộ điều khiển tranh nhau.
*/ */
class RecoveryBehavior class RecoveryBehavior
{ {
public: public:
/// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace /// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace.
/// (boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(...)).
using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>; using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>;
virtual ~RecoveryBehavior() = default; virtual ~RecoveryBehavior() = default;
RecoveryBehavior(const RecoveryBehavior&) = delete;
RecoveryBehavior& operator=(const RecoveryBehavior&) = delete;
// ------------------------------------------------------------------ // ------------------------------------------------------------------
// API công khai — NON-VIRTUAL, base xử lý phần chung. // API công khai — NON-VIRTUAL.
// ------------------------------------------------------------------ // ------------------------------------------------------------------
/** /**
* @brief Cấu hình một lần: cache ngữ cảnh (không sở hữu), đọc config chung, gọi onConfigure(). * @brief Cấu hình một lần.
* @param name Tên instance (namespace param + log). * @param name Tên instance, dùng cho log.
* @param ctx Ngữ cảnh môi trường (tf/costmap/global_path). Gọi lại lần 2 bị bỏ qua. * @param ctx Các cổng môi trường. Base kiểm cổng bắt buộc theo @ref outputKind.
* @param nh NodeHandle **đã được caller scope sẵn vào namespace param của instance này**.
* Plugin không tự dựng NodeHandle từ disk — nếu tự dựng thì không test được mà không
* có cây config thật, và đó là lý do bộ test cũ phải đọc config production.
* @return false nếu thiếu cổng bắt buộc, param không hợp lệ, hoặc gọi lần thứ hai.
*/ */
void configure(const std::string& name, const RecoveryContext& ctx); bool configure(const std::string& name, const RecoveryContext& ctx, robot::NodeHandle& nh);
/** /**
* @brief Bắt đầu một lượt recovery với mục tiêu RUNTIME. * @brief Bắt đầu một lượt recovery.
* @param goal Mục tiêu lượt này (góc/khoảng lùi/pose + override). Field 0 = dùng default plugin. * @param goal Mục tiêu lượt này. Trường không đặt = dùng default của plugin.
* @return Kết quả tick đầu (thường kRunning; kFailed nếu chưa configure hoặc goal không hợp lệ). * @param now Thời điểm hiện tại — mốc cho `elapsed` và `timeout`.
* @return false nếu chưa configure, goal không hợp lệ, hoặc plugin từ chối khởi động (ví dụ cung
* sẽ quay đã bị chặn). **Không sinh tick ở đây** — tick chỉ ra từ `update()`.
*/ */
RecoveryResult start(const RecoveryGoal& goal); bool start(const RecoveryGoal& goal, const robot::Time& now);
/** /**
* @brief Một control cycle. Guard chưa start/cancel trước khi gọi onUpdate(). * @brief Một control cycle.
* @param now Thời điểm hiện tại. Base tự tính `dt = now - lần update trước`.
*
* Gọi sau khi lượt đã kết thúc thì trả lại đúng trạng thái kết thúc kèm stop output, không tick
* thêm. Gọi khi chưa `start()` thì trả `kFailed` + stop output.
*/ */
RecoveryResult update(); RecoveryResult update(const robot::Time& now);
/** /**
* @brief Yêu cầu huỷ: update() kế tiếp trả stop output + kCancelled. * @brief Yêu cầu dừng. Tick kế tiếp trả stop output + `kCancelled`.
*
* @note Trong `move_base2`, state machine không tick recovery sau khi cancel (state chuyển sang
* `CANCELLING`, nơi mọi nguồn vận tốc bị khoá), nên `kCancelled` thường không bao giờ đến
* tay caller. Đường này vẫn phải đúng: nó là hàng rào cho host khác và cho tương lai.
*/ */
void cancel(); void cancel();
/// @brief Trạng thái hiện tại của lượt recovery. /// @brief Họ output của behavior. Khai một lần; base dùng để cưỡng chế bất biến.
virtual RecoveryOutputType outputKind() const = 0;
RecoveryStatus status() const RecoveryStatus status() const
{ {
return status_; return status_;
@@ -93,10 +112,16 @@ public:
return name_; return name_;
} }
/// Giữ tên cũ cho tương thích call-site loader. /// @brief [s] thời gian trôi từ `start()`. 0 nếu chưa start.
std::string getNameRecoveryBehavior() const double elapsed() const
{ {
return name_; return elapsed_;
}
/// @brief [s] trần thời gian một lượt; 0 = không giới hạn.
double timeout() const
{
return timeout_;
} }
protected: protected:
@@ -106,23 +131,79 @@ protected:
// Hook cho plugin. // Hook cho plugin.
// ------------------------------------------------------------------ // ------------------------------------------------------------------
/// @brief Đọc param riêng (qua ctx()/NodeHandle) sau khi base cache ngữ cảnh. Tuỳ chọn. /**
virtual void onConfigure() {} * @brief Đọc param riêng của plugin từ @p nh (đã scope sẵn).
* @return false nếu param không hợp lệ tới mức không chạy được. Sai nhẹ thì log + dùng default.
*/
virtual bool onConfigure(robot::NodeHandle& nh) = 0;
/// @brief Chốt mục tiêu lượt này; reset bộ đếm tiến độ nội bộ. Trả tick khởi đầu. /**
virtual RecoveryResult onStart(const RecoveryGoal& goal) = 0; * @brief Chốt mục tiêu lượt này, reset bộ đếm tiến độ, kiểm điều kiện an toàn để khởi động.
* @return false = từ chối khởi động; base đặt status về kFailed.
*/
virtual bool onStart(const RecoveryGoal& goal) = 0;
/// @brief Một tick. Họ one-shot trả kSucceeded/kFailed ngay; họ velocity trả kRunning tới goal. /**
virtual RecoveryResult onUpdate() = 0; * @brief Một tick.
* @param now Thời điểm hiện tại.
* @param dt [s] khoảng cách tới tick trước, đo **thật** chứ không phải chu kỳ cấu hình.
* Luôn >= 0; tick đầu sau `start()` có dt = 0.
*
* @warning Không được block quá một phần nhỏ của chu kỳ control — nó chạy trên thread phát
* cmd_vel. Không parse file, không cấp phát lớn, không gọi việc nặng của costmap.
*/
virtual RecoveryResult onUpdate(const robot::Time& now, double dt) = 0;
// Truy cập cho plugin (chỉ đọc ngữ cảnh/goal). /**
const RecoveryContext& ctx() const { return ctx_; } * @brief Cơ hội phát lệnh giảm tốc trước khi dừng hẳn.
const RecoveryGoal& goal() const { return goal_; } *
bool cancelRequested() const { return cancel_requested_; } * Default trả stop output ngay. Plugin họ velocity đang chạy nhanh nên override để ramp về 0 thay
* vì nhảy bậc — dù `VelocityArbiter` phía ngoài có clamp gia tốc, kế hoạch dừng là việc của
* plugin, không phải của hàng rào cuối.
*/
virtual RecoveryResult onCancel();
// ------------------------------------------------------------------
// Truy cập chỉ-đọc cho plugin.
// ------------------------------------------------------------------
const RecoveryContext& ctx() const
{
return ctx_;
}
const RecoveryGoal& goal() const
{
return goal_;
}
bool cancelRequested() const
{
return cancel_requested_;
}
/// @brief Stop output đúng họ của behavior này, kèm @p status.
RecoveryResult stopResult(RecoveryStatus status) const;
private:
/// Kiểm ngữ cảnh có đủ cổng bắt buộc cho họ output này không.
bool validateContext() const;
/// Kiểm goal không mang NaN/Inf và distance > 0 nếu được đặt.
bool validateGoal(const RecoveryGoal& goal) const;
/// Ép bất biến output + gắn elapsed lên kết quả plugin trả về.
RecoveryResult finalize(RecoveryResult result) const;
RecoveryContext ctx_; RecoveryContext ctx_;
RecoveryGoal goal_; RecoveryGoal goal_;
std::string name_; std::string name_;
robot::Time start_time_;
robot::Time last_update_;
double elapsed_ = 0.0; ///< [s]
double timeout_ = 0.0; ///< [s], 0 = không giới hạn
bool configured_ = false; bool configured_ = false;
bool started_ = false; bool started_ = false;
bool cancel_requested_ = false; bool cancel_requested_ = false;

126
include/recovery_core/recovery_context.h Normal file
View File

@@ -0,0 +1,126 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — ngữ cảnh môi trường cấp cho recovery behavior.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_RECOVERY_CONTEXT_H_
#define RECOVERY_CORE_RECOVERY_CONTEXT_H_
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
// Forward declare: contract không kéo header costmap vào mọi plugin.
namespace robot_costmap_2d { class Costmap2DROBOT; }
namespace recovery_core
{
/**
* @enum RecoveryTrigger
* @brief Vì sao caller vào recovery.
*
* Khai riêng ở đây thay vì dùng `move_base2::RecoveryTrigger` vì chiều phụ thuộc là một chiều:
* `move_base2` biết `recovery_core`, không được ngược lại. `RecoveryRunner` map 1:1 hai enum bằng
* `switch`, nên compiler bắt được ngay khi một bên thêm giá trị mới.
*/
enum class RecoveryTrigger
{
kUnspecified, ///< Caller không nói lý do.
kPlanningFailed, ///< Không lập được plan trong thời gian cho phép.
kControllingFailed, ///< Không sinh được lệnh vận tốc hợp lệ trong thời gian cho phép.
kOscillation ///< Robot quẩn tại chỗ quá lâu.
};
const char* toString(RecoveryTrigger trigger);
/**
* @class PoseProvider
* @brief Nguồn pose robot trong global frame.
*
* Runtime: bọc `Costmap2DROBOT::getRobotPose` (đã gồm tra TF và kiểm `transform_tolerance`).
* Test: fake bơm pose theo kịch bản.
*
* @invariant Trả `false` nghĩa là **không biết robot đang ở đâu** — TF thiếu, TF quá hạn, hoặc
* frame chưa sẵn sàng. Plugin PHẢI dừng an toàn, tuyệt đối không dùng pose cũ để đi
* tiếp. @p pose không được ghi khi hàm trả `false`.
*/
class PoseProvider
{
public:
virtual ~PoseProvider() = default;
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0;
};
/**
* @class CollisionChecker
* @brief Kiểm va chạm theo footprint tại một pose giả định.
*
* @warning Bên gọi chỉ được kiểm `< 0`. **Không so sánh với một giá trị âm cụ thể**: các hiện thực
* trong workspace không thống nhất mã lỗi (`robot_base_local_planner::CostmapModel` dùng
* -1 cho lethal và -3 cho ngoài bản đồ; `nav_test_harness::FakeCollisionChecker` dùng
* -1 cho ngoài bản đồ và -2 cho lethal). Mọi giá trị âm đều mang đúng một nghĩa:
* **không đặt robot ở đây được**.
*/
class CollisionChecker
{
public:
virtual ~CollisionChecker() = default;
/**
* @param x,y Vị trí giả định trong global frame [m].
* @param theta Hướng giả định [rad].
* @return `< 0` nghĩa là không đặt được (va chạm / ngoài bản đồ / unknown);
* `>= 0` là cost lớn nhất gặp phải dọc biên footprint.
*/
virtual double footprintCost(double x, double y, double theta) const = 0;
};
/**
* @class PlanProvider
* @brief Nguồn global plan hiện hành.
*
* Cố ý là **accessor**, không phải con trỏ cache trong context: runtime dùng mô hình triple-buffer
* và **xoay con trỏ** giữa ba bộ đệm plan, nên một con trỏ lấy lúc `configure()` sẽ trỏ vào bộ đệm
* đang giữ vai trò scratch sau vài chu kỳ planner — không phải plan đang chạy.
*/
class PlanProvider
{
public:
virtual ~PlanProvider() = default;
/**
* @param[out] out Plan hiện hành trong global frame. Chỉ được ghi khi hàm trả `true`.
* @return false nếu chưa có plan nào.
*/
virtual bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const = 0;
};
/**
* @struct RecoveryContext
* @brief Các cổng môi trường cấp cho behavior một lần qua `configure()`.
*
* Chỉ chứa **cổng**, không chứa dữ liệu động. Cổng nào bắt buộc là tuỳ họ output của behavior —
* `RecoveryBehavior::configure()` kiểm và từ chối nếu thiếu, thay vì để plugin phát hiện lúc chạy.
*
* Con trỏ costmap là **non-owning và có thể bị thay** giữa hai lượt: caller (`RecoveryRunner`) làm
* mới context trước mỗi `start()`/`update()`. Plugin không được cache `Costmap2D*` bên trong qua
* các tick.
*/
struct RecoveryContext
{
const PoseProvider* pose = nullptr; ///< Bắt buộc cho họ velocity.
const CollisionChecker* collision = nullptr; ///< Bắt buộc cho họ velocity.
const PlanProvider* plan = nullptr; ///< Bắt buộc cho họ path.
robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr; ///< non-owning.
robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr; ///< non-owning.
};
} // namespace recovery_core
#endif // RECOVERY_CORE_RECOVERY_CONTEXT_H_

91
include/recovery_core/recovery_math.h Normal file
View File

@@ -0,0 +1,91 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — helper hình học dùng chung cho plugin.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_RECOVERY_MATH_H_
#define RECOVERY_CORE_RECOVERY_MATH_H_
#include <cmath>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Quaternion.h>
namespace recovery_core
{
/// @brief Yaw [rad] của quaternion, trong (-pi, pi].
inline double yawFromQuaternion(const robot_geometry_msgs::Quaternion& q)
{
const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
return std::atan2(siny_cosp, cosy_cosp);
}
/// @brief Yaw [rad] của một pose.
inline double yawOf(const robot_geometry_msgs::PoseStamped& pose)
{
return yawFromQuaternion(pose.pose.orientation);
}
/// @brief Đưa góc về (-pi, pi].
inline double normalizeAngle(double angle)
{
while (angle > M_PI)
{
angle -= 2.0 * M_PI;
}
while (angle <= -M_PI)
{
angle += 2.0 * M_PI;
}
return angle;
}
/**
* @brief Quãng đường robot đã đi **dọc theo một hướng cho trước**, tính từ pose xuất phát.
*
* Đây là cách đo tiến độ đúng cho họ velocity: hình chiếu delta pose lên hướng ban đầu. Tích phân
* vận tốc *lệnh* thì bánh trượt hay bị chặn vẫn báo đi đủ quãng.
*
* @param current,start Hai pose trong cùng global frame.
* @param heading Hướng chiếu [rad].
* @return [m] Dương nghĩa là đã đi theo chiều @p heading; âm là đi ngược lại.
*/
inline double projectOntoHeading(const robot_geometry_msgs::PoseStamped& current,
const robot_geometry_msgs::PoseStamped& start, double heading)
{
const double dx = current.pose.position.x - start.pose.position.x;
const double dy = current.pose.position.y - start.pose.position.y;
return dx * std::cos(heading) + dy * std::sin(heading);
}
/**
* @brief Kẹp một lượng vận tốc theo trần gia tốc.
* @param target Vận tốc mong muốn (độ lớn, >= 0).
* @param current Vận tốc đang phát (độ lớn, >= 0).
* @param acc_lim Trần gia tốc [đơn vị/s^2]; <= 0 nghĩa là không giới hạn.
* @param dt [s] khoảng cách tới tick trước.
* @return Độ lớn vận tốc được phép phát ở tick này.
*/
inline double rampToward(double target, double current, double acc_lim, double dt)
{
if (acc_lim <= 0.0 || dt <= 0.0)
{
return target;
}
const double max_step = acc_lim * dt;
if (target > current)
{
return std::min(target, current + max_step);
}
return std::max(target, current - max_step);
}
} // namespace recovery_core
#endif // RECOVERY_CORE_RECOVERY_MATH_H_

110
include/recovery_core/recovery_registry.h Normal file
View File

@@ -0,0 +1,110 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — nạp và giữ danh sách recovery behavior theo YAML.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_RECOVERY_REGISTRY_H_
#define RECOVERY_CORE_RECOVERY_REGISTRY_H_
#include <cstddef>
#include <functional>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
namespace recovery_core
{
/**
* @class RecoveryRegistry
* @brief Danh sách behavior **có thứ tự**, nạp từ YAML bằng Boost.DLL.
*
* Thứ tự chính là hành vi: caller thử behavior 0 trước, hỏng thì sang 1, hết danh sách thì abort.
* Vì thế đây là `vector` chứ không phải bảng tra theo tên.
*
* Cấu hình mong đợi (xem `pnkx_nav_core/config/recovery_behaviors_params.yaml`):
*
* @code{.yaml}
* recovery:
* behaviors:
* - {name: wait, type: WaitRecovery}
* - {name: rotate, type: RotateRecovery}
* wait:
* wait_duration: 3.0
* rotate:
* angular_speed: 0.4
*
* WaitRecovery: { library_path: librecovery_core_wait_recovery }
* RotateRecovery: { library_path: librecovery_core_rotate_recovery }
* @endcode
*
* Khoá `library_path` là thứ hay quên nhất: thiếu nó thì plugin build xong vẫn báo "không tìm
* thấy". Registry vì thế báo lỗi nêu **đích danh** khoá bị thiếu.
*/
class RecoveryRegistry
{
public:
RecoveryRegistry() = default;
~RecoveryRegistry();
RecoveryRegistry(const RecoveryRegistry&) = delete;
RecoveryRegistry& operator=(const RecoveryRegistry&) = delete;
/**
* @brief Nạp toàn bộ behavior khai trong `<ns>/behaviors`, theo đúng thứ tự khai báo.
* @param nh NodeHandle gốc.
* @param ns Namespace chứa danh sách (mặc định `recovery`).
* @param ctx Ngữ cảnh cấp cho mọi behavior khi `configure()`.
* @return false nếu có bất kỳ behavior nào không nạp được. Các behavior còn lại **vẫn** được giữ
* (một đường phục hồi hỏng không nên xoá sạch các đường còn lại), và mỗi lỗi được log kèm
* lý do cụ thể.
*/
bool loadFromConfig(robot::NodeHandle& nh, const std::string& ns, const RecoveryContext& ctx);
/**
* @brief Thêm một behavior đã dựng sẵn vào cuối danh sách (test, hoặc behavior biên dịch thẳng
* vào host). Caller tự chịu trách nhiệm đã `configure()` nó.
* @return false nếu con trỏ null.
*/
bool registerBehavior(const RecoveryBehavior::RecoveryBehaviorPtr& behavior);
std::size_t size() const
{
return behaviors_.size();
}
/// @brief Behavior thứ @p index, hoặc nullptr nếu index sai.
RecoveryBehavior* at(std::size_t index) const;
/// @brief Tên behavior thứ @p index, hoặc chuỗi rỗng nếu index sai.
std::string nameAt(std::size_t index) const;
void clear();
private:
/// Nạp một behavior. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh,
const RecoveryContext& ctx, const std::string& ns);
std::vector<RecoveryBehavior::RecoveryBehaviorPtr> behaviors_;
/**
* Giữ factory của Boost.DLL sống đúng bằng vòng đời registry.
*
* Đây **không** phải biến thừa: factory nắm `shared_library` bên trong, thả nó ra là `.so` bị
* unload trong khi các behavior tạo từ nó vẫn còn sống — vtable trỏ vào vùng nhớ đã gỡ. Thứ tự
* huỷ trong `clear()` cũng vì lý do đó: behavior chết trước, factory chết sau.
*/
std::vector<std::function<RecoveryBehavior::RecoveryBehaviorPtr()>> factories_;
};
} // namespace recovery_core
#endif // RECOVERY_CORE_RECOVERY_REGISTRY_H_

112
include/recovery_core/recovery_types.h
View File

@@ -2,7 +2,7 @@
* *
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* recovery_core — kiểu hợp đồng output cho recovery behaviors. * recovery_core — kiểu hợp đồng goal/output cho recovery behaviors.
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
@@ -10,16 +10,14 @@
#define RECOVERY_CORE_RECOVERY_TYPES_H_ #define RECOVERY_CORE_RECOVERY_TYPES_H_
#include <map> #include <map>
#include <optional>
#include <string> #include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h> #include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h> #include <robot_geometry_msgs/Twist.h>
#include <robot_nav_msgs/Path.h> #include <robot_nav_msgs/Path.h>
// Forward declare để không kéo header nặng vào contract type. #include <recovery_core/recovery_context.h>
namespace tf3 { class BufferCore; }
namespace robot_costmap_2d { class Costmap2DROBOT; }
namespace recovery_core namespace recovery_core
{ {
@@ -37,55 +35,47 @@ enum class RecoveryStatus
kCancelled ///< Bị caller huỷ giữa chừng (trả stop output). kCancelled ///< Bị caller huỷ giữa chừng (trả stop output).
}; };
const char* toString(RecoveryStatus status);
/** /**
* @enum RecoveryOutputType * @enum RecoveryOutputType
* @brief Loại output hành vi sinh ra ở kết quả hiện tại. * @brief Họ output của behavior. Quyết định trường nào trong RecoveryResult đọc được.
* Quyết định trường nào trong RecoveryResult là hợp lệ để đọc. *
* Đây **không** phải cờ tự do cho từng kết quả: mỗi plugin khai một lần qua
* `RecoveryBehavior::outputKind()`, và base cưỡng chế mọi kết quả trả về phải khớp. Trước kia
* `output_type` do plugin tự đặt mỗi tick nên caller không route theo nó được.
*/ */
enum class RecoveryOutputType enum class RecoveryOutputType
{ {
kNone, ///< Không output (vd: clear costmap) — chỉ đọc status. kNone, ///< Không output (vd: clear costmap, wait) — chỉ đọc status.
kVelocity, ///< Output là command (Twist) — họ rotation/backup. kVelocity, ///< Output là Twist — họ rotate/backup.
kPath ///< Output là path — họ regen path. kPath ///< Output là Path — họ sinh lại đường đi.
}; };
/** const char* toString(RecoveryOutputType kind);
* @struct RecoveryContext
* @brief Ngữ cảnh môi trường cấp cho behavior một lần qua configure().
*
* Gói các con trỏ KHÔNG sở hữu (tf/costmap/global_path) thay cho danh sách tham số dài của
* initialize() cũ. Thêm field mới ở đây không phá vỡ chữ ký configure() của mọi plugin.
*/
struct RecoveryContext
{
tf3::BufferCore* tf = nullptr; ///< Transform buffer.
std::vector<robot_geometry_msgs::PoseStamped>* global_path = nullptr; ///< Plan hiện tại.
robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr; ///< Costmap toàn cục.
robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr; ///< Costmap cục bộ.
};
/** /**
* @struct RecoveryGoal * @struct RecoveryGoal
* @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào start(goal). * @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào `start(goal, now)`.
* *
* Đây là điểm cốt lõi giúp behavior "thông minh" hơn: cùng một plugin, mỗi lượt caller có thể * Cùng một plugin phục vụ nhiều yêu cầu khác nhau mà không phải đổi config.
* yêu cầu góc quay / khoảng lùi khác nhau, thay vì cố định trong config lúc configure().
* *
* Quy ước dùng default: trường mang giá trị 0 (hoặc has_target_pose == false) nghĩa là "dùng * `std::optional` chứ **không** dùng quy ước "0 nghĩa là dùng default": `angle = 0` là một yêu cầu
* default đã cấu hình của plugin". Đơn vị: angle [rad], distance [m]. * hợp lệ ("đừng quay") và phải phân biệt được với "caller không đặt". Bản cũ dùng sentinel 0 nên
* một góc tính ra ~0 từ hình học bị âm thầm thay bằng π/2 — robot quay 90° mà không log gì.
*/ */
struct RecoveryGoal struct RecoveryGoal
{ {
double angle = 0.0; ///< rad — góc quay đích. 0 = dùng default plugin. RecoveryTrigger trigger = RecoveryTrigger::kUnspecified; ///< Vì sao vào recovery.
double velocity = 0.0; ///< m/s — tốc độ di chuyển. 0 = dùng default plugin.
double distance = 0.0; ///< m — khoảng lùi đích. 0 = dùng default plugin.
robot_geometry_msgs::PoseStamped target_pose; ///< Pose đích (họ path/detour), tuỳ chọn. std::optional<double> angle; ///< [rad] có dấu, + là ngược chiều kim đồng hồ.
bool has_target_pose = false; ///< true nếu target_pose hợp lệ. std::optional<double> distance; ///< [m] > 0, độ dài quãng đi (backup: quãng lùi).
std::map<std::string, double> params; ///< Override mở rộng theo từng plugin (vd tốc độ). std::optional<robot_geometry_msgs::PoseStamped> target_pose; ///< Pose đích (họ path).
/// @brief Đọc override double trong params, trả default nếu không có. std::map<std::string, double> params; ///< Override mở rộng theo từng plugin.
/// @brief Đọc override double trong params, trả @p fallback nếu không có.
double param(const std::string& key, double fallback) const double param(const std::string& key, double fallback) const
{ {
const auto it = params.find(key); const auto it = params.find(key);
@@ -95,45 +85,61 @@ struct RecoveryGoal
/** /**
* @struct RecoveryResult * @struct RecoveryResult
* @brief Kết quả hợp nhất + rich feedback cho cả 3 họ recovery. * @brief Kết quả một tick, hợp nhất cho cả ba họ + feedback cho caller giám sát.
* *
* Bất biến output: chỉ đọc trường khớp với @ref output_type. * Bất biến output được **base cưỡng chế**, không chỉ ghi trong comment: `output_type` luôn thuộc
* - kNone : bỏ qua command/path. * `{outputKind(), kNone}`. Đọc dữ liệu qua @ref velocity / @ref pathOut để không thể đọc nhầm
* - kVelocity : dùng command; path để mặc định. * trường của họ khác.
* - kPath : dùng path; command để mặc định.
* *
* Feedback (progress/remaining/elapsed/message) luôn hợp lệ để caller giám sát/log, độc lập với * Feedback (`progress`/`remaining`/`elapsed`/`message`) luôn hợp lệ, độc lập với `output_type`.
* output_type. progress trong [0,1]; remaining theo đơn vị của goal (rad hoặc m). * `progress` trong [0,1]; `remaining` theo đơn vị của goal (rad hoặc m); `elapsed` [s] do base ghi.
*/ */
struct RecoveryResult struct RecoveryResult
{ {
RecoveryStatus status = RecoveryStatus::kRunning; RecoveryStatus status = RecoveryStatus::kRunning;
RecoveryOutputType output_type = RecoveryOutputType::kNone; RecoveryOutputType output_type = RecoveryOutputType::kNone;
robot_geometry_msgs::Twist command; ///< Hợp lệ khi output_type == kVelocity. robot_geometry_msgs::Twist command; ///< [m/s],[rad/s]. Chỉ hợp lệ khi output_type == kVelocity.
robot_nav_msgs::Path path; ///< Hợp lệ khi output_type == kPath. robot_nav_msgs::Path path; ///< Chỉ hợp lệ khi output_type == kPath.
double progress = 0.0; ///< [0,1] tiến độ tới goal. double progress = 0.0; ///< [0,1] tiến độ tới goal.
double remaining = 0.0; ///< Phần còn lại tới goal (rad hoặc m). >= 0. double remaining = 0.0; ///< Phần còn lại tới goal (rad hoặc m). >= 0.
double elapsed = 0.0; ///< s — thời gian trôi từ start(). double elapsed = 0.0; ///< [s] thời gian trôi từ start(). Base ghi, plugin không phải đặt.
std::string message; ///< Mô tả người-đọc-được (vd "rotated 1.20/1.57 rad"). std::string message; ///< Mô tả người-đọc-được, dùng để log khi state đổi.
/// @brief Twist nếu kết quả này thực sự là output vận tốc, ngược lại nullptr.
const robot_geometry_msgs::Twist* velocity() const
{
return output_type == RecoveryOutputType::kVelocity ? &command : nullptr;
}
/// @brief Path nếu kết quả này thực sự là output path, ngược lại nullptr.
const robot_nav_msgs::Path* pathOut() const
{
return output_type == RecoveryOutputType::kPath ? &path : nullptr;
}
/// @brief true nếu lượt recovery đã kết thúc (không cần tick thêm).
bool terminal() const
{
return status != RecoveryStatus::kRunning;
}
/// @brief Đang chạy, không output. /// @brief Đang chạy, không output.
static RecoveryResult Running(); static RecoveryResult Running();
/// @brief Thành công, không output. /// @brief Thành công, không output.
static RecoveryResult Succeeded(); static RecoveryResult Succeeded();
/// @brief Thất bại, không output (caller nên dừng an toàn). /// @brief Thất bại, không output (caller dừng an toàn).
static RecoveryResult Failed(); static RecoveryResult Failed();
/// @brief Bị huỷ, không output (caller nên dừng an toàn). /// @brief Bị huỷ, không output (caller dừng an toàn).
static RecoveryResult Cancelled(); static RecoveryResult Cancelled();
/// @brief Output vận tốc kèm status (kRunning/kSucceeded/kFailed/kCancelled). /// @brief Output vận tốc kèm status.
static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command, static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
RecoveryStatus status); RecoveryStatus status);
/// @brief Output path kèm status. /// @brief Output path kèm status.
static RecoveryResult PathOut(const robot_nav_msgs::Path& path, static RecoveryResult PathOut(const robot_nav_msgs::Path& path, RecoveryStatus status);
RecoveryStatus status);
/// @brief Gắn thêm feedback (fluent) — trả về chính nó để chain. /// @brief Gắn thêm feedback (fluent). progress bị kẹp về [0,1], remaining về >= 0.
RecoveryResult& withProgress(double progress_value, double remaining_value); RecoveryResult& withProgress(double progress_value, double remaining_value);
/// @brief Gắn message (fluent). /// @brief Gắn message (fluent).
RecoveryResult& withMessage(std::string text); RecoveryResult& withMessage(std::string text);

25
package.xml
View File

@@ -2,15 +2,17 @@
<name>recovery_core</name> <name>recovery_core</name>
<version>0.1.0</version> <version>0.1.0</version>
<description> <description>
recovery_core định nghĩa interface (base class) cho các hành vi recovery của navigation Interface tick-based cho recovery behavior, kèm bộ behavior mặc định.
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, Vòng đời hướng-goal: configure(name, ctx, nh) -> start(goal, now) -> lặp update(now) ->
robot_cpp, robot_time). Core library không publish trực tiếp; các behavior mẫu build thành [cancel()]. Mỗi behavior khai một họ output qua outputKind(): không output (wait, clear
plugin Boost.DLL riêng để adapter/caller nạp và tiêu thụ RecoveryResult. costmap), vận tốc theo từng control cycle (rotate, back up), hoặc path. Base giữ toàn bộ bất
biến — guard vòng đời, cổng bắt buộc theo họ, đo elapsed, ép timeout, và cưỡng chế output_type
khớp họ đã khai.
Không chạy roscpp/ROS master; dùng lớp ROS-like robot_* (robot_costmap_2d, tf3, robot_cpp,
robot_time). Behavior mặc định build thành plugin Boost.DLL riêng, nạp qua RecoveryRegistry
theo khoá library_path trong YAML.
</description> </description>
<author>T800 Robotics</author> <author>T800 Robotics</author>
<maintainer email="tdd@gmail.com">T800 Robotics</maintainer> <maintainer email="tdd@gmail.com">T800 Robotics</maintainer>
@@ -39,4 +41,11 @@
<build_depend>robot_xmlrpcpp</build_depend> <build_depend>robot_xmlrpcpp</build_depend>
<run_depend>robot_xmlrpcpp</run_depend> <run_depend>robot_xmlrpcpp</run_depend>
<build_depend>yaml-cpp</build_depend>
<run_depend>yaml-cpp</run_depend>
<!-- Chỉ dùng cho test: fake clock/costmap/pose/collision. Cố ý KHÔNG có run_depend và không nằm
trong catkin_package(CATKIN_DEPENDS) — consumer của recovery_core không phải kéo theo nó. -->
<build_depend>nav_test_harness</build_depend>
</package> </package>

254
plugins/back_up_recovery.cpp
View File

@@ -2,12 +2,13 @@
* *
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* recovery_core — per-cycle backup recovery plugin (goal-driven). * recovery_core — lùi thẳng một quãng, đo bằng pose thật và có kiểm va chạm.
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
#include <recovery_core/recovery_behavior.h> #include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@@ -20,18 +21,30 @@ namespace recovery_plugins
{ {
namespace namespace
{ {
constexpr double kDefaultBackupDistance = 0.5; // m. constexpr double kDefaultBackupDistance = 0.3; // [m]
constexpr double kDefaultLinearSpeed = 0.1; // m/s. constexpr double kDefaultBackupDistanceMax = 1.0; // [m] trần cứng cho quãng lùi
constexpr double kDefaultControlPeriod = 0.1; // s per update tick. constexpr double kDefaultLinearSpeed = 0.1; // [m/s] độ lớn
constexpr double kDefaultAccLimX = 0.3; // [m/s^2]
constexpr double kMaxLinearSpeed = 1.0; // [m/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3; // [m]
} // namespace } // namespace
/** /**
* @class BackUpRecovery * @class BackUpRecovery
* @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal). * @brief Lùi thẳng theo hướng ban đầu tới khi đủ quãng yêu cầu.
* *
* goal.distance (m, > 0) là khoảng lùi lượt này; 0 nghĩa là dùng default configured. Tốc độ * Đây là behavior **nguy hiểm nhất** trong bộ default: lùi là hướng robot thường không có sensor.
* tuyến tính mặc định đọc từ param, có thể override qua goal.params["linear_speed"]. Mỗi * Vì vậy nó xếp cuối danh sách, và có ba lớp bảo vệ:
* update() trả Twist.linear.x < 0 kèm progress/remaining tới khi đủ khoảng -> kSucceeded. *
* 1. **Tiến độ đo bằng pose thật** — hình chiếu delta pose lên hướng xuất phát, không tích phân
* vận tốc lệnh. Bản trước nhân vận tốc lệnh với `control_period` lấy từ config, nên control
* loop chạy chậm gấp đôi là robot lùi gấp đôi quãng yêu cầu, còn bánh trượt thì vẫn báo xong.
* 2. **Kiểm va chạm mỗi tick** trên pose dự đoán ở cuối chu kỳ tới, trước khi phát lệnh; và một
* lần nữa lúc `onStart()` để không bao giờ khởi động vào chỗ đã bị chặn.
* 3. **Mất pose là dừng** — `PoseProvider` trả false thì trả `kFailed` + Twist 0.
*
* Quãng lùi: `goal.distance` cho lượt này, ngược lại param `backup_distance`; cả hai bị kẹp bởi
* `backup_distance_max`.
*/ */
class BackUpRecovery final : public recovery_core::RecoveryBehavior class BackUpRecovery final : public recovery_core::RecoveryBehavior
{ {
@@ -43,101 +56,188 @@ public:
return std::make_shared<BackUpRecovery>(); return std::make_shared<BackUpRecovery>();
} }
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kVelocity;
}
protected: protected:
void onConfigure() override bool onConfigure(robot::NodeHandle& nh) override
{ {
robot::NodeHandle private_nh("~/" + name_); nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
private_nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance); nh.param("backup_distance_max", backup_distance_max_, kDefaultBackupDistanceMax);
private_nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed); nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
private_nh.param("control_period", control_period_, kDefaultControlPeriod); nh.param("acc_lim_x", acc_lim_x_, kDefaultAccLimX);
private_nh.param("require_costmap", require_costmap_, false);
if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0) if (!std::isfinite(backup_distance_max_) || backup_distance_max_ <= 0.0)
{ {
robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.", robot::log_warning("[recovery_core] '%s': backup_distance_max=%.3f m is invalid; using %.3f "
name_.c_str()); "m.", name().c_str(), backup_distance_max_,
default_backup_distance_ = kDefaultBackupDistance; kDefaultBackupDistanceMax);
} backup_distance_max_ = kDefaultBackupDistanceMax;
if (!std::isfinite(default_linear_speed_) || default_linear_speed_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
name_.c_str());
default_linear_speed_ = kDefaultLinearSpeed;
}
if (!std::isfinite(control_period_) || control_period_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
name_.c_str());
control_period_ = kDefaultControlPeriod;
}
} }
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override default_backup_distance_ = clampDistance(default_backup_distance_, kDefaultBackupDistance);
default_linear_speed_ = clampSpeed(default_linear_speed_, kDefaultLinearSpeed);
if (!std::isfinite(acc_lim_x_) || acc_lim_x_ < 0.0)
{ {
// Costmap là bắt buộc? fail sớm trước khi xuất vận tốc lùi. robot::log_warning("[recovery_core] '%s': acc_lim_x=%.3f m/s^2 is invalid; using %.3f.",
if (require_costmap_ && ctx().local_costmap == nullptr) name().c_str(), acc_lim_x_, kDefaultAccLimX);
{ acc_lim_x_ = kDefaultAccLimX;
return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap");
} }
backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0) return true;
? goal.distance
: default_backup_distance_;
linear_speed_ = std::abs(goal.param("linear_speed", default_linear_speed_));
if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
{
linear_speed_ = default_linear_speed_;
} }
traveled_distance_ = 0.0; bool onStart(const recovery_core::RecoveryGoal& goal) override
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), {
recovery_core::RecoveryStatus::kRunning) // Base đã bảo đảm ctx().pose và ctx().collision khác null cho họ velocity.
.withProgress(0.0, backup_distance_) if (!ctx().pose->getRobotPose(start_pose_))
.withMessage("backup start"); {
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
name().c_str());
return false;
} }
recovery_core::RecoveryResult onUpdate() override start_yaw_ = recovery_core::yawOf(start_pose_);
backup_distance_ = clampDistance(goal.distance.value_or(default_backup_distance_),
default_backup_distance_);
linear_speed_ = clampSpeed(std::abs(goal.param("linear_speed", default_linear_speed_)),
default_linear_speed_);
current_speed_ = 0.0;
// Kiểm ngay tại chỗ: nếu vị trí lùi đầu tiên đã bị chặn thì từ chối khởi động, để state machine
// chuyển sang behavior kế tiếp thay vì phát một lệnh lùi rồi mới hỏng.
if (blockedAhead(start_pose_, kProbeDistance))
{ {
if (traveled_distance_ >= backup_distance_) robot::log_warning("[recovery_core] '%s': the space behind is already blocked, refusing to "
{ "back up.",
return succeeded(); name().c_str());
return false;
} }
robot_geometry_msgs::Twist command; return true;
command.linear.x = -std::abs(linear_speed_);
traveled_distance_ = std::min(
backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_);
if (traveled_distance_ >= backup_distance_)
{
return succeeded();
} }
return recovery_core::RecoveryResult::Velocity(command, recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
recovery_core::RecoveryStatus::kRunning) {
.withProgress(traveled_distance_ / backup_distance_, robot_geometry_msgs::PoseStamped pose;
backup_distance_ - traveled_distance_) if (!ctx().pose->getRobotPose(pose))
.withMessage("backing up"); {
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("robot pose lost (TF stale?) — stopping the back-up");
} }
private: // Tiến độ = phần đi ngược hướng xuất phát. Dấu âm của hình chiếu chính là quãng đã lùi.
recovery_core::RecoveryResult succeeded() const double traveled = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
const double remaining = backup_distance_ - traveled;
if (remaining <= kGoalTolerance)
{ {
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), current_speed_ = 0.0;
recovery_core::RecoveryStatus::kSucceeded) return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0) .withProgress(1.0, 0.0)
.withMessage("backup complete"); .withMessage("backup complete");
} }
double default_backup_distance_ = kDefaultBackupDistance; // Vận tốc của tick này, đã ramp theo trần gia tốc, và không vượt quãng còn lại nếu dt cho phép.
double default_linear_speed_ = kDefaultLinearSpeed; double speed = recovery_core::rampToward(linear_speed_, current_speed_, acc_lim_x_, dt);
double control_period_ = kDefaultControlPeriod; if (dt > 0.0)
bool require_costmap_ = false; {
speed = std::min(speed, remaining / dt);
}
speed = std::max(speed, 0.0);
double backup_distance_ = kDefaultBackupDistance; // Pose dự đoán ở CUỐI chu kỳ tới — kiểm trước khi phát lệnh, không phải sau.
double linear_speed_ = kDefaultLinearSpeed; const double probe = std::max(speed * std::max(dt, kMinProbeDt), kProbeDistance);
double traveled_distance_ = 0.0; if (blockedAhead(pose, probe))
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("obstacle behind — cancelling the back-up");
}
current_speed_ = speed;
robot_geometry_msgs::Twist command;
command.linear.x = -speed; // [m/s], âm = lùi
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(traveled / backup_distance_, remaining)
.withMessage("backing up");
}
recovery_core::RecoveryResult onCancel() override
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("backup cancelled");
}
private:
/// Khoảng dò tối thiểu [m] — luôn nhìn trước ít nhất một ô costmap kể cả khi dt rất nhỏ.
static constexpr double kProbeDistance = 0.05;
/// dt tối thiểu [s] dùng khi dự đoán, để tick đầu (dt = 0) vẫn dò về phía trước.
static constexpr double kMinProbeDt = 0.1;
/// @return true nếu đặt robot lùi thêm @p distance từ @p from là va chạm.
bool blockedAhead(const robot_geometry_msgs::PoseStamped& from, double distance) const
{
const double next_x = from.pose.position.x - std::cos(start_yaw_) * distance;
const double next_y = from.pose.position.y - std::sin(start_yaw_) * distance;
return ctx().collision->footprintCost(next_x, next_y, start_yaw_) < 0.0;
}
double clampDistance(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m is invalid; using %.3f m.",
name().c_str(), value, fallback);
return std::min(fallback, backup_distance_max_);
}
if (value > backup_distance_max_)
{
robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m > limit %.3f m; clamped.",
name().c_str(), value, backup_distance_max_);
return backup_distance_max_;
}
return value;
}
double clampSpeed(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s is invalid; using %.3f m/s.",
name().c_str(), value, fallback);
return fallback;
}
if (value > kMaxLinearSpeed)
{
robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s > limit %.3f m/s; clamped.",
name().c_str(), value, kMaxLinearSpeed);
return kMaxLinearSpeed;
}
return value;
}
// Config
double default_backup_distance_ = kDefaultBackupDistance; ///< [m]
double backup_distance_max_ = kDefaultBackupDistanceMax; ///< [m]
double default_linear_speed_ = kDefaultLinearSpeed; ///< [m/s]
double acc_lim_x_ = kDefaultAccLimX; ///< [m/s^2]
// Trạng thái lượt hiện tại
robot_geometry_msgs::PoseStamped start_pose_;
double start_yaw_ = 0.0; ///< [rad]
double backup_distance_ = kDefaultBackupDistance; ///< [m]
double linear_speed_ = kDefaultLinearSpeed; ///< [m/s]
double current_speed_ = 0.0; ///< [m/s] đang phát, để ramp
}; };
} // namespace recovery_plugins } // namespace recovery_plugins

184
plugins/clear_costmap_recovery.cpp
View File

@@ -2,7 +2,7 @@
* *
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* recovery_core — no-output clear costmap plugin. * recovery_core — xoá vật cản đã tích trong costmap. Một tick, không output.
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
@@ -19,12 +19,16 @@
#include <boost/pointer_cast.hpp> #include <boost/pointer_cast.hpp>
#include <boost/thread/locks.hpp> #include <boost/thread/locks.hpp>
#include <robot/robot.h> #include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/costmap_layer.h> #include <robot_costmap_2d/costmap_layer.h>
namespace recovery_plugins namespace recovery_plugins
{ {
namespace namespace
{ {
constexpr double kDefaultResetDistance = 3.0; // [m]
constexpr double kMaxResetDistance = 100.0; // [m] trần vệ sinh cho param sai
std::string leafName(std::string name) std::string leafName(std::string name)
{ {
const std::string::size_type slash = name.rfind('/'); const std::string::size_type slash = name.rfind('/');
@@ -34,13 +38,23 @@ std::string leafName(std::string name)
} }
return name; return name;
} }
bool isValidResetDistance(double value)
{
return std::isfinite(value) && value > 0.0;
}
} // namespace } // namespace
/**
* @class ClearCostmapRecovery
* @brief Xoá vùng vật cản đã tích trong các layer được chỉ định.
*
* Dùng **hai instance** trong bộ default:
* - `conservative_reset` (`invert_area_to_clear: false`) xoá vùng gần robot;
* - `aggressive_reset` (`invert_area_to_clear: true`) xoá mọi thứ **ngoài** vùng đó.
*
* Không phát output, hoàn tất trong một tick.
*
* @note Cố ý **không** gọi `Costmap2DROBOT::updateMap()`. Hàm đó giữ mutex master rồi chạy toàn bộ
* chuỗi layer `updateBounds`/`updateCosts` — cỡ chục tới trăm ms — trong khi behavior này
* chạy trên thread phát cmd_vel ở 30 Hz. Costmap tự update ở chu kỳ riêng của nó ngay sau
* đó; ép update tại đây chỉ để đổi lấy một chu kỳ control bị lỡ.
*/
class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
{ {
public: public:
@@ -51,82 +65,107 @@ public:
return std::make_shared<ClearCostmapRecovery>(); return std::make_shared<ClearCostmapRecovery>();
} }
protected: recovery_core::RecoveryOutputType outputKind() const override
void onConfigure() override
{ {
robot::NodeHandle private_nh("~/" + name_); return recovery_core::RecoveryOutputType::kNone;
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_)) protected:
bool onConfigure(robot::NodeHandle& nh) override
{ {
robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.", nh.param("reset_distance", reset_distance_, kDefaultResetDistance);
name_.c_str()); nh.param("invert_area_to_clear", invert_area_to_clear_, false);
reset_distance_ = 3.0; nh.param("affected_maps", affected_maps_, std::string("both"));
if (!std::isfinite(reset_distance_) || reset_distance_ <= 0.0 ||
reset_distance_ > kMaxResetDistance)
{
robot::log_warning("[recovery_core] '%s': reset_distance=%.3f m outside (0, %.0f]; using "
"%.3f m.", name().c_str(), reset_distance_, kMaxResetDistance,
kDefaultResetDistance);
reset_distance_ = kDefaultResetDistance;
} }
if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both") if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
{ {
robot::log_warning("[recovery_core] Invalid affected_maps '%s' for '%s'; using 'both'.", robot::log_warning("[recovery_core] '%s': affected_maps='%s' is invalid; using 'both'.",
affected_maps_.c_str(), name_.c_str()); name().c_str(), affected_maps_.c_str());
affected_maps_ = "both"; affected_maps_ = "both";
} }
std::vector<std::string> clearable_layers_default; std::vector<std::string> clearable_layers_default{"obstacles"};
clearable_layers_default.emplace_back("obstacles");
std::vector<std::string> clearable_layers; std::vector<std::string> clearable_layers;
private_nh.param("layer_names", clearable_layers, clearable_layers_default); nh.param("layer_names", clearable_layers, clearable_layers_default);
clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end()); clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
}
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override if (clearable_layers_.empty())
{ {
// One-shot: công việc thực hiện ở onUpdate() lần đầu. robot::log_error("[recovery_core] '%s': layer_names is empty — this behavior will not clear "
return recovery_core::RecoveryResult::Running().withMessage("clear costmap start"); "anything.",
name().c_str());
return false;
} }
recovery_core::RecoveryResult onUpdate() override const bool needs_global = affected_maps_ == "global" || affected_maps_ == "both";
const bool needs_local = affected_maps_ == "local" || affected_maps_ == "both";
if (needs_global && ctx().global_costmap == nullptr)
{
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the global costmap is "
"missing.",
name().c_str(), affected_maps_.c_str());
return false;
}
if (needs_local && ctx().local_costmap == nullptr)
{
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the local costmap is missing.",
name().c_str(), affected_maps_.c_str());
return false;
}
return true;
}
bool onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
// One-shot: công việc thực hiện ở tick đầu tiên, giữ start() không có tác dụng phụ.
return true;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
{ {
bool ok = true; bool ok = true;
if (affected_maps_ == "global" || affected_maps_ == "both") if (affected_maps_ == "global" || affected_maps_ == "both")
{ {
ok = clear(ctx().global_costmap) && ok; ok = clear(ctx().global_costmap, "global") && ok;
if (ok && force_updating_ && ctx().global_costmap != nullptr)
{
ctx().global_costmap->updateMap();
}
} }
if (affected_maps_ == "local" || affected_maps_ == "both") if (affected_maps_ == "local" || affected_maps_ == "both")
{ {
ok = clear(ctx().local_costmap) && ok; ok = clear(ctx().local_costmap, "local") && ok;
if (ok && force_updating_ && ctx().local_costmap != nullptr)
{
ctx().local_costmap->updateMap();
}
} }
return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete") return ok ? recovery_core::RecoveryResult::Succeeded()
.withProgress(1.0, 0.0)
.withMessage("clear costmap complete")
: recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed"); : recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
} }
private: private:
bool clear(robot_costmap_2d::Costmap2DROBOT* costmap) bool clear(robot_costmap_2d::Costmap2DROBOT* costmap, const char* which)
{ {
if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr) if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
{ {
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap.", robot::log_error("[recovery_core] '%s': %s costmap is missing.", name().c_str(), which);
name_.c_str());
return false; return false;
} }
robot_geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
if (!costmap->getRobotPose(pose)) if (!costmap->getRobotPose(pose))
{ {
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' cannot get robot pose.", robot::log_error("[recovery_core] '%s': could not get the robot pose on the %s costmap.",
name_.c_str()); name().c_str(), which);
return false; return false;
} }
@@ -134,12 +173,14 @@ private:
costmap->getLayeredCostmap()->getPlugins(); costmap->getLayeredCostmap()->getPlugins();
if (plugins == nullptr) if (plugins == nullptr)
{ {
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap layers.", robot::log_error("[recovery_core] '%s': %s costmap has no layer.", name().c_str(),
name_.c_str()); which);
return false; return false;
} }
bool touched_layer = false; bool touched_layer = false;
std::string available;
for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins) for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
{ {
if (!plugin) if (!plugin)
@@ -147,16 +188,23 @@ private:
continue; continue;
} }
const std::string name = leafName(plugin->getName()); const std::string layer_name = leafName(plugin->getName());
if (clearable_layers_.count(name) == 0)
if (!available.empty())
{
available += ", ";
}
available += layer_name;
if (clearable_layers_.count(layer_name) == 0)
{ {
continue; continue;
} }
if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr) if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
{ {
robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.", robot::log_warning("[recovery_core] '%s': layer '%s' is not a CostmapLayer; skipped.",
name.c_str()); name().c_str(), layer_name.c_str());
continue; continue;
} }
@@ -165,13 +213,22 @@ private:
touched_layer = true; touched_layer = true;
} }
return touched_layer; if (!touched_layer)
{
// Nguyên nhân phổ biến nhất của "recovery này không làm gì" là sai tên layer trong config.
// Bản trước trả kFailed lặng lẽ, nên không có cách nào biết vì sao.
robot::log_error("[recovery_core] '%s': no layer of the %s costmap matches layer_names. "
"Layers present: [%s].", name().c_str(), which, available.c_str());
return false;
} }
void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap, return true;
double pose_x, double pose_y) }
void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& layer, double pose_x,
double pose_y)
{ {
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex())); boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(layer->getMutex()));
const double start_point_x = pose_x - reset_distance_ / 2.0; const double start_point_x = pose_x - reset_distance_ / 2.0;
const double start_point_y = pose_y - reset_distance_ / 2.0; const double start_point_y = pose_y - reset_distance_ / 2.0;
@@ -182,19 +239,20 @@ private:
int start_y = 0; int start_y = 0;
int end_x = 0; int end_x = 0;
int end_y = 0; int end_y = 0;
costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y); layer->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y); layer->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_); layer->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
costmap->addExtraBounds(costmap->getOriginX(), costmap->getOriginY(),
costmap->getOriginX() + costmap->getSizeInMetersX(), // Báo cho layer biết toàn bộ vùng của nó cần được ghi lại vào master ở chu kỳ update kế tiếp.
costmap->getOriginY() + costmap->getSizeInMetersY()); layer->addExtraBounds(layer->getOriginX(), layer->getOriginY(),
layer->getOriginX() + layer->getSizeInMetersX(),
layer->getOriginY() + layer->getSizeInMetersY());
} }
bool force_updating_ = false; double reset_distance_ = kDefaultResetDistance; ///< [m] cạnh vùng vuông quanh robot
double reset_distance_ = 3.0; bool invert_area_to_clear_ = false; ///< true = xoá phần NGOÀI vùng
bool invert_area_to_clear_ = false; std::string affected_maps_ = "both"; ///< local | global | both
std::string affected_maps_ = "both";
std::set<std::string> clearable_layers_; std::set<std::string> clearable_layers_;
}; };

62
plugins/regen_path_recovery.cpp
View File

@@ -1,62 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — path output recovery plugin (goal-driven, one-shot).
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
namespace recovery_plugins
{
/**
* @class RegenPathRecovery
* @brief Họ A (path output), one-shot: trả lại robot_nav_msgs::Path từ global_path hiện tại.
*
* Hoàn tất ngay ở lần update() đầu. Guard chưa configure/global_path null hoặc rỗng -> Failed().
*/
class RegenPathRecovery final : public recovery_core::RecoveryBehavior
{
public:
RegenPathRecovery() = default;
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<RegenPathRecovery>();
}
protected:
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
// One-shot: công việc thực hiện ở onUpdate() lần đầu, giữ start() gọn.
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withMessage("regen path start");
}
recovery_core::RecoveryResult onUpdate() override
{
const auto* global_path = ctx().global_path;
if (global_path == nullptr || global_path->empty())
{
return recovery_core::RecoveryResult::Failed().withMessage("no global path to regenerate");
}
robot_nav_msgs::Path path;
path.poses = *global_path;
return recovery_core::RecoveryResult::PathOut(path, recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("regen path complete");
}
};
} // namespace recovery_plugins
BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)

279
plugins/rotate_recovery.cpp
View File

@@ -2,12 +2,13 @@
* *
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* recovery_core — per-cycle rotate recovery plugin (goal-driven). * recovery_core — quay tại chỗ, quét cung trước khi quay và đo bằng pose thật.
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
#include <recovery_core/recovery_behavior.h> #include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@@ -20,18 +21,30 @@ namespace recovery_plugins
{ {
namespace namespace
{ {
constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad. constexpr double kTwoPi = 2.0 * M_PI;
constexpr double kDefaultAngularSpeed = 0.4; // rad/s. constexpr double kDefaultAngularSpeed = 0.4; // [rad/s] độ lớn
constexpr double kDefaultControlPeriod = 0.1; // s per update tick. constexpr double kDefaultAccLimTheta = 0.8; // [rad/s^2]
constexpr double kDefaultSimGranularity = 0.1; // [rad] bước quét cung
constexpr double kMaxAngularSpeed = 2.0; // [rad/s] trần vệ sinh cho param sai
constexpr double kGoalTolerance = 1e-3; // [rad]
} // namespace } // namespace
/** /**
* @class RotateRecovery * @class RotateRecovery
* @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal). * @brief Quay tại chỗ, mặc định đủ một vòng, để costmap quan sát lại xung quanh.
* *
* goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ * Quay đủ 2π là công dụng chính của rotate trong một bộ recovery: nó cho obstacle/voxel layer nhìn
* góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả * thấy toàn bộ vùng quanh robot rồi mới lập plan lại. Đặt `full_rotation: false` hoặc truyền
* Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command). * `goal.angle` để quay một góc cụ thể.
*
* Hai lớp bảo vệ so với bản trước (bản trước không dùng `ctx()` một lần nào):
* 1. **Quét toàn bộ cung sẽ quay** tại `onStart()` theo bước `sim_granularity`; chạm vật cản ở bất
* kỳ góc nào là từ chối khởi động, chứ không quay tới nơi mới phát hiện.
* 2. **Tiến độ đo bằng pose thật**, cộng dồn từng chênh lệch yaw đã chuẩn hoá — nên quay > π vẫn
* đếm đúng, và loop chạy chậm không làm robot quay quá góc.
*
* @note Yêu cầu robot xoay tại chỗ được (differential/omni). Config workspace hiện tại thoả:
* `min_turn_radius: 0.0`, `use_rotate_to_heading: true`.
*/ */
class RotateRecovery final : public recovery_core::RecoveryBehavior class RotateRecovery final : public recovery_core::RecoveryBehavior
{ {
@@ -43,95 +56,221 @@ public:
return std::make_shared<RotateRecovery>(); return std::make_shared<RotateRecovery>();
} }
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kVelocity;
}
protected: protected:
void onConfigure() override bool onConfigure(robot::NodeHandle& nh) override
{ {
robot::NodeHandle private_nh("~/" + name_); nh.param("full_rotation", full_rotation_, true);
private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle); nh.param("target_angle", default_target_angle_, kTwoPi);
private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed); nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
private_nh.param("control_period", control_period_, kDefaultControlPeriod); nh.param("acc_lim_theta", acc_lim_theta_, kDefaultAccLimTheta);
nh.param("sim_granularity", sim_granularity_, kDefaultSimGranularity);
if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0) default_target_angle_ = clampAngle(default_target_angle_, kTwoPi);
default_angular_speed_ = clampSpeed(default_angular_speed_, kDefaultAngularSpeed);
if (!std::isfinite(acc_lim_theta_) || acc_lim_theta_ < 0.0)
{ {
robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.", robot::log_warning("[recovery_core] '%s': acc_lim_theta=%.3f rad/s^2 is invalid; using %.3f.",
name_.c_str()); name().c_str(), acc_lim_theta_, kDefaultAccLimTheta);
default_target_angle_ = kDefaultTargetAngle; acc_lim_theta_ = kDefaultAccLimTheta;
}
if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
name_.c_str());
default_angular_speed_ = kDefaultAngularSpeed;
}
if (!std::isfinite(control_period_) || control_period_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
name_.c_str());
control_period_ = kDefaultControlPeriod;
}
} }
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override if (!std::isfinite(sim_granularity_) || sim_granularity_ <= 0.0)
{ {
// Góc đích: goal.angle nếu hợp lệ, ngược lại default configured. robot::log_warning("[recovery_core] '%s': sim_granularity=%.3f rad is invalid; using %.3f "
target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0) "rad.", name().c_str(), sim_granularity_, kDefaultSimGranularity);
? goal.angle sim_granularity_ = kDefaultSimGranularity;
: default_target_angle_;
angular_speed_ = std::abs(goal.param("angular_speed", default_angular_speed_));
if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
{
angular_speed_ = default_angular_speed_;
} }
rotated_angle_ = 0.0; return true;
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withProgress(0.0, std::abs(target_angle_))
.withMessage("rotate start");
} }
recovery_core::RecoveryResult onUpdate() override bool onStart(const recovery_core::RecoveryGoal& goal) override
{
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
{
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
name().c_str());
return false;
}
start_pose_ = pose;
start_yaw_ = recovery_core::yawOf(pose);
last_yaw_ = start_yaw_;
rotated_ = 0.0;
current_speed_ = 0.0;
// goal.angle có giá trị (kể cả 0.0) thì tôn trọng đúng giá trị đó. Bản trước coi 0 là "chưa
// đặt" nên một góc tính ra ~0 bị âm thầm thay bằng pi/2.
const double requested = goal.angle.value_or(full_rotation_ ? kTwoPi : default_target_angle_);
target_angle_ = clampAngle(requested, default_target_angle_);
angular_speed_ = clampSpeed(std::abs(goal.param("angular_speed", default_angular_speed_)),
default_angular_speed_);
if (std::abs(target_angle_) <= kGoalTolerance)
{
// Caller nói rõ "đừng quay". Đó là một yêu cầu hợp lệ và hoàn tất ngay.
zero_rotation_ = true;
return true;
}
zero_rotation_ = false;
if (!arcIsClear())
{
robot::log_warning("[recovery_core] '%s': the %.3f rad arc is blocked, refusing to rotate.",
name().c_str(), target_angle_);
return false;
}
return true;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{ {
const double target = std::abs(target_angle_); const double target = std::abs(target_angle_);
if (rotated_angle_ >= target) if (zero_rotation_)
{ {
return succeeded(target); return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate 0 rad — nothing to rotate");
} }
robot_geometry_msgs::Twist command; robot_geometry_msgs::PoseStamped pose;
command.angular.z = std::copysign(angular_speed_, target_angle_); if (!ctx().pose->getRobotPose(pose))
rotated_angle_ =
std::min(target, rotated_angle_ + std::abs(command.angular.z) * control_period_);
if (rotated_angle_ >= target)
{ {
return succeeded(target); current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("robot pose lost (TF stale?) — stopping the rotation");
} }
return recovery_core::RecoveryResult::Velocity(command, const double yaw = recovery_core::yawOf(pose);
recovery_core::RecoveryStatus::kRunning)
.withProgress(rotated_angle_ / target, target - rotated_angle_)
.withMessage("rotating");
}
private: // Cộng dồn từng bước đã chuẩn hoá: cách duy nhất đếm đúng khi tổng góc quay vượt pi.
recovery_core::RecoveryResult succeeded(double target) rotated_ += std::abs(recovery_core::normalizeAngle(yaw - last_yaw_));
last_yaw_ = yaw;
const double remaining = target - rotated_;
if (remaining <= kGoalTolerance)
{ {
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), current_speed_ = 0.0;
recovery_core::RecoveryStatus::kSucceeded) return stopResult(recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0) .withProgress(1.0, 0.0)
.withMessage("rotate complete"); .withMessage("rotate complete");
} }
double default_target_angle_ = kDefaultTargetAngle; double speed = recovery_core::rampToward(angular_speed_, current_speed_, acc_lim_theta_, dt);
double default_angular_speed_ = kDefaultAngularSpeed; if (dt > 0.0)
double control_period_ = kDefaultControlPeriod; {
speed = std::min(speed, remaining / dt);
}
speed = std::max(speed, 0.0);
double target_angle_ = kDefaultTargetAngle; // Cung đã quét ở onStart(), nhưng costmap đổi giữa chừng thì phải phát hiện: kiểm góc dự đoán
double angular_speed_ = kDefaultAngularSpeed; // ở cuối chu kỳ tới trước khi phát lệnh.
double rotated_angle_ = 0.0; const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
const double next_yaw = yaw + direction * std::max(speed * dt, sim_granularity_);
if (ctx().collision->footprintCost(pose.pose.position.x, pose.pose.position.y, next_yaw) < 0.0)
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kFailed)
.withMessage("the rotation arc became blocked midway — stopping the rotation");
}
current_speed_ = speed;
robot_geometry_msgs::Twist command;
command.angular.z = std::copysign(speed, target_angle_); // [rad/s], + = ngược chiều kim đồng hồ
return recovery_core::RecoveryResult::Velocity(command,
recovery_core::RecoveryStatus::kRunning)
.withProgress(rotated_ / target, remaining)
.withMessage("rotating");
}
recovery_core::RecoveryResult onCancel() override
{
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("rotate cancelled");
}
private:
/// @return true nếu toàn bộ cung sẽ quay đều đặt được footprint.
bool arcIsClear() const
{
const double target = std::abs(target_angle_);
const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
const double x = start_pose_.pose.position.x;
const double y = start_pose_.pose.position.y;
for (double swept = 0.0; swept < target; swept += sim_granularity_)
{
if (ctx().collision->footprintCost(x, y, start_yaw_ + direction * swept) < 0.0)
{
return false;
}
}
// Kiểm luôn góc cuối: vòng lặp trên dừng trước target nếu target không chia hết cho bước quét.
return ctx().collision->footprintCost(x, y, start_yaw_ + direction * target) >= 0.0;
}
double clampAngle(double value, double fallback) const
{
if (!std::isfinite(value))
{
robot::log_warning("[recovery_core] '%s': target_angle is not finite; using %.3f rad.",
name().c_str(), fallback);
return fallback;
}
if (std::abs(value) > kTwoPi)
{
robot::log_warning("[recovery_core] '%s': target_angle=%.3f rad exceeds +/-2pi; clamped.",
name().c_str(), value);
return std::copysign(kTwoPi, value);
}
return value;
}
double clampSpeed(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0)
{
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s is invalid; using %.3f "
"rad/s.", name().c_str(), value, fallback);
return fallback;
}
if (value > kMaxAngularSpeed)
{
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s > limit %.3f; clamped.",
name().c_str(), value, kMaxAngularSpeed);
return kMaxAngularSpeed;
}
return value;
}
// Config
bool full_rotation_ = true;
double default_target_angle_ = kTwoPi; ///< [rad]
double default_angular_speed_ = kDefaultAngularSpeed; ///< [rad/s]
double acc_lim_theta_ = kDefaultAccLimTheta; ///< [rad/s^2]
double sim_granularity_ = kDefaultSimGranularity; ///< [rad]
// Trạng thái lượt hiện tại
robot_geometry_msgs::PoseStamped start_pose_;
double start_yaw_ = 0.0; ///< [rad]
double last_yaw_ = 0.0; ///< [rad]
double rotated_ = 0.0; ///< [rad] cộng dồn, luôn >= 0
double target_angle_ = kTwoPi; ///< [rad] có dấu
double angular_speed_ = kDefaultAngularSpeed; ///< [rad/s] độ lớn
double current_speed_ = 0.0; ///< [rad/s] đang phát, để ramp
bool zero_rotation_ = false; ///< caller yêu cầu góc 0
}; };
} // namespace recovery_plugins } // namespace recovery_plugins

103
plugins/wait_recovery.cpp Normal file
View File

@@ -0,0 +1,103 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — đợi tại chỗ, không phát output.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <string>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
namespace recovery_plugins
{
namespace
{
constexpr double kDefaultWaitDuration = 3.0; // [s]
constexpr double kMaxWaitDuration = 300.0; // [s] trần vệ sinh cho param cấu hình sai.
} // namespace
/**
* @class WaitRecovery
* @brief Đứng yên một khoảng thời gian rồi báo thành công.
*
* Đây là recovery **an toàn nhất** trong bộ default và nên đứng đầu danh sách: nó không di chuyển,
* không cần pose, không cần collision check. Với AMR/AGV trong kho, phần lớn tình huống chặn đường
* là vật cản động (người, xe khác) — đợi vài giây rồi lập plan lại giải quyết được đa số, trong khi
* mọi behavior khác đều bắt robot cử động trong lúc chưa biết chuyện gì đang xảy ra.
*
* Thời lượng: `goal.params["wait_duration"]` cho lượt này, ngược lại param `wait_duration`.
*/
class WaitRecovery final : public recovery_core::RecoveryBehavior
{
public:
WaitRecovery() = default;
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<WaitRecovery>();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return recovery_core::RecoveryOutputType::kNone;
}
protected:
bool onConfigure(robot::NodeHandle& nh) override
{
nh.param("wait_duration", default_wait_duration_, kDefaultWaitDuration);
default_wait_duration_ = sanitizeDuration(default_wait_duration_, kDefaultWaitDuration);
return true;
}
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
wait_duration_ = sanitizeDuration(goal.param("wait_duration", default_wait_duration_),
default_wait_duration_);
return true;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
{
// elapsed() do base đo bằng đồng hồ thật, nên loop chạy chậm không làm sai thời lượng đợi.
const double waited = elapsed();
if (waited >= wait_duration_)
{
return recovery_core::RecoveryResult::Succeeded()
.withProgress(1.0, 0.0)
.withMessage("wait complete");
}
return recovery_core::RecoveryResult::Running()
.withProgress(waited / wait_duration_, wait_duration_ - waited)
.withMessage("waiting");
}
private:
double sanitizeDuration(double value, double fallback) const
{
if (!std::isfinite(value) || value <= 0.0 || value > kMaxWaitDuration)
{
robot::log_warning("[recovery_core] '%s': wait_duration=%.3f s outside (0, %.0f]; using %.3f "
"s.",
name().c_str(), value, kMaxWaitDuration, fallback);
return fallback;
}
return value;
}
double default_wait_duration_ = kDefaultWaitDuration; ///< [s]
double wait_duration_ = kDefaultWaitDuration; ///< [s] mục tiêu lượt này
};
} // namespace recovery_plugins
BOOST_DLL_ALIAS(recovery_plugins::WaitRecovery::create, WaitRecovery)

260
src/recovery_behavior.cpp
View File

@@ -1,79 +1,167 @@
/********************************************************************* /*********************************************************************
* recovery_core — phần chung (template-method) của RecoveryBehavior. * recovery_core — phần chung (template-method) của RecoveryBehavior.
* *
* configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời và xử lý * Base giữ toàn bộ bất biến vòng đời, thời gian, và họ output; plugin chỉ triển khai hook.
* cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
#include <recovery_core/recovery_behavior.h> #include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <robot/robot.h>
namespace recovery_core namespace recovery_core
{ {
namespace namespace
{ {
robot_geometry_msgs::Twist zeroTwist() constexpr double kDefaultTimeout = 0.0; ///< [s] 0 = không giới hạn.
constexpr double kMaxTimeout = 600.0; ///< [s] trần vệ sinh cho param cấu hình sai.
/// Một giá trị optional hợp lệ phải hữu hạn.
bool finiteIfSet(const std::optional<double>& value)
{ {
return robot_geometry_msgs::Twist(); return !value.has_value() || std::isfinite(*value);
} }
} // namespace } // namespace
void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx) bool RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx,
robot::NodeHandle& nh)
{ {
if (configured_) if (configured_)
{ {
return; // Gọi lại với ctx khác là lỗi lập trình của caller. Bản cũ im lặng return, nên caller không bao
// giờ biết context thứ hai đã bị bỏ đi.
robot::log_error("[recovery_core] '%s': configure() called twice, ignored.", name_.c_str());
return false;
}
if (name.empty())
{
robot::log_error("[recovery_core] configure() with an empty instance name.");
return false;
} }
name_ = name; name_ = name;
ctx_ = ctx; ctx_ = ctx;
status_ = RecoveryStatus::kIdle;
onConfigure(); if (!validateContext())
{
configured_ = true; name_.clear();
ctx_ = RecoveryContext();
return false;
} }
RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal) nh.param("timeout", timeout_, kDefaultTimeout);
if (!std::isfinite(timeout_) || timeout_ < 0.0 || timeout_ > kMaxTimeout)
{
robot::log_warning("[recovery_core] '%s': timeout=%.3f s outside [0, %.0f]; using 0 (no "
"limit).", name_.c_str(), timeout_, kMaxTimeout);
timeout_ = kDefaultTimeout;
}
if (!onConfigure(nh))
{
robot::log_error("[recovery_core] '%s': onConfigure() failed.", name_.c_str());
name_.clear();
ctx_ = RecoveryContext();
return false;
}
status_ = RecoveryStatus::kIdle;
configured_ = true;
return true;
}
bool RecoveryBehavior::start(const RecoveryGoal& goal, const robot::Time& now)
{ {
if (!configured_) if (!configured_)
{ {
status_ = RecoveryStatus::kFailed; robot::log_error("[recovery_core] start() before configure().");
return RecoveryResult::Failed().withMessage("start() before configure()"); return false;
}
if (started_ && status_ == RecoveryStatus::kRunning && !cancel_requested_)
{
// Không từ chối: sau cancel(), state machine có thể start lượt mới mà lượt cũ chưa kịp về
// terminal (nó không tick recovery ở state CANCELLING). Nhưng vẫn phải báo, vì nếu KHÔNG phải
// đường cancel thì đây là caller đang bỏ dở một behavior đang lái robot.
robot::log_warning("[recovery_core] '%s': start() while the previous run is still going — "
"resetting.",
name_.c_str());
}
if (!validateGoal(goal))
{
return false;
} }
goal_ = goal; goal_ = goal;
cancel_requested_ = false; cancel_requested_ = false;
started_ = true; start_time_ = now;
last_update_ = now;
elapsed_ = 0.0;
status_ = RecoveryStatus::kRunning; status_ = RecoveryStatus::kRunning;
started_ = true;
RecoveryResult result = onStart(goal_); if (!onStart(goal_))
status_ = result.status; {
return result; status_ = RecoveryStatus::kFailed;
robot::log_warning("[recovery_core] '%s': refused to start (trigger=%s).", name_.c_str(),
toString(goal_.trigger));
return false;
} }
RecoveryResult RecoveryBehavior::update() return true;
}
RecoveryResult RecoveryBehavior::update(const robot::Time& now)
{ {
if (!configured_ || !started_) if (!configured_ || !started_)
{ {
status_ = RecoveryStatus::kFailed; status_ = RecoveryStatus::kFailed;
return RecoveryResult::Failed().withMessage("update() before start()"); return stopResult(RecoveryStatus::kFailed).withMessage("update() before start()");
} }
// Đã kết thúc: giữ nguyên trạng thái, không tick thêm.
if (status_ != RecoveryStatus::kRunning) if (status_ != RecoveryStatus::kRunning)
{ {
return RecoveryResult::Velocity(zeroTwist(), status_); // Lượt đã kết thúc: giữ nguyên kết luận, không tick thêm.
return stopResult(status_);
}
// dt đo THẬT. Đây là điểm sửa cốt lõi so với bản cũ: bản cũ tích phân vận tốc lệnh nhân với
// control_period lấy từ config, nên control loop chạy chậm là robot đi quá quãng yêu cầu.
double dt = (now - last_update_).toSec();
if (!std::isfinite(dt) || dt < 0.0)
{
// Đồng hồ đi lùi (đổi nguồn thời gian, hoặc sim reset). Coi như không có thời gian trôi thay vì
// tích phân một dt âm vào tiến độ.
robot::log_warning("[recovery_core] '%s': dt=%.6f s is invalid, treated as 0.", name_.c_str(),
dt);
dt = 0.0;
}
last_update_ = now;
elapsed_ = (now - start_time_).toSec();
if (!std::isfinite(elapsed_) || elapsed_ < 0.0)
{
elapsed_ = 0.0;
} }
if (cancel_requested_) if (cancel_requested_)
{ {
status_ = RecoveryStatus::kCancelled; RecoveryResult result = finalize(onCancel());
return RecoveryResult::Velocity(zeroTwist(), status_) status_ = result.status;
.withMessage("cancelled by caller"); return result;
} }
RecoveryResult result = onUpdate(); if (timeout_ > 0.0 && elapsed_ >= timeout_)
{
status_ = RecoveryStatus::kFailed;
return stopResult(RecoveryStatus::kFailed)
.withMessage("exceeded the timeout of " + std::to_string(timeout_) + " s");
}
RecoveryResult result = finalize(onUpdate(now, dt));
status_ = result.status; status_ = result.status;
return result; return result;
} }
@@ -83,4 +171,126 @@ void RecoveryBehavior::cancel()
cancel_requested_ = true; cancel_requested_ = true;
} }
RecoveryResult RecoveryBehavior::onCancel()
{
return stopResult(RecoveryStatus::kCancelled).withMessage("cancelled by caller");
}
RecoveryResult RecoveryBehavior::stopResult(RecoveryStatus status) const
{
RecoveryResult result;
result.status = status;
result.elapsed = elapsed_;
if (outputKind() == RecoveryOutputType::kVelocity)
{
// Họ velocity: phát lệnh dừng TƯỜNG MINH. Caller đang lấy cmd_vel từ behavior này nên "không
// output" và "output vận tốc 0" là hai chuyện khác nhau.
result.output_type = RecoveryOutputType::kVelocity;
result.command = robot_geometry_msgs::Twist();
}
else
{
// Họ khác: KHÔNG bịa ra output vận tốc. Bản cũ trả Velocity(zero) cho mọi họ, nên một behavior
// clear-costmap báo cáo mình phát vận tốc.
result.output_type = RecoveryOutputType::kNone;
}
return result;
}
bool RecoveryBehavior::validateContext() const
{
const RecoveryOutputType kind = outputKind();
if (kind == RecoveryOutputType::kVelocity)
{
if (ctx_.pose == nullptr)
{
robot::log_error("[recovery_core] '%s': the velocity family requires a PoseProvider — "
"progress must be measured from a real pose, not dead-reckoned.",
name_.c_str());
return false;
}
if (ctx_.collision == nullptr)
{
robot::log_error("[recovery_core] '%s': the velocity family requires a CollisionChecker — "
"driving blind is not allowed.", name_.c_str());
return false;
}
}
if (kind == RecoveryOutputType::kPath && ctx_.plan == nullptr)
{
robot::log_error("[recovery_core] '%s': the path family requires a PlanProvider.",
name_.c_str());
return false;
}
return true;
}
bool RecoveryBehavior::validateGoal(const RecoveryGoal& goal) const
{
if (!finiteIfSet(goal.angle) || !finiteIfSet(goal.distance))
{
robot::log_error("[recovery_core] '%s': goal contains NaN/Inf.", name_.c_str());
return false;
}
if (goal.distance.has_value() && *goal.distance <= 0.0)
{
robot::log_error("[recovery_core] '%s': goal.distance=%.3f m must be > 0.", name_.c_str(),
*goal.distance);
return false;
}
for (const auto& entry : goal.params)
{
if (!std::isfinite(entry.second))
{
robot::log_error("[recovery_core] '%s': goal.params['%s'] is not finite.", name_.c_str(),
entry.first.c_str());
return false;
}
}
return true;
}
RecoveryResult RecoveryBehavior::finalize(RecoveryResult result) const
{
const RecoveryOutputType kind = outputKind();
if (result.output_type != kind && result.output_type != RecoveryOutputType::kNone)
{
// Plugin trả sai họ. Hạ về kNone thay vì tin theo: caller route bằng output_type, nên một họ
// sai ở đây là caller đọc nhầm trường.
robot::log_error("[recovery_core] '%s': returned output_type='%s' but outputKind()='%s'; "
"downgraded to 'none'.", name_.c_str(), toString(result.output_type), toString(kind));
result.output_type = RecoveryOutputType::kNone;
result.command = robot_geometry_msgs::Twist();
result.path = robot_nav_msgs::Path();
}
if (result.output_type == RecoveryOutputType::kVelocity)
{
const robot_geometry_msgs::Twist& cmd = result.command;
if (!std::isfinite(cmd.linear.x) || !std::isfinite(cmd.linear.y) ||
!std::isfinite(cmd.linear.z) || !std::isfinite(cmd.angular.x) ||
!std::isfinite(cmd.angular.y) || !std::isfinite(cmd.angular.z))
{
// NaN/Inf lọt ra cmd_vel là lỗi không được phép đi tiếp: đổi thành lệnh dừng + kFailed.
robot::log_error("[recovery_core] '%s': velocity command contains NaN/Inf — forcing a stop.",
name_.c_str());
RecoveryResult stop = stopResult(RecoveryStatus::kFailed);
stop.message = "velocity command is not finite";
return stop;
}
}
result.elapsed = elapsed_;
return result;
}
} // namespace recovery_core } // namespace recovery_core

183
src/recovery_registry.cpp Normal file
View File

@@ -0,0 +1,183 @@
/*********************************************************************
* recovery_core — nạp behavior theo YAML bằng Boost.DLL.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_registry.h>
#include <utility>
#include <boost/dll/import.hpp>
#include <boost/system/system_error.hpp>
#include <yaml-cpp/yaml.h>
#include <robot/robot.h>
namespace recovery_core
{
RecoveryRegistry::~RecoveryRegistry()
{
clear();
}
void RecoveryRegistry::clear()
{
// Behavior phải chết TRƯỚC factory: factory là thứ giữ .so còn nạp, thả ngược thứ tự sẽ gỡ thư
// viện trong khi vẫn còn object của nó, và vtable sẽ trỏ vào vùng đã unmap.
behaviors_.clear();
factories_.clear();
}
bool RecoveryRegistry::registerBehavior(const RecoveryBehavior::RecoveryBehaviorPtr& behavior)
{
if (!behavior)
{
robot::log_error("[recovery_core] RecoveryRegistry: behavior null.");
return false;
}
behaviors_.push_back(behavior);
return true;
}
RecoveryBehavior* RecoveryRegistry::at(std::size_t index) const
{
return index < behaviors_.size() ? behaviors_[index].get() : nullptr;
}
std::string RecoveryRegistry::nameAt(std::size_t index) const
{
return index < behaviors_.size() ? behaviors_[index]->name() : std::string();
}
bool RecoveryRegistry::loadOne(const std::string& name, const std::string& type,
robot::NodeHandle& nh, const RecoveryContext& ctx,
const std::string& ns)
{
robot::PluginLoaderHelper loader(nh);
const std::string library_path = loader.findLibraryPath(type);
if (library_path.empty())
{
robot::log_error("[recovery_core] RecoveryRegistry: no library found for '%s' — check the key "
"'%s/library_path' in the YAML and that the .so file exists.",
type.c_str(), type.c_str());
return false;
}
std::function<RecoveryBehavior::RecoveryBehaviorPtr()> factory;
try
{
factory = boost::dll::import_alias<RecoveryBehavior::RecoveryBehaviorPtr()>(
library_path, type, boost::dll::load_mode::append_decorations);
}
catch (const boost::system::system_error& ex)
{
// Sai tên symbol hoặc file không nạp được. Bắt tại đây để một plugin hỏng không giết cả tiến
// trình, nhưng vẫn báo lỗi để không ai tưởng đường phục hồi này đang chạy.
robot::log_error("[recovery_core] RecoveryRegistry: could not load symbol '%s' from '%s': %s",
type.c_str(), library_path.c_str(), ex.what());
return false;
}
catch (const std::exception& ex)
{
robot::log_error("[recovery_core] RecoveryRegistry: error while loading '%s': %s", type.c_str(),
ex.what());
return false;
}
RecoveryBehavior::RecoveryBehaviorPtr behavior;
try
{
behavior = factory();
}
catch (const std::exception& ex)
{
robot::log_error("[recovery_core] RecoveryRegistry: factory of '%s' threw an exception: %s",
type.c_str(), ex.what());
return false;
}
if (!behavior)
{
robot::log_error("[recovery_core] RecoveryRegistry: factory of '%s' returned null.",
type.c_str());
return false;
}
// Namespace param của instance do REGISTRY dựng, không phải plugin tự đi tìm trên disk. Nhờ vậy
// test chỉ cần trỏ NodeHandle vào cây config của mình là chạy được.
const std::string param_ns = ns.empty() ? name : ns + "/" + name;
robot::NodeHandle behavior_nh(nh, param_ns);
if (!behavior->configure(name, ctx, behavior_nh))
{
robot::log_error("[recovery_core] RecoveryRegistry: '%s' (instance '%s') configure() failed.",
type.c_str(), name.c_str());
return false;
}
behaviors_.push_back(behavior);
// Chỉ giữ factory sau khi behavior đã vào danh sách: nếu hỏng thì cũng không giữ .so lại.
factories_.push_back(std::move(factory));
robot::log_info("[recovery_core] RecoveryRegistry: loaded '%s' (instance '%s', family '%s').",
type.c_str(), name.c_str(), toString(behavior->outputKind()));
return true;
}
bool RecoveryRegistry::loadFromConfig(robot::NodeHandle& nh, const std::string& ns,
const RecoveryContext& ctx)
{
const std::string key = ns.empty() ? std::string("behaviors") : ns + "/behaviors";
YAML::Node behaviors;
if (!nh.getParam(key, behaviors) || !behaviors.IsSequence() || behaviors.size() == 0)
{
robot::log_error("[recovery_core] RecoveryRegistry: '%s' is missing or is not a list — no "
"recovery path was loaded.", key.c_str());
return false;
}
bool all_ok = true;
for (std::size_t i = 0; i < behaviors.size(); ++i)
{
const YAML::Node& entry = behaviors[i];
if (!entry.IsMap() || !entry["type"])
{
robot::log_error("[recovery_core] RecoveryRegistry: '%s[%zu]' is missing the 'type' key.",
key.c_str(), i);
all_ok = false;
continue;
}
std::string type;
std::string name;
try
{
type = entry["type"].as<std::string>();
name = entry["name"] ? entry["name"].as<std::string>() : type;
}
catch (const YAML::Exception& ex)
{
robot::log_error("[recovery_core] RecoveryRegistry: '%s[%zu]' could not be read: %s",
key.c_str(), i, ex.what());
all_ok = false;
continue;
}
if (!loadOne(name, type, nh, ctx, ns))
{
all_ok = false;
}
}
return all_ok;
}
} // namespace recovery_core

53
src/recovery_types.cpp
View File

@@ -1,5 +1,5 @@
/********************************************************************* /*********************************************************************
* recovery_core — factory cho RecoveryResult. * recovery_core — factory + tên hiển thị cho kiểu hợp đồng.
* *
* Author: DuongTD * Author: DuongTD
*********************************************************************/ *********************************************************************/
@@ -10,6 +10,54 @@
namespace recovery_core namespace recovery_core
{ {
const char* toString(RecoveryTrigger trigger)
{
switch (trigger)
{
case RecoveryTrigger::kUnspecified:
return "unspecified";
case RecoveryTrigger::kPlanningFailed:
return "planning_failed";
case RecoveryTrigger::kControllingFailed:
return "controlling_failed";
case RecoveryTrigger::kOscillation:
return "oscillation";
}
return "unknown";
}
const char* toString(RecoveryStatus status)
{
switch (status)
{
case RecoveryStatus::kIdle:
return "idle";
case RecoveryStatus::kRunning:
return "running";
case RecoveryStatus::kSucceeded:
return "succeeded";
case RecoveryStatus::kFailed:
return "failed";
case RecoveryStatus::kCancelled:
return "cancelled";
}
return "unknown";
}
const char* toString(RecoveryOutputType kind)
{
switch (kind)
{
case RecoveryOutputType::kNone:
return "none";
case RecoveryOutputType::kVelocity:
return "velocity";
case RecoveryOutputType::kPath:
return "path";
}
return "unknown";
}
RecoveryResult RecoveryResult::Running() RecoveryResult RecoveryResult::Running()
{ {
RecoveryResult result; RecoveryResult result;
@@ -57,8 +105,7 @@ RecoveryResult RecoveryResult::Velocity(const robot_geometry_msgs::Twist& comman
return result; return result;
} }
RecoveryResult RecoveryResult::PathOut(const robot_nav_msgs::Path& path, RecoveryResult RecoveryResult::PathOut(const robot_nav_msgs::Path& path, RecoveryStatus status)
RecoveryStatus status)
{ {
RecoveryResult result; RecoveryResult result;
result.status = status; result.status = status;

26
test/CMakeLists.txt
View File

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

186
test/backup_safety_test.cpp Normal file
View File

@@ -0,0 +1,186 @@
/*********************************************************************
*
* Kiểm ba lớp an toàn của BackUpRecovery.
*
* Bản trước không dùng collision checker (chỉ null-check con trỏ, và chỉ khi `require_costmap` bật
* — mặc định TẮT), nên mặc định robot lùi mù. Lùi là hướng robot thường không có sensor, nên đây là
* bộ test quan trọng nhất của Phase 3.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
struct BackUpFixture
{
BackUpFixture()
{
// Robot nhìn theo +x tại gốc; lùi nghĩa là đi về phía -x.
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
back_up = recovery_test::findBehavior(registry, "back_up");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* back_up = nullptr;
};
TEST(BackupSafety, RefusesToStartWhenObstacleIsBehind)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
// Vật cản chạm biên sau của footprint (footprint 0.6 x 0.4 -> biên sau ở x = -0.3).
fixture.rig.costmap.setLethalCircle(-0.35, 0.0, 0.10);
EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(BackupSafety, StopsWithZeroCommandWhenObstacleAppearsMidRun)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// Vài tick đầu chạy bình thường.
robot::Time now(1000.0);
for (int i = 0; i < 3; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_LT(result.velocity()->linear.x, 0.0); // âm = lùi
fixture.rig.applyCommand(result.command, 0.1);
}
// Có người bước vào phía sau robot.
const double robot_x = fixture.rig.pose.rawPose().x;
fixture.rig.costmap.setLethalCircle(robot_x - 0.36, 0.0, 0.10);
now = robot::Time(now.toSec() + 0.1);
const auto blocked = fixture.back_up->update(now);
EXPECT_EQ(blocked.status, RecoveryStatus::kFailed);
ASSERT_NE(blocked.velocity(), nullptr);
EXPECT_DOUBLE_EQ(blocked.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(blocked.velocity()->angular.z, 0.0);
}
TEST(BackupSafety, StopsWhenPoseIsLost)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
// TF quá hạn / thiếu frame.
fixture.rig.pose.setAvailable(false);
const auto result = fixture.back_up->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}
TEST(BackupSafety, RefusesToStartWhenPoseIsUnavailable)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
fixture.rig.pose.setAvailable(false);
EXPECT_FALSE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(BackupSafety, CommandNeverExceedsConfiguredSpeed)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < 40; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
break;
}
ASSERT_NE(result.velocity(), nullptr);
// linear_speed: 0.1 m/s trong config test.
EXPECT_LE(std::abs(result.velocity()->linear.x), 0.1 + 1e-9);
fixture.rig.applyCommand(result.command, 0.1);
}
}
TEST(BackupSafety, RampsUpInsteadOfSteppingToFullSpeed)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// acc_lim_x: 0.3 m/s^2 -> sau 0.05 s không thể vượt 0.015 m/s.
const auto first = fixture.back_up->update(robot::Time(1000.05));
ASSERT_NE(first.velocity(), nullptr);
EXPECT_LE(std::abs(first.velocity()->linear.x), 0.015 + 1e-9);
}
TEST(BackupSafety, CancelEmitsZeroCommand)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.back_up->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.back_up->cancel();
const auto result = fixture.back_up->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

68
test/config/recovery_behaviors_params.yaml Normal file
View File

@@ -0,0 +1,68 @@
# Config CHỈ dùng cho test của gói. Bản runtime nằm ở
# `pnkx_nav_core/config/recovery_behaviors_params.yaml` (C2) — sửa tham số vận hành thì sửa ở đó.
#
# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/recovery_core/test/config
#
# Chỉ khai các behavior chạy được với fake của nav_test_harness. ClearCostmapRecovery cần
# Costmap2DROBOT thật (TF + layer), nên nó được kiểm ở tầng tích hợp chứ không ở đây.
recovery:
# Thứ tự CHÍNH LÀ hành vi. Bản test giữ đúng thứ tự tương đối của bộ default.
behaviors:
- {name: wait, type: WaitRecovery}
- {name: rotate, type: RotateRecovery}
- {name: back_up, type: BackUpRecovery}
wait:
wait_duration: 3.0 # [s]
rotate:
full_rotation: true # quay đủ 2*pi để costmap thấy xung quanh
angular_speed: 0.4 # [rad/s] độ lớn; dấu do goal.angle quyết định
acc_lim_theta: 0.8 # [rad/s^2]
sim_granularity: 0.1 # [rad] bước quét cung lúc start
timeout: 20.0 # [s]
back_up:
backup_distance: 0.28 # [m] cố ý KHÔNG chia hết cho quãng đi mỗi tick của test
backup_distance_max: 1.0 # [m] trần cứng
linear_speed: 0.1 # [m/s] độ lớn; dấu âm do plugin đặt
acc_lim_x: 0.3 # [m/s^2]
timeout: 15.0 # [s]
# Namespace dành riêng cho test đường lỗi: param ngoài dải cho phép phải bị từ chối kèm cảnh báo,
# không được nhận nguyên giá trị.
recovery_bad:
bad_timeout:
timeout: -5.0 # [s] âm -> base phải quay về 0 (không giới hạn)
# Namespace dành riêng cho test đường lỗi: type không có khoá library_path tương ứng bên dưới.
recovery_missing_library:
behaviors:
- {name: ghost, type: GhostRecovery}
# Namespace dành riêng cho test đường lỗi: một behavior tốt, một behavior hỏng — behavior tốt vẫn
# phải được giữ lại.
recovery_partial:
behaviors:
- {name: wait, type: WaitRecovery}
- {name: ghost, type: GhostRecovery}
wait:
wait_duration: 1.0 # [s]
# Bảng symbol -> thư viện cho Boost.DLL. Thiếu khoá library_path là nguyên nhân phổ biến nhất của
# lỗi "plugin build xong nhưng runtime báo không tìm thấy".
WaitRecovery:
library_path: librecovery_core_wait_recovery
RotateRecovery:
library_path: librecovery_core_rotate_recovery
BackUpRecovery:
library_path: librecovery_core_back_up_recovery
ClearCostmapRecovery:
library_path: librecovery_core_clear_costmap_recovery
# GhostRecovery cố ý KHÔNG khai library_path — registry_test dựa vào đó để kiểm thông báo lỗi có
# nêu đích danh khoá bị thiếu hay không.

239
test/goal_semantics_test.cpp Normal file
View File

@@ -0,0 +1,239 @@
/*********************************************************************
*
* Kiểm ngữ nghĩa RecoveryGoal sau khi bỏ sentinel "0 = dùng default".
*
* Bản trước dùng `std::abs(goal.angle) > 0.0` để quyết định "caller có đặt góc không", nên một góc
* tính từ hình học ra đúng 0 bị âm thầm thay bằng pi/2 — robot quay 90 độ mà không log gì. Test này
* khoá lại: `std::optional` phân biệt được "không đặt" với "đặt bằng 0".
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kTwoPi = 2.0 * M_PI;
/// Nạp bộ behavior test qua đúng đường Boost.DLL mà runtime dùng.
struct PluginFixture
{
PluginFixture()
{
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
}
recovery_core::RecoveryBehavior* behavior(const std::string& name)
{
return recovery_test::findBehavior(registry, name);
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
};
TEST(GoalSemantics, ExplicitZeroAngleDoesNotRotate)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 0.0; // "đừng quay" — một yêu cầu hợp lệ
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(GoalSemantics, UnsetAngleUsesFullRotationDefault)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal; // angle không đặt
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}
TEST(GoalSemantics, TinyAngleIsRespectedNotReplaced)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 0.05; // nhỏ nhưng khác 0
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 0.05, 1e-3);
}
TEST(GoalSemantics, NegativeAngleRotatesClockwise)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = -1.0;
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
fixture.rig.clock.advance(0.1);
const auto result = rotate->update(fixture.rig.clock.now());
ASSERT_NE(result.velocity(), nullptr);
EXPECT_LT(result.velocity()->angular.z, 0.0);
}
TEST(GoalSemantics, AngleBeyondTwoPiIsClamped)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = 100.0; // ~16 vòng — bản cũ nhận nguyên
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
const auto result = rotate->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, kTwoPi, 1e-3);
}
TEST(GoalSemantics, UnsetDistanceUsesConfiguredDefault)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal; // distance không đặt -> backup_distance: 0.28 trong config test
ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
const auto result = back_up->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 0.28, 1e-3);
}
TEST(GoalSemantics, DistanceBeyondMaxIsClamped)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal;
goal.distance = 50.0; // backup_distance_max: 1.0
ASSERT_TRUE(back_up->start(goal, robot::Time(1000.0)));
const auto result = back_up->update(robot::Time(1000.1));
ASSERT_EQ(result.status, RecoveryStatus::kRunning);
EXPECT_NEAR(result.remaining, 1.0, 1e-3);
}
TEST(GoalSemantics, NonPositiveDistanceIsRejectedByBase)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* back_up = fixture.behavior("back_up");
ASSERT_NE(back_up, nullptr);
RecoveryGoal goal;
goal.distance = -0.1;
// distance có giá trị thì phải > 0. Base bắt trước khi plugin nhìn thấy goal.
EXPECT_FALSE(back_up->start(goal, robot::Time(1000.0)));
}
TEST(GoalSemantics, NonFiniteGoalIsRejectedByBase)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal nan_angle;
nan_angle.angle = std::numeric_limits<double>::quiet_NaN();
EXPECT_FALSE(rotate->start(nan_angle, robot::Time(1000.0)));
RecoveryGoal nan_param;
nan_param.params["angular_speed"] = std::numeric_limits<double>::infinity();
EXPECT_FALSE(rotate->start(nan_param, robot::Time(1000.0)));
}
TEST(GoalSemantics, PerRunSpeedOverrideIsApplied)
{
PluginFixture fixture;
ASSERT_TRUE(fixture.loaded);
auto* rotate = fixture.behavior("rotate");
ASSERT_NE(rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
goal.params["angular_speed"] = 0.2;
ASSERT_TRUE(rotate->start(goal, robot::Time(1000.0)));
// Đi đủ lâu để ramp gia tốc đạt trần tốc độ yêu cầu.
robot::Time now(1000.0);
double commanded = 0.0;
for (int i = 0; i < 20; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = rotate->update(now);
ASSERT_NE(result.velocity(), nullptr);
commanded = result.velocity()->angular.z;
fixture.rig.applyCommand(result.command, 0.1);
}
EXPECT_NEAR(commanded, 0.2, 1e-6);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

190
test/output_kind_test.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*********************************************************************
*
* Kiểm base CƯỠNG CHẾ bất biến họ output.
*
* Bản trước để plugin tự đặt `output_type` mỗi tick, nên nó không dùng được để route: một plugin họ
* path trả `kVelocity` ở tick đầu, còn base thì tự sinh `Velocity(zero)` cho mọi họ ở nhánh
* terminal. Test này khoá lại hành vi đúng.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <robot/node_handle.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;
/// Dựng một MockBehavior đã configure + start, sẵn sàng nhận update().
struct Fixture
{
explicit Fixture(RecoveryOutputType kind) : behavior(kind)
{
ctx = rig.ctx;
EXPECT_TRUE(behavior.configure("mock", ctx, nh));
EXPECT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
MockBehavior behavior;
};
TEST(OutputKind, NoneFamilyNeverReportsVelocity)
{
Fixture fixture(RecoveryOutputType::kNone);
// Plugin cố tình trả output vận tốc dù nó khai họ kNone.
robot_geometry_msgs::Twist rogue;
rogue.linear.x = 0.5;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.velocity(), nullptr);
// Quan trọng nhất: giá trị rogue không được rò ra ngoài dưới bất kỳ dạng nào.
EXPECT_DOUBLE_EQ(result.command.linear.x, 0.0);
}
TEST(OutputKind, PathFamilyNeverReportsVelocity)
{
Fixture fixture(RecoveryOutputType::kPath);
robot_geometry_msgs::Twist rogue;
rogue.angular.z = 1.0;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.velocity(), nullptr);
EXPECT_EQ(result.pathOut(), nullptr);
}
TEST(OutputKind, VelocityFamilyNeverReportsPath)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_nav_msgs::Path rogue;
rogue.poses.resize(2);
fixture.behavior.next_result =
recovery_core::RecoveryResult::PathOut(rogue, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.pathOut(), nullptr);
EXPECT_TRUE(result.path.poses.empty());
}
TEST(OutputKind, MatchingKindPassesThrough)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist cmd;
cmd.linear.x = -0.1;
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(cmd, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, -0.1);
}
TEST(OutputKind, NoneIsAlwaysAllowed)
{
// Một behavior họ velocity vẫn được phép nói "tick này không có output".
Fixture fixture(RecoveryOutputType::kVelocity);
fixture.behavior.next_result = recovery_core::RecoveryResult::Running();
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(result.status, RecoveryStatus::kRunning);
}
TEST(OutputKind, TerminalStopOutputMatchesFamily)
{
{
Fixture none_family(RecoveryOutputType::kNone);
none_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
ASSERT_EQ(none_family.behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
// Tick sau khi đã kết thúc: họ kNone KHÔNG được báo cáo mình phát vận tốc.
const auto after = none_family.behavior.update(robot::Time(1000.2));
EXPECT_EQ(after.output_type, RecoveryOutputType::kNone);
}
{
Fixture velocity_family(RecoveryOutputType::kVelocity);
velocity_family.behavior.next_result = recovery_core::RecoveryResult::Succeeded();
ASSERT_EQ(velocity_family.behavior.update(robot::Time(1000.1)).status,
RecoveryStatus::kSucceeded);
// Họ velocity thì ngược lại: phải có lệnh dừng tường minh để caller publish.
const auto after = velocity_family.behavior.update(robot::Time(1000.2));
ASSERT_NE(after.velocity(), nullptr);
EXPECT_DOUBLE_EQ(after.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(after.velocity()->angular.z, 0.0);
}
}
TEST(OutputKind, NonFiniteVelocityIsForcedToStop)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist bad;
bad.linear.x = std::numeric_limits<double>::quiet_NaN();
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
// NaN lọt ra cmd_vel là không được phép đi tiếp.
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_TRUE(std::isfinite(result.velocity()->linear.x));
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
}
TEST(OutputKind, InfiniteAngularVelocityIsForcedToStop)
{
Fixture fixture(RecoveryOutputType::kVelocity);
robot_geometry_msgs::Twist bad;
bad.angular.z = std::numeric_limits<double>::infinity();
fixture.behavior.next_result =
recovery_core::RecoveryResult::Velocity(bad, RecoveryStatus::kRunning);
const auto result = fixture.behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

288
test/plugin_loader_contract_test.cpp
View File

@@ -1,288 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — Boost.DLL plugin contract test.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_behavior.h>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>
#include <robot/robot.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <boost/dll/import.hpp>
#include <boost/dll/shared_library.hpp>
#include <boost/filesystem/path.hpp>
namespace
{
using Factory = recovery_core::RecoveryBehavior::RecoveryBehaviorPtr();
struct PluginConfig
{
std::string name;
std::string type;
};
std::vector<boost::dll::shared_library> libraries_;
std::vector<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr> creators_;
std::vector<std::string> name_plugins_;
std::vector<robot_geometry_msgs::PoseStamped> global_path_;
void expect(bool condition, const std::string& message)
{
if (!condition)
{
std::cerr << "[FAIL] " << message << std::endl;
std::exit(1);
}
}
std::vector<PluginConfig> getListRecoveryPlugins()
{
std::vector<PluginConfig> my_list;
robot::NodeHandle priv_nh;
if (priv_nh.hasParam("recovery_behaviors"))
{
YAML::Node my_plugins = priv_nh.getParamValue("recovery_behaviors");
if (my_plugins.IsDefined() && my_plugins.IsSequence())
{
std::set<std::string> name_plugins;
for (std::size_t i = 0; i < my_plugins.size(); ++i)
{
YAML::Node plugin_i = my_plugins[i];
// 1. Phải là map
if (!plugin_i.IsMap())
{
std::cerr<< "Recovery plugin at index " << i << " must be a map." << std::endl;
continue;
}
// 2. Phải có name và type
if (!plugin_i["name"].IsDefined() || !plugin_i["type"].IsDefined())
{
std::cerr << "Recovery plugin at index " << i << " must have 'name' and 'type'." << std::endl;
continue;
}
PluginConfig p;
try
{
p.name = plugin_i["name"].as<std::string>();
p.type = plugin_i["type"].as<std::string>();
}
catch (const YAML::Exception& e)
{
std::cerr << "Invalid recovery plugin at index " << i << ": " << e.what() << std::endl;
continue;
}
// 3. Kiểm tra duplicate name
const auto result = name_plugins.insert(p.name);
if (!result.second)
{
std::cerr << "A recovery plugin with name '" << p.name << "' already exists." << std::endl;
continue;
}
// 4. Chỉ thêm sau khi validate hoàn toàn
my_list.push_back(p);
name_plugins_.push_back(p.name);
robot::log_warning("Load plugin: name: %s, type: %s", p.name.c_str(), p.type.c_str());
}
}
else
{
std::cerr << "'recovery_behaviors' must be a sequence." << std::endl;
}
}
return my_list;
}
void testLoadPlugins()
{
std::vector<PluginConfig> my_list_plugin = getListRecoveryPlugins();
if(my_list_plugin.empty())
{
robot::log_error("No recovery plugins found in configuration.");
return;
}
for(const auto& plugin : my_list_plugin)
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(plugin.type);
if(path_file_so == "")
{
robot::log_error("Cannot find library for recovery behavior type '%s'", plugin.type.c_str());
return;
}
robot::log_info("Loading recovery behavior type '%s' from '%s'", plugin.type.c_str(), path_file_so.c_str());
try
{
// 1. Load library vào local handle.
boost::dll::shared_library library(path_file_so);
// 2. Lấy factory alias.
auto& factory = library.get_alias<Factory>(plugin.type);
// 3. Factory tạo behavior object.
recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = factory();
if (!behavior)
{
robot::log_error("Factory returned nullptr for '%s'", plugin.type.c_str());
return;
}
// 4. Chuyển quyền giữ library vào storage sống lâu dài.
libraries_.push_back(std::move(library));
recovery_core::RecoveryContext ctx;
ctx.global_path = &global_path_;
behavior->configure(plugin.name, ctx);
creators_.push_back(behavior);
}
catch (const std::exception& e)
{
robot::log_error("Failed to load recovery behavior '%s': %s", plugin.type.c_str(), e.what());
return;
}
// expect(static_cast<bool>(behavior), "Failed to load plugin: " + plugin.name + " of type: " + plugin.type);
}
}
void testRotatePlugin()
{
for(const auto& behavior : creators_)
{
if(behavior->getNameRecoveryBehavior() == "rotation_rc")
{
// Caller đặt góc quay RUNTIME = pi/2 (90 độ) cho lượt này.
recovery_core::RecoveryGoal goal;
goal.angle = 1.57079632679;
const recovery_core::RecoveryResult started = behavior->start(goal);
expect(started.status == recovery_core::RecoveryStatus::kRunning,
"rotate must be running right after start");
const recovery_core::RecoveryResult first = behavior->update();
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 for +angle");
expect(first.progress >= 0.0 && first.progress < 1.0,
"rotate progress must advance within [0,1)");
expect(first.remaining > 0.0, "rotate must report remaining angle while running");
recovery_core::RecoveryResult last = first;
for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
{
last = behavior->update();
}
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");
expect(std::abs(last.progress - 1.0) < 1e-9, "rotate must report full progress on success");
expect(last.remaining < 1e-9, "rotate must report zero remaining on success");
}
}
}
void testBackupPlugin()
{
for(const auto& behavior : creators_)
{
if(behavior->getNameRecoveryBehavior() == "backward_rc")
{
// Caller đặt khoảng lùi RUNTIME = 0.3 m cho lượt này.
recovery_core::RecoveryGoal goal;
goal.distance = 0.3;
behavior->start(goal);
const recovery_core::RecoveryResult first = behavior->update();
expect(first.status == recovery_core::RecoveryStatus::kRunning,
"backup first cycle must be running");
expect(first.command.linear.x < 0.0, "backup must command negative linear.x");
recovery_core::RecoveryResult last = first;
for (int i = 0; i < 1000 && last.status == recovery_core::RecoveryStatus::kRunning; ++i)
{
last = behavior->update();
}
expect(last.status == recovery_core::RecoveryStatus::kSucceeded,
"backup must finish within bounded cycles");
expect(std::abs(last.command.linear.x) < 1e-9,
"backup must return a zero command when complete");
expect(std::abs(last.progress - 1.0) < 1e-9, "backup must report full progress on success");
}
}
}
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")
{
behavior->start(recovery_core::RecoveryGoal());
const recovery_core::RecoveryResult result = behavior->update();
expect(result.status == recovery_core::RecoveryStatus::kSucceeded,
"regen path must succeed with a non-empty global path");
expect(result.output_type == recovery_core::RecoveryOutputType::kPath,
"regen path must return path output");
expect(result.path.poses.size() == global_path_.size(),
"regen path must preserve the global path size");
global_path_ = result.path.poses;
}
}
}
} // namespace
int main()
{
testLoadPlugins();
if(creators_.empty()) return 0;
// for(auto& behavior : creators_)
// {
// std::cout<<behavior->getNameRecoveryBehavior()<<std::endl;
// }
testRegenPathPlugin();
testRotatePlugin();
testBackupPlugin();
std::cout << "[PASS] recovery_core plugin loader contract" << std::endl;
return 0;
}

188
test/pose_progress_test.cpp Normal file
View File

@@ -0,0 +1,188 @@
/*********************************************************************
*
* Kiểm tiến độ đo bằng POSE THẬT, không dead-reckon theo chu kỳ cấu hình.
*
* Đây là test cho lỗi nặng nhất của bản trước: quãng đi được tính bằng
* `|cmd.linear.x| * control_period` với `control_period` lấy từ YAML. Control loop chạy chậm gấp N
* lần là robot đi quá quãng gấp N lần, còn bánh trượt thì vẫn báo hoàn thành.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kConfiguredDistance = 0.28; // [m] khớp `recovery/back_up/backup_distance`
struct BackUpFixture
{
BackUpFixture()
{
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
back_up = recovery_test::findBehavior(registry, "back_up");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* back_up = nullptr;
};
/**
* @brief Chạy trọn một lượt lùi với chu kỳ @p dt, mô phỏng robot đi đúng lệnh phát ra.
* @return quãng đường thực tế robot đã lùi [m].
*/
double runBackup(BackUpFixture& fixture, double dt, int max_ticks = 2000)
{
EXPECT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < max_ticks; ++i)
{
now = robot::Time(now.toSec() + dt);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
break;
}
fixture.rig.applyCommand(result.command, dt);
}
return -fixture.rig.pose.rawPose().x; // lùi theo -x
}
TEST(PoseProgress, NominalRateStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 0.1);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, FiveTimesSlowerLoopStillStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
// Control loop chạy chậm gấp 5. Bản cũ sai ~400% ở đây vì nhân với hằng số config.
const double traveled = runBackup(fixture, 0.5);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, TwentyTimesSlowerLoopStillStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 2.0);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, FasterLoopStopsAtRequestedDistance)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
const double traveled = runBackup(fixture, 0.02);
EXPECT_NEAR(traveled, kConfiguredDistance, 0.05 * kConfiguredDistance);
}
TEST(PoseProgress, StalledRobotNeverReportsSuccess)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
// Bánh trượt hoàn toàn: lệnh vẫn phát nhưng pose không đổi. Bản cũ tích phân vận tốc LỆNH nên vẫn
// báo kSucceeded; bản này phải chạy tới khi timeout chứ không được nói dối.
robot::Time now(1000.0);
bool reported_success = false;
for (int i = 0; i < 200; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.status == RecoveryStatus::kSucceeded)
{
reported_success = true;
break;
}
if (result.terminal())
{
break; // timeout -> kFailed, đúng như mong đợi
}
// KHÔNG applyCommand: robot không nhúc nhích.
}
EXPECT_FALSE(reported_success);
}
TEST(PoseProgress, ProgressAndRemainingTrackRealPose)
{
BackUpFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.back_up, nullptr);
ASSERT_TRUE(fixture.back_up->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
double last_progress = -1.0;
for (int i = 0; i < 10; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.back_up->update(now);
if (result.terminal())
{
break;
}
const double traveled = -fixture.rig.pose.rawPose().x;
EXPECT_NEAR(result.remaining, kConfiguredDistance - traveled, 1e-6);
EXPECT_GE(result.progress, last_progress);
EXPECT_GE(result.progress, 0.0);
EXPECT_LE(result.progress, 1.0);
last_progress = result.progress;
fixture.rig.applyCommand(result.command, 0.1);
}
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

280
test/recovery_lifecycle_test.cpp Normal file
View File

@@ -0,0 +1,280 @@
/*********************************************************************
*
* Kiểm bất biến vòng đời của RecoveryBehavior: guard configure/start/update, kiểm ngữ cảnh bắt
* buộc theo họ output, dt đo bằng đồng hồ thật, và đường cancel.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <robot/node_handle.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;
TEST(RecoveryLifecycle, StartBeforeConfigureFails)
{
MockBehavior behavior(RecoveryOutputType::kNone);
const robot::Time now(1000.0);
EXPECT_FALSE(behavior.start(RecoveryGoal(), now));
EXPECT_EQ(behavior.start_calls, 0);
}
TEST(RecoveryLifecycle, UpdateBeforeStartReturnsFailedStopOutput)
{
MockBehavior behavior(RecoveryOutputType::kNone);
const robot::Time now(1000.0);
const auto result = behavior.update(now);
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
EXPECT_EQ(behavior.update_calls, 0);
}
TEST(RecoveryLifecycle, ConfigureTwiceRejected)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
EXPECT_TRUE(behavior.configure("mock", ctx, nh));
// Gọi lần hai với ctx khác là lỗi lập trình của caller — phải báo, không được nuốt.
EXPECT_FALSE(behavior.configure("mock", ctx, nh));
EXPECT_EQ(behavior.configure_calls, 1);
}
TEST(RecoveryLifecycle, EmptyInstanceNameRejected)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
EXPECT_FALSE(behavior.configure("", ctx, nh));
EXPECT_EQ(behavior.configure_calls, 0);
}
TEST(RecoveryLifecycle, VelocityFamilyRequiresPoseProvider)
{
VelocityRig rig;
MockBehavior behavior(RecoveryOutputType::kVelocity);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx = rig.ctx;
ctx.pose = nullptr;
// Không có pose thì tiến độ chỉ có thể dead-reckon — đúng lớp lỗi Phase 3 phải diệt.
EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}
TEST(RecoveryLifecycle, VelocityFamilyRequiresCollisionChecker)
{
VelocityRig rig;
MockBehavior behavior(RecoveryOutputType::kVelocity);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx = rig.ctx;
ctx.collision = nullptr;
EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}
TEST(RecoveryLifecycle, PathFamilyRequiresPlanProvider)
{
MockBehavior behavior(RecoveryOutputType::kPath);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
EXPECT_FALSE(behavior.configure("mock", ctx, nh));
}
TEST(RecoveryLifecycle, NoneFamilyNeedsNoPorts)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
EXPECT_TRUE(behavior.configure("mock", ctx, nh));
}
TEST(RecoveryLifecycle, ConfigureFailsWhenPluginRejects)
{
MockBehavior behavior(RecoveryOutputType::kNone);
behavior.configure_ok = false;
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
EXPECT_FALSE(behavior.configure("mock", ctx, nh));
// Không được coi là đã cấu hình: start() sau đó phải hỏng.
EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(RecoveryLifecycle, StartRejectedWhenPluginRefuses)
{
MockBehavior behavior(RecoveryOutputType::kNone);
behavior.start_ok = false;
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
EXPECT_FALSE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(behavior.status(), RecoveryStatus::kFailed);
}
TEST(RecoveryLifecycle, TerminalStateDoesNotTickAgain)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.next_result = recovery_core::RecoveryResult::Succeeded();
ASSERT_EQ(behavior.update(robot::Time(1000.1)).status, RecoveryStatus::kSucceeded);
const int calls_after_success = behavior.update_calls;
const auto again = behavior.update(robot::Time(1000.2));
EXPECT_EQ(again.status, RecoveryStatus::kSucceeded);
EXPECT_EQ(behavior.update_calls, calls_after_success); // không gọi thêm onUpdate
}
TEST(RecoveryLifecycle, CancelYieldsCancelledStopOutput)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.cancel();
const auto result = behavior.update(robot::Time(1000.1));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
EXPECT_EQ(behavior.cancel_calls, 1);
EXPECT_EQ(behavior.update_calls, 0); // cancel thay thế tick, không chạy logic plugin
}
TEST(RecoveryLifecycle, CancelOnVelocityFamilyEmitsExplicitZeroTwist)
{
VelocityRig rig;
MockBehavior behavior(RecoveryOutputType::kVelocity);
robot::NodeHandle nh;
ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.cancel();
const auto result = behavior.update(robot::Time(1000.1));
// Họ velocity phải nhận lệnh dừng TƯỜNG MINH: caller đang lấy cmd_vel từ đây.
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(RecoveryLifecycle, DtMeasuredFromRealClockNotConfiguredPeriod)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.update(robot::Time(1000.0));
EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9); // tick đầu ngay sau start
behavior.update(robot::Time(1000.5));
EXPECT_NEAR(behavior.last_dt, 0.5, 1e-6);
behavior.update(robot::Time(1002.5));
EXPECT_NEAR(behavior.last_dt, 2.0, 1e-6); // loop chạy chậm -> dt lớn, không phải hằng số config
}
TEST(RecoveryLifecycle, BackwardClockYieldsZeroDt)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.update(robot::Time(1001.0));
behavior.update(robot::Time(1000.5)); // đồng hồ đi lùi
EXPECT_NEAR(behavior.last_dt, 0.0, 1e-9);
}
TEST(RecoveryLifecycle, ElapsedTracksClockFromStart)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
const auto first = behavior.update(robot::Time(1001.25));
EXPECT_NEAR(first.elapsed, 1.25, 1e-6);
EXPECT_NEAR(behavior.elapsed(), 1.25, 1e-6);
const auto second = behavior.update(robot::Time(1004.0));
EXPECT_NEAR(second.elapsed, 4.0, 1e-6);
}
TEST(RecoveryLifecycle, RestartResetsElapsed)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
behavior.update(robot::Time(1005.0));
ASSERT_NEAR(behavior.elapsed(), 5.0, 1e-6);
behavior.next_result = recovery_core::RecoveryResult::Running();
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(2000.0)));
EXPECT_NEAR(behavior.elapsed(), 0.0, 1e-9);
const auto result = behavior.update(robot::Time(2000.5));
EXPECT_NEAR(result.elapsed, 0.5, 1e-6);
}
TEST(RecoveryLifecycle, GoalIsHandedToPluginVerbatim)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
RecoveryGoal goal;
goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
goal.angle = 1.5;
goal.params["custom"] = 7.0;
ASSERT_TRUE(behavior.start(goal, robot::Time(1000.0)));
EXPECT_EQ(behavior.last_goal.trigger, recovery_core::RecoveryTrigger::kOscillation);
ASSERT_TRUE(behavior.last_goal.angle.has_value());
EXPECT_DOUBLE_EQ(*behavior.last_goal.angle, 1.5);
EXPECT_DOUBLE_EQ(behavior.last_goal.param("custom", 0.0), 7.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

213
test/recovery_test_utils.h Normal file
View File

@@ -0,0 +1,213 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* recovery_core — tiện ích dùng chung cho test.
*
* Author: DuongTD
*********************************************************************/
#ifndef RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_
#define RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_
#include <cmath>
#include <string>
#include <utility>
#include <vector>
#include <nav_test_harness/fake_clock.h>
#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_math.h>
#include <recovery_core/recovery_registry.h>
namespace recovery_test
{
/// @brief Behavior trong @p registry mang tên @p name, hoặc nullptr.
inline recovery_core::RecoveryBehavior* findBehavior(const recovery_core::RecoveryRegistry& registry,
const std::string& name)
{
for (std::size_t i = 0; i < registry.size(); ++i)
{
if (registry.nameAt(i) == name)
{
return registry.at(i);
}
}
return nullptr;
}
/**
* @brief Nối `nav_test_harness::FakePoseProvider` vào cổng của recovery_core.
*
* Hai interface cố ý tách nhau: `recovery_core` không được phụ thuộc gói test harness, và
* `nav_test_harness` phục vụ nhiều gói khác nhau. Adapter mỏng ở đây chính là thứ `RecoveryRunner`
* sẽ làm với `move_base2::PosePort` ở Phase 4.
*/
class HarnessPoseProvider final : public recovery_core::PoseProvider
{
public:
explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
{
}
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
{
return fake_ != nullptr && fake_->getRobotPose(pose);
}
private:
nav_test_harness::FakePoseProvider* fake_ = nullptr;
};
/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng của recovery_core.
class HarnessCollisionChecker final : public recovery_core::CollisionChecker
{
public:
explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
{
}
double footprintCost(double x, double y, double theta) const override
{
return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
}
private:
nav_test_harness::FakeCollisionChecker* fake_ = nullptr;
};
/// @brief Nguồn plan đơn giản do test bơm thẳng.
class StubPlanProvider final : public recovery_core::PlanProvider
{
public:
void setPlan(std::vector<robot_geometry_msgs::PoseStamped> plan)
{
plan_ = std::move(plan);
}
bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const override
{
if (plan_.empty())
{
return false;
}
out = plan_;
return true;
}
private:
std::vector<robot_geometry_msgs::PoseStamped> plan_;
};
/**
* @brief Behavior giả, cho phép test điều khiển từng hook.
*
* Dùng để kiểm phần **base** (guard vòng đời, timeout, cưỡng chế họ output) mà không phụ thuộc vào
* hành vi của plugin thật.
*/
class MockBehavior final : public recovery_core::RecoveryBehavior
{
public:
explicit MockBehavior(recovery_core::RecoveryOutputType kind) : kind_(kind)
{
next_result = recovery_core::RecoveryResult::Running();
}
recovery_core::RecoveryOutputType outputKind() const override
{
return kind_;
}
// Núm điều khiển cho test.
bool configure_ok = true;
bool start_ok = true;
recovery_core::RecoveryResult next_result;
// Ghi nhận để assert.
int configure_calls = 0;
int start_calls = 0;
int update_calls = 0;
int cancel_calls = 0;
double last_dt = -1.0;
recovery_core::RecoveryGoal last_goal;
protected:
bool onConfigure(robot::NodeHandle& /*nh*/) override
{
++configure_calls;
return configure_ok;
}
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
++start_calls;
last_goal = goal;
return start_ok;
}
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{
++update_calls;
last_dt = dt;
return next_result;
}
recovery_core::RecoveryResult onCancel() override
{
++cancel_calls;
return recovery_core::RecoveryBehavior::onCancel();
}
private:
recovery_core::RecoveryOutputType kind_;
};
/**
* @brief Bộ đồ nghề đầy đủ cho một test plugin họ velocity.
*
* Gom costmap giả, pose giả, collision checker giả và đồng hồ giả, kèm hàm mô phỏng robot chạy
* theo đúng lệnh vận tốc mà behavior phát ra.
*/
struct VelocityRig
{
VelocityRig(double span_m = 8.0, double resolution = 0.05,
double footprint_length = 0.6, double footprint_width = 0.4)
: costmap(nav_test_harness::FakeCostmap::centered(span_m, resolution))
, checker(&costmap,
nav_test_harness::FakeCollisionChecker::rectangleFootprint(footprint_length,
footprint_width))
, pose_port(&pose)
, collision_port(&checker)
{
ctx.pose = &pose_port;
ctx.collision = &collision_port;
ctx.plan = &plan;
}
/// @brief Cho robot chạy theo @p command trong @p dt giây, cập nhật pose giả.
void applyCommand(const robot_geometry_msgs::Twist& command, double dt)
{
const double yaw = pose.rawPose().theta;
pose.moveBy(command.linear.x * std::cos(yaw) * dt, command.linear.x * std::sin(yaw) * dt,
command.angular.z * dt);
}
nav_test_harness::FakeCostmap costmap;
nav_test_harness::FakeCollisionChecker checker;
nav_test_harness::FakePoseProvider pose;
nav_test_harness::FakeClock clock;
StubPlanProvider plan;
HarnessPoseProvider pose_port;
HarnessCollisionChecker collision_port;
recovery_core::RecoveryContext ctx;
};
} // namespace recovery_test
#endif // RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_

181
test/registry_test.cpp Normal file
View File

@@ -0,0 +1,181 @@
/*********************************************************************
*
* Kiểm đường nạp plugin thật: YAML -> library_path -> Boost.DLL -> configure.
*
* Bản test cũ của gói KHÔNG kiểm được gì: nó `return` khi không nạp được plugin nào và `main` trả 0,
* nên "không plugin nào chạy" cũng in [PASS]. Test này phải fail được.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <memory>
#include <string>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryOutputType;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;
TEST(Registry, LoadsDeclaredBehaviorsInOrder)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
// Thứ tự CHÍNH LÀ hành vi: caller thử behavior 0 trước.
ASSERT_EQ(registry.size(), 3u);
EXPECT_EQ(registry.nameAt(0), "wait");
EXPECT_EQ(registry.nameAt(1), "rotate");
EXPECT_EQ(registry.nameAt(2), "back_up");
}
TEST(Registry, LoadedBehaviorsReportCorrectOutputKind)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
ASSERT_EQ(registry.size(), 3u);
EXPECT_EQ(registry.at(0)->outputKind(), RecoveryOutputType::kNone); // wait
EXPECT_EQ(registry.at(1)->outputKind(), RecoveryOutputType::kVelocity); // rotate
EXPECT_EQ(registry.at(2)->outputKind(), RecoveryOutputType::kVelocity); // back_up
}
TEST(Registry, PerInstanceParamsComeFromItsOwnNamespace)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
// `recovery/rotate/timeout: 20.0`, `recovery/back_up/timeout: 15.0`, `recovery/wait` không khai.
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "rotate")->timeout(), 20.0);
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "back_up")->timeout(), 15.0);
EXPECT_DOUBLE_EQ(recovery_test::findBehavior(registry, "wait")->timeout(), 0.0);
}
TEST(Registry, MissingLibraryPathIsReportedAndFails)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
// `GhostRecovery` được khai trong danh sách nhưng không có khoá library_path.
EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_missing_library", rig.ctx));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, OneBadBehaviorDoesNotDropTheGoodOnes)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
// Một đường phục hồi hỏng không nên xoá sạch các đường còn lại.
EXPECT_FALSE(registry.loadFromConfig(nh, "recovery_partial", rig.ctx));
ASSERT_EQ(registry.size(), 1u);
EXPECT_EQ(registry.nameAt(0), "wait");
}
TEST(Registry, MissingBehaviorListFails)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
EXPECT_FALSE(registry.loadFromConfig(nh, "namespace_khong_ton_tai", rig.ctx));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, IndexOutOfRangeIsSafe)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
EXPECT_EQ(registry.at(99), nullptr);
EXPECT_TRUE(registry.nameAt(99).empty());
}
TEST(Registry, RegisterBehaviorRejectsNull)
{
recovery_core::RecoveryRegistry registry;
EXPECT_FALSE(registry.registerBehavior(nullptr));
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, RegisterBehaviorAppendsInOrder)
{
recovery_core::RecoveryRegistry registry;
auto first = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);
auto second = std::make_shared<MockBehavior>(RecoveryOutputType::kNone);
ASSERT_TRUE(registry.registerBehavior(first));
ASSERT_TRUE(registry.registerBehavior(second));
ASSERT_EQ(registry.size(), 2u);
EXPECT_EQ(registry.at(0), first.get());
EXPECT_EQ(registry.at(1), second.get());
}
TEST(Registry, ClearReleasesBehaviors)
{
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
ASSERT_TRUE(registry.loadFromConfig(nh, "recovery", rig.ctx));
ASSERT_GT(registry.size(), 0u);
registry.clear();
EXPECT_EQ(registry.size(), 0u);
}
TEST(Registry, BehaviorsStayUsableAfterLoaderScopeEnds)
{
VelocityRig rig;
recovery_core::RecoveryRegistry registry;
{
// NodeHandle chết trước registry: behavior vẫn phải sống, vì registry mới là thứ giữ .so.
robot::NodeHandle scoped_nh;
ASSERT_TRUE(registry.loadFromConfig(scoped_nh, "recovery", rig.ctx));
}
auto* wait = recovery_test::findBehavior(registry, "wait");
ASSERT_NE(wait, nullptr);
ASSERT_TRUE(wait->start(recovery_core::RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(wait->update(robot::Time(1003.0)).status, recovery_core::RecoveryStatus::kSucceeded);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

216
test/rotate_safety_test.cpp Normal file
View File

@@ -0,0 +1,216 @@
/*********************************************************************
*
* Kiểm quét cung và đo góc bằng pose thật của RotateRecovery.
*
* Bản trước không dùng `ctx()` một lần nào trong toàn file: quay mù, và đếm góc bằng
* `angular_speed * control_period`.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cmath>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kTwoPi = 2.0 * M_PI;
struct RotateFixture
{
RotateFixture()
{
rig.pose.setPose(0.0, 0.0, 0.0);
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
rotate = recovery_test::findBehavior(registry, "rotate");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* rotate = nullptr;
};
TEST(RotateSafety, RefusesToStartWhenArcIsBlocked)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Footprint 0.6 x 0.4 quanh gốc: khi quay 90 độ, mũi robot quét tới y ~ +/-0.3.
// Đặt vật cản ở đó -> cung quay bị chặn dù vị trí hiện tại vẫn trống.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_FALSE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, StartsWhenArcIsClear)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
EXPECT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
}
TEST(RotateSafety, PartialArcAvoidsBlockedSector)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
// Vật cản chỉ chặn khi robot đã quay đáng kể; cung nhỏ vẫn phải đi được.
fixture.rig.costmap.setLethalCircle(0.0, 0.32, 0.06);
RecoveryGoal small_arc;
small_arc.angle = 0.05;
EXPECT_TRUE(fixture.rotate->start(small_arc, robot::Time(1000.0)));
}
TEST(RotateSafety, StopsWithZeroCommandWhenPoseIsLost)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rig.pose.setAvailable(false);
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kFailed);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
TEST(RotateSafety, RefusesToStartWhenPoseIsUnavailable)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
fixture.rig.pose.setAvailable(false);
EXPECT_FALSE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
}
TEST(RotateSafety, FullRotationCountsPastPiCorrectly)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = kTwoPi;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
// Cộng dồn góc quay THẬT do test tự đo, độc lập với con số plugin báo.
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
robot::Time now(1000.0);
bool finished = false;
for (int i = 0; i < 2000; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
finished = true;
break;
}
fixture.rig.applyCommand(result.command, 0.1);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
ASSERT_TRUE(finished);
// Quay đủ vòng: phép chuẩn hoá từng bước phải đếm đúng qua mốc pi, không bị wrap về 0.
EXPECT_NEAR(swept, kTwoPi, 0.05 * kTwoPi);
}
TEST(RotateSafety, SlowLoopDoesNotOvershoot)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
RecoveryGoal goal;
goal.angle = 1.0;
ASSERT_TRUE(fixture.rotate->start(goal, robot::Time(1000.0)));
double swept = 0.0;
double previous_yaw = fixture.rig.pose.rawPose().theta;
// dt gấp 5 lần nhịp thường.
robot::Time now(1000.0);
for (int i = 0; i < 200; ++i)
{
now = robot::Time(now.toSec() + 0.5);
const auto result = fixture.rotate->update(now);
if (result.terminal())
{
break;
}
fixture.rig.applyCommand(result.command, 0.5);
const double yaw = fixture.rig.pose.rawPose().theta;
swept += std::abs(recovery_core::normalizeAngle(yaw - previous_yaw));
previous_yaw = yaw;
}
EXPECT_NEAR(swept, 1.0, 0.05);
}
TEST(RotateSafety, CancelEmitsZeroCommand)
{
RotateFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.rotate, nullptr);
ASSERT_TRUE(fixture.rotate->start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(fixture.rotate->update(robot::Time(1000.1)).status, RecoveryStatus::kRunning);
fixture.rotate->cancel();
const auto result = fixture.rotate->update(robot::Time(1000.2));
EXPECT_EQ(result.status, RecoveryStatus::kCancelled);
ASSERT_NE(result.velocity(), nullptr);
EXPECT_DOUBLE_EQ(result.velocity()->angular.z, 0.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

132
test/timeout_test.cpp Normal file
View File

@@ -0,0 +1,132 @@
/*********************************************************************
*
* Kiểm `elapsed` + `timeout` của base.
*
* Bản trước khai `RecoveryResult::elapsed` trong header nhưng KHÔNG nơi nào ghi vào nó, nên caller
* không có cách phát hiện recovery treo. Test này khoá cả hai chiều: elapsed phải bám đồng hồ thật,
* và quá timeout phải là kFailed kèm stop output đúng họ.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <string>
#include <robot/node_handle.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::MockBehavior;
using recovery_test::VelocityRig;
/// NodeHandle trỏ vào một namespace có sẵn khoá `timeout` trong config test.
robot::NodeHandle timeoutNodeHandle(const std::string& ns)
{
robot::NodeHandle root;
return robot::NodeHandle(root, ns);
}
TEST(Timeout, DisabledByDefault)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
// Không khai `timeout` trong namespace này -> 0 = không giới hạn.
EXPECT_DOUBLE_EQ(behavior.timeout(), 0.0);
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
const auto result = behavior.update(robot::Time(1000.0 + 3600.0));
EXPECT_EQ(result.status, RecoveryStatus::kRunning);
}
TEST(Timeout, ReadFromConfiguredNamespace)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate");
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
EXPECT_DOUBLE_EQ(behavior.timeout(), 20.0);
}
TEST(Timeout, ExceededYieldsFailedAndStopsTicking)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate"); // timeout: 20 s
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
ASSERT_EQ(behavior.update(robot::Time(1019.0)).status, RecoveryStatus::kRunning);
const int calls_before = behavior.update_calls;
const auto timed_out = behavior.update(robot::Time(1020.5));
EXPECT_EQ(timed_out.status, RecoveryStatus::kFailed);
EXPECT_NEAR(timed_out.elapsed, 20.5, 1e-6);
EXPECT_FALSE(timed_out.message.empty());
// Quá hạn thì KHÔNG giao quyền cho plugin nữa.
EXPECT_EQ(behavior.update_calls, calls_before);
}
TEST(Timeout, VelocityFamilyStopsWithExplicitZeroTwist)
{
VelocityRig rig;
MockBehavior behavior(RecoveryOutputType::kVelocity);
robot::NodeHandle nh = timeoutNodeHandle("recovery/rotate"); // timeout: 20 s
ASSERT_TRUE(behavior.configure("mock", rig.ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
const auto timed_out = behavior.update(robot::Time(1021.0));
ASSERT_EQ(timed_out.status, RecoveryStatus::kFailed);
ASSERT_NE(timed_out.velocity(), nullptr);
EXPECT_DOUBLE_EQ(timed_out.velocity()->linear.x, 0.0);
EXPECT_DOUBLE_EQ(timed_out.velocity()->angular.z, 0.0);
}
TEST(Timeout, ElapsedIsSetOnEveryResult)
{
MockBehavior behavior(RecoveryOutputType::kNone);
robot::NodeHandle nh;
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
ASSERT_TRUE(behavior.start(RecoveryGoal(), robot::Time(1000.0)));
// Plugin không đặt elapsed; base phải điền vào.
behavior.next_result = recovery_core::RecoveryResult::Running();
EXPECT_NEAR(behavior.update(robot::Time(1002.0)).elapsed, 2.0, 1e-6);
behavior.next_result = recovery_core::RecoveryResult::Succeeded();
EXPECT_NEAR(behavior.update(robot::Time(1007.5)).elapsed, 7.5, 1e-6);
}
TEST(Timeout, OutOfRangeConfigFallsBackToDisabled)
{
MockBehavior behavior(RecoveryOutputType::kNone);
// `recovery/bad_timeout` khai timeout âm -> phải cảnh báo và về 0 chứ không nhận giá trị âm.
robot::NodeHandle nh = timeoutNodeHandle("recovery_bad/bad_timeout");
recovery_core::RecoveryContext ctx;
ASSERT_TRUE(behavior.configure("mock", ctx, nh));
EXPECT_DOUBLE_EQ(behavior.timeout(), 0.0);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

179
test/wait_recovery_test.cpp Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* Kiểm WaitRecovery — behavior mới của bộ default.
*
* Đây là recovery an toàn nhất (robot không di chuyển) và hữu dụng nhất cho AMR trong kho, nơi phần
* lớn tình huống chặn đường là vật cản động. Nó cũng là chỗ rẻ nhất để chứng minh đường `elapsed`
* của base chạy đúng theo đồng hồ thật.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <cstdlib>
#include <robot/node_handle.h>
#include <recovery_core/recovery_registry.h>
#include "recovery_test_utils.h"
namespace
{
using recovery_core::RecoveryGoal;
using recovery_core::RecoveryOutputType;
using recovery_core::RecoveryStatus;
using recovery_test::VelocityRig;
constexpr double kConfiguredWait = 3.0; // [s] khớp `recovery/wait/wait_duration`
struct WaitFixture
{
WaitFixture()
{
loaded = registry.loadFromConfig(nh, "recovery", rig.ctx);
wait = recovery_test::findBehavior(registry, "wait");
}
VelocityRig rig;
robot::NodeHandle nh;
recovery_core::RecoveryRegistry registry;
bool loaded = false;
recovery_core::RecoveryBehavior* wait = nullptr;
};
TEST(WaitRecovery, DeclaresNoOutputFamily)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
EXPECT_EQ(fixture.wait->outputKind(), RecoveryOutputType::kNone);
}
TEST(WaitRecovery, NeverEmitsVelocity)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
robot::Time now(1000.0);
for (int i = 0; i < 60; ++i)
{
now = robot::Time(now.toSec() + 0.1);
const auto result = fixture.wait->update(now);
// Behavior đứng yên tuyệt đối không được làm caller tưởng nó đang lái robot.
EXPECT_EQ(result.velocity(), nullptr);
EXPECT_EQ(result.output_type, RecoveryOutputType::kNone);
if (result.terminal())
{
break;
}
}
}
TEST(WaitRecovery, SucceedsAfterConfiguredDuration)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1002.9)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, CountsByClockNotByTickCount)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
// Một tick duy nhất nhưng nhảy qua trọn thời lượng: phải xong ngay, không cần đủ số nhịp.
const auto result = fixture.wait->update(robot::Time(1000.0 + kConfiguredWait));
EXPECT_EQ(result.status, RecoveryStatus::kSucceeded);
EXPECT_NEAR(result.elapsed, kConfiguredWait, 1e-6);
}
TEST(WaitRecovery, ProgressAdvancesMonotonically)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
double last = -1.0;
for (int i = 1; i <= 5; ++i)
{
const auto result = fixture.wait->update(robot::Time(1000.0 + 0.5 * i));
EXPECT_GE(result.progress, last);
EXPECT_GE(result.remaining, 0.0);
last = result.progress;
}
}
TEST(WaitRecovery, PerRunDurationOverride)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
RecoveryGoal goal;
goal.params["wait_duration"] = 1.0;
ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1000.5)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1001.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, InvalidOverrideFallsBackToConfiguredDuration)
{
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
RecoveryGoal goal;
goal.params["wait_duration"] = -5.0; // vô lý -> phải cảnh báo và dùng default
ASSERT_TRUE(fixture.wait->start(goal, robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1002.0)).status, RecoveryStatus::kRunning);
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
TEST(WaitRecovery, NeedsNoPoseOrCollisionPorts)
{
// Điểm mạnh của WaitRecovery: chạy được cả khi TF hỏng, nên nó là đường phục hồi cuối cùng còn
// dùng được khi mọi thứ khác đã mất pose.
WaitFixture fixture;
ASSERT_TRUE(fixture.loaded);
ASSERT_NE(fixture.wait, nullptr);
fixture.rig.pose.setAvailable(false);
ASSERT_TRUE(fixture.wait->start(RecoveryGoal(), robot::Time(1000.0)));
EXPECT_EQ(fixture.wait->update(robot::Time(1003.0)).status, RecoveryStatus::kSucceeded);
}
} // namespace
int main(int argc, char** argv)
{
#ifdef RECOVERY_CORE_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", RECOVERY_CORE_TEST_CONFIG_DIR, 0);
#endif
#ifdef RECOVERY_CORE_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", RECOVERY_CORE_TEST_LIBRARY_DIR, 0);
#endif
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}