122 lines
6.1 KiB
Markdown
122 lines
6.1 KiB
Markdown
# recovery_core
|
|
|
|
Interface tick-based cho **recovery behavior** của navigation stack, kèm bộ behavior mặc định.
|
|
|
|
## Phạm vi
|
|
|
|
- Đị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
|
|
|
|
```cpp
|
|
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 giữ toàn bộ bất
|
|
biến, plugin chỉ triển khai hook `onConfigure` / `onStart` / `onUpdate` / `onCancel`.
|
|
|
|
## Ba họ output
|
|
|
|
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.
|
|
|
|
| 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_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 và test
|
|
|
|
```bash
|
|
catkin_make --pkg recovery_core
|
|
|
|
# Đă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
|
|
```
|
|
|
|
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.
|
|
|
|
Viết plugin mới: [docs/PLUGIN_GUIDE.md](docs/PLUGIN_GUIDE.md).
|