optimal & fix file cmake

2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
--- a/PLAN.md
+++ b/PLAN.md
@@ -1,430 +1,43 @@
-# PLAN - `recovery_core`
-
-`recovery_core` là package **interface** cho recovery behavior trong navigation stack ROS-like
-T800. Package này không chứa thuật toán recovery cụ thể; nó định nghĩa contract chung để các
-plugin recovery có thể trả trạng thái, velocity command hoặc path.
-
-## 1. Mục Tiêu
-
- Cung cấp base class `recovery_core::RecoveryBehavior` cho các hành vi recovery.
- Dựa trên pattern `robot_nav_core::RecoveryBehavior`, nhưng mở rộng output để bao được 3 nhóm:
-  - recovery không output, ví dụ clear/reset costmap;
-  - recovery sinh vận tốc theo từng cycle, ví dụ rotate/backup;
-  - recovery sinh path, ví dụ regen path hoặc detour path.
- Dùng stack ROS-like trong workspace (`robot_*`, `tf3`, `robot_costmap_2d`), không phụ thuộc
-  `roscpp` hoặc ROS master thật.
- Giữ core sạch: plugin trả `RecoveryResult`; caller/adapter quyết định publish command, thay
-  path, clear service hoặc request replan.
-
-## 2. Ranh Giới Thiết Kế
-
-### 2.1. `recovery_core` chịu trách nhiệm
-
- Định nghĩa interface `RecoveryBehavior`.
- Định nghĩa result contract:
-  - `RecoveryStatus`
-  - `RecoveryOutputType`
-  - `RecoveryResult`
- Cung cấp ngữ cảnh + mục tiêu runtime:
-  - `RecoveryContext` (tf/costmap/global_path)
-  - `RecoveryGoal` (angle/distance/target_pose/params)
- Param riêng plugin đọc trong `onConfigure()`; core base không giữ timeout chung.
- Cung cấp docs/test stub để plugin sau này implement đúng contract.
-
-### 2.2. `recovery_core` không chịu trách nhiệm
-
- Không implement recovery cụ thể như clear costmap, rotate, backup, regen path.
- Không publish `cmd_vel`.
- Không gọi ROS service hoặc action.
- Không tự collision-check velocity/path output.
- Không trực tiếp nạp plugin bằng Boost.DLL trong core interface.
- Không thay thế trực tiếp `robot_nav_core::RecoveryBehavior` trong `move_base` nếu chưa có
-  adapter riêng.
-
-## 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.
+# Trạng thái `recovery_core`
+
+Nội dung PLAN.md cũ đã được gỡ. Hai lý do:
+
+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,
+   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`,
+   `MockBehavior` và `interface_contract_test.cpp` — grep cả ba symbol ra 0 hit, và `test/` khi đó
+   chỉ có đúng một file. Một checklist nói dối còn tệ hơn không có checklist.
+
+## Nguồn chuẩn hiện tại
+
+| Cần gì | Đọc ở đâu |
+|---|---|
+| Contract, họ output, bất biến an toàn | [`README.md`](README.md) |
+| 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) |
+| 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 |
+
+## Đã xong
+
+- Contract: `bool configure(name, ctx, nh)` / `bool start(goal, now)` / `update(now)` / `cancel()`;
+  `outputKind()` khai họ output; toàn bộ state của base là `private`.
+- `RecoveryContext` chỉ chứa **cổng** (`PoseProvider`, `CollisionChecker`, `PlanProvider`), không
+  chứa con trỏ tới dữ liệu động.
+- `RecoveryGoal` dùng `std::optional` + `trigger`; bỏ quy ước sentinel "0 = dùng default".
+- `RecoveryRegistry` nạp theo YAML qua Boost.DLL, giữ factory sống đúng vòng đời.
+- `adapters/`: `CostmapPoseProvider`, `CostmapCollisionChecker`.
+- Bộ mặc định 4 plugin / 5 instance: `wait`, `conservative_reset`, `rotate`, `aggressive_reset`,
+  `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.
+
+## Chưa xong
+
+- **Họ `kPath` chưa có behavior nào.** Contract đã sẵn sàng (`PlanProvider` + `outputKind()` trả
+  `kPath`), nhưng một behavior sinh đường thoát thật cần thông tin từ global planner — nằm ngoài
+  phạm vi gói này.
+- **`ClearCostmapRecovery` chưa có test tự động.** Nó cần `Costmap2DROBOT` thật (TF + chuỗi layer),
+  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
+  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.