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

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

138
README.md
View File

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