# Kế Hoạch Xây Dựng Package `recovery_core` (Interface Thuần, Không Chạy ROS) > `recovery_core` là **package ĐỊNH NGHĨA INTERFACE** cho các hành vi recovery — đóng đúng > vai trò như `robot_nav_core` cung cấp `RecoveryBehavior`, nhưng **không chạy ROS** và > biểu đạt hợp đồng qua `robot_geometry_msgs` / `robot_nav_msgs` (header-only). > > Bản thân package **không** chứa hành vi cụ thể (backup, spin, clear costmap, regen path). > Các hành vi đó là **plugin implement interface** — làm ở Phase 3 hoặc ở package khác. --- ## 1. Bối Cảnh & Quyết Định Thiết Kế ### 1.1. `recovery_core` là gì - Là **thư viện interface** (giống `robot_nav_core::RecoveryBehavior`), nhưng ROS-free: - **Không** include `robot/robot.h`, **không** `NodeHandle`, **không** `Costmap2DROBOT`, **không** `tf3::BufferCore`. - Chỉ phụ thuộc kiểu dữ liệu header-only: `robot_geometry_msgs` (Twist, Pose2D, PoseStamped) và `robot_nav_msgs` (Path). Các kiểu này không kéo runtime ROS. - Cung cấp **một base class thuần ảo** đủ tổng quát để bao 3 họ recovery (mục 1.2), cùng các kiểu phụ trợ (status, result, config, context). So với `robot_nav_core::RecoveryBehavior` (blocking `runBehavior()`, cần costmap + tf): `recovery_core` giữ *tinh thần interface* nhưng thay hạ tầng ROS bằng abstraction ROS-free và mở rộng để hành vi có thể **trả về output** (không chỉ chạy rồi thôi). ### 1.2. Ba họ recovery interface phải bao được | Họ | Ví dụ | Output đặc trưng | |----|-------|------------------| | **A. Trả v�� path** | tạo lại một đoạn đường thoát/né | `robot_nav_msgs::Path` | | **B. Không output** | clear costmap, reset state | chỉ trạng thái (SUCCEEDED/FAILED) | | **C. Trả về vận tốc** | rotation tại chỗ, backup (không ROS) | `robot_geometry_msgs::Twist` theo cycle | Interface phải diễn đạt cả 3 mà **không ép** một hành vi phải điền output nó không dùng. → Dùng **một `RecoveryResult` hợp nhất** mang cờ *loại output* + các trường optional (status luôn có; `twist`, `path` chỉ hợp lệ khi cờ tương ứng bật). ### 1.3. Ranh giới trách nhiệm `recovery_core` **CHỈ** cung cấp: - Base class `RecoveryBehavior` (thuần ảo) + vòng đời chuẩn. - Kiểu hợp đồng: `RecoveryStatus`, `RecoveryOutputType`, `RecoveryResult`, `RecoveryConfig`, `RecoveryContext` (abstraction ROS-free thay cho tf/costmap/publisher). - Không thuật toán hành vi cụ thể, không collision-check, không I/O, không vòng lặp thời gian. Caller / plugin chịu trách nhiệm: cấp pose (qua `RecoveryContext`), tiêu thụ output (Twist → cmd_vel, Path → planner), đảm bảo an toàn. ### 1.4. Vị trí & chuẩn - Đặt tại `.../Navigations/Libraries/recovery_core` (cùng cấp `robot_clear_costmap_recovery`). - C++17, namespace `recovery_core`, guard `RECOVERY_CORE__H_`. - Identifiers/comments tiếng Anh; tài liệu `.md` tiếng Việt. - Build catkin + standalone (theo pattern `robot_clear_costmap_recovery`). - Phần lớn là **header-only** (interface) — `.cpp` chỉ cho helper/validate không inline. --- ## 2. Thiết Kế Interface (bản chốt ở Phase 2, nháp ở đây) ### 2.1. Kiểu hợp đồng ```cpp namespace recovery_core { // Trạng thái tiến trình 1 lượt recovery. enum class RecoveryStatus { kIdle, kRunning, kSucceeded, kFailed }; // Loại output mà hành vi sinh ra ở cycle hiện tại. enum class RecoveryOutputType { kNone, kVelocity, kPath }; // Kết quả hợp nhất cho cả 3 họ. Chỉ đọc trường khớp với output_type. struct RecoveryResult { RecoveryStatus status = RecoveryStatus::kRunning; RecoveryOutputType output_type = RecoveryOutputType::kNone; robot_geometry_msgs::Twist command; // hợp lệ khi output_type == kVelocity robot_nav_msgs::Path path; // hợp lệ khi output_type == kPath // Tiện ích khởi tạo nhanh (định nghĩa trong .cpp hoặc inline): static RecoveryResult Running(); static RecoveryResult Succeeded(); static RecoveryResult Failed(); static RecoveryResult Velocity(const robot_geometry_msgs::Twist&, RecoveryStatus); static RecoveryResult PathOut(const robot_nav_msgs::Path&, RecoveryStatus); }; ``` ### 2.2. Context ROS-free (thay tf/costmap/publisher) `RecoveryContext` là abstraction để plugin lấy trạng thái thế giới mà **không** biết ROS. `recovery_core` chỉ khai báo interface; caller cấp implementation cụ thể (đọc từ đâu là việc của caller). ```cpp // Interface tối thiểu; mở rộng dần khi có nhu cầu thực. class RecoveryContext { public: virtual ~RecoveryContext() = default; // Pose robot hiện tại trong frame quy ước (rad, m). Trả false nếu không có. virtual bool getRobotPose(robot_geometry_msgs::Pose2D* out) const = 0; // (tuỳ chọn, cho họ cần) truy vấn cost tại điểm; mặc định không hỗ trợ. // virtual bool getCost(double x, double y, unsigned char* cost) const { return false; } }; ``` > Ghi chú: interface base **không ép** hành vi phải dùng context (họ C rotation thuần có thể > chỉ cần pose truyền vào computeCommand). Context là kênh mở rộng cho họ A/B cần hỏi thế giới. ### 2.3. Base class interface ```cpp class RecoveryBehavior { public: // shared_ptr để KHỚP cơ chế nạp Boost.DLL của workspace: // boost::dll::import_alias(...) // (xem move_base.cpp loadRecoveryBehaviors + robot_nav_core::RecoveryBehavior::Ptr). using Ptr = std::shared_ptr; virtual ~RecoveryBehavior() = default; // 1) Nạp + validate config. Không throw; trả false + set error nếu sai. virtual bool configure(const RecoveryConfig& config, std::string* error) = 0; // 2) Bắt đầu 1 lượt: chốt state khởi đầu, reset progress về kRunning. virtual void start(const robot_geometry_msgs::Pose2D& current_pose) = 0; // 3a) Kiểu PER-CYCLE (họ C, và họ A nếu sinh path dần): gọi mỗi control cycle. // dt (s) > 0. Guard chưa configure/start -> Failed + output kNone. virtual RecoveryResult computeCommand(const robot_geometry_msgs::Pose2D& current_pose, double dt) = 0; // 3b) Kiểu ONE-SHOT/BLOCKING (họ B clear costmap, họ A regen path 1 lần): // chạy trọn hành vi qua context, trả kết quả cuối. Mặc định: lặp computeCommand. virtual RecoveryResult runBehavior(RecoveryContext* ctx); // có default impl virtual void reset() = 0; virtual RecoveryStatus status() const = 0; virtual const char* name() const = 0; protected: RecoveryBehavior() = default; }; ``` **Vì sao có cả `computeCommand` lẫn `runBehavior`:** - Họ **C** (rotation/backup) tự nhiên là *per-cycle* → override `computeCommand`. - Họ **B** (clear costmap) tự nhiên là *one-shot* → override `runBehavior` (dùng `ctx`), `computeCommand` chỉ trả trạng thái. - Họ **A** (regen path) có thể one-shot (`runBehavior` trả `Path`) hoặc per-cycle tuỳ plugin. - `runBehavior` có **default implementation** trong `recovery_core` (lặp `computeCommand` tới khi khác `kRunning`) để plugin per-cycle không phải viết lại; plugin one-shot thì override. ### 2.4. Config ```cpp struct RecoveryConfig { std::string type; // "back_up" | "spin" | "clear_costmap" | "regen_path" ... double control_frequency = 20.0; // Hz, dùng cho default runBehavior loop (dt = 1/f) double timeout = 0.0; // s, 0 = không timeout std::map params; // tham số riêng của từng plugin // validate(): control_frequency > 0, timeout >= 0, hữu hạn. Trả false + message. bool validate(std::string* error) const; }; ``` > Package interface **không** biết tham số riêng của backup/spin — để plugin tự đọc từ > `params`. `recovery_core` chỉ validate phần chung. --- ## 3. PHASE 1 — Dựng Khung Package (KHÔNG code triển khai) **Mục tiêu:** tạo cây thư mục + file stub (header có guard/namespace/Doxygen, `.cpp` thân rỗng compile được), `package.xml`, `CMakeLists.txt` build ra lib rỗng. Package phải configure CMake thành công. ### 3.1. Cây thư mục mục tiêu ``` recovery_core/ ├── PLAN.md # tài liệu này ├── README.md # phạm vi: INTERFACE, không ROS, 3 họ recovery ├── package.xml # depend: robot_geometry_msgs, robot_nav_msgs ├── CMakeLists.txt # catkin + standalone; lib chủ yếu header, .cpp cho helper ├── include/ │ └── recovery_core/ │ ├── recovery_types.h # enum Status, OutputType; struct RecoveryResult │ ├── recovery_config.h # struct RecoveryConfig + validate() │ ├── recovery_context.h # interface RecoveryContext (ROS-free) │ └── recovery_behavior.h # base class RecoveryBehavior (interface chính) ├── src/ │ ├── recovery_types.cpp # (stub) factory helper Running()/Failed()/... │ ├── recovery_config.cpp # (stub) validate() │ └── recovery_behavior.cpp # (stub) default runBehavior() loop ├── test/ │ ├── CMakeLists.txt # khai báo test target (tuỳ chọn) │ ├── interface_contract_test.cpp # (stub) dùng 1 MockBehavior kiểm vòng đời │ └── mock_behavior.h # (stub) implement tối thiểu để test interface ├── examples/ │ └── minimal_plugin.cpp # (stub) ví dụ 1 plugin per-cycle bé xíu, không ROS └── docs/ ├── ARCHITECTURE.md # sơ đồ interface + luồng runBehavior/computeCommand ├── PLUGIN_GUIDE.md # hướng dẫn viết plugin cho từng họ A/B/C └── SAFETY.md # cảnh báo: không collision-check trong core ``` ### 3.2. Quy ước nội dung stub (Phase 1) - **Header**: license/author ngắn, guard `RECOVERY_CORE_*_H_`, include tối thiểu (``, ``, `` khi cần), khai báo đầy đủ chữ ký + Doxygen public API, thân hàm non-inline để trong `.cpp`. - **Source**: thân tối thiểu `// TODO(phase-2): implement` + `return {}` để compile. - **`package.xml`**: format 2, `robot_geometry_msgs`, `robot_nav_msgs`, buildtool catkin. **Không** depend costmap/tf/robot_cpp. - **`CMakeLists.txt`**: khung catkin+standalone của `robot_clear_costmap_recovery` nhưng chỉ giữ 2 msgs depend; tạo library `recovery_core` từ các `.cpp` (kể cả khi phần lớn header-only, vẫn build 1 lib nhỏ cho validate/helper). Install header + export target 2 chế độ. ### 3.3. Checklist Phase 1 - [ ] Tạo đủ cây thư mục 3.1. - [ ] 4 header có guard, namespace `recovery_core`, Doxygen; khai báo khớp mục 2. - [ ] 3 `.cpp` compile với thân stub. - [ ] `package.xml` chỉ depend `robot_geometry_msgs` + `robot_nav_msgs`. - [ ] `CMakeLists.txt` configure OK cả catkin lẫn standalone; build ra `librecovery_core` rỗng. - [ ] `README.md` nêu rõ: đây là INTERFACE, không ROS, 3 họ recovery, không hành vi cụ thể. - [ ] **Kiểm chứng:** `cmake` + `make` thành công. --- ## 4. PHASE 2 — Triển Khai Interface (điền logic phần thuộc về core) > Core là interface nên "triển khai" ở đây = hoàn thiện **phần chung** mà base cung cấp, KHÔNG > phải viết hành vi cụ thể. Cụ thể: ### 4.1. Nội dung triển khai 1. **`recovery_types`**: định nghĩa các factory `RecoveryResult::Running/Succeeded/Failed/` `Velocity/PathOut`, đảm bảo set đúng `output_type` + `status`. Bất biến: khi `output_type==kNone` thì `command`/`path` để mặc định (không rác). 2. **`recovery_config::validate`**: kiểm `control_frequency > 0`, `timeout >= 0`, hữu hạn; `type` không rỗng. Trả `false` + message rõ. Không throw. 3. **`recovery_behavior.cpp` — default `runBehavior(ctx)`**: - Guard `ctx == nullptr` → `Failed`. - Lấy pose qua `ctx->getRobotPose`; nếu fail → `Failed`. - `dt = 1 / control_frequency`; lặp `computeCommand(pose, dt)` tới khi status khác `kRunning` **hoặc** vượt `timeout` (nếu > 0) → khi timeout trả `Failed`. - Mỗi vòng cập nhật pose qua `ctx` (mô phỏng caller thật). Trả result cuối. - **Không** cấp phát trong vòng lặp; không log spam (chỉ log khi đổi status). 4. **Bất biến & guard chung** (document + test): - Chưa `configure` thành công → mọi call trả `Failed`. - `dt <= 0` hoặc pose NaN/Inf → `Failed`, output `kNone`. ### 4.2. Kiểm thử interface (không cần plugin thật) - `test/mock_behavior.h`: `MockBehavior` per-cycle đơn giản (đếm N cycle rồi `kSucceeded`, trả `kVelocity`) + `MockContext` cấp pose cố định. - `interface_contract_test.cpp`: - `configure` sai → `false` + message. - `start` → `status()==kRunning`. - `computeCommand` tiến trình đúng, đạt đích → `kSucceeded`. - default `runBehavior` chạy hết vòng đời qua `MockContext`, tôn trọng `timeout`. - guard: chưa configure, dt<=0, ctx null, pose NaN. ### 4.3. Checklist Phase 2 - [ ] `RecoveryResult` factories set đúng cờ; có test bất biến output. - [ ] `RecoveryConfig::validate` từ chối mọi input sai với message rõ. - [ ] default `runBehavior` đúng: loop, timeout, guard ctx/pose; không alloc trong loop. - [ ] `MockBehavior`/`MockContext` + contract test pass. - [ ] `docs/PLUGIN_GUIDE.md` mô tả cách 1 plugin họ A/B/C override method nào. - [ ] **Kiểm chứng:** build + test pass. --- ## 5. PHASE 3 — Tạo Recovery Plugin (implement interface) **Mục tiêu:** viết các hành vi cụ thể **implement** `recovery_core::RecoveryBehavior`, mỗi họ ít nhất một ví dụ, + cơ chế nạp theo string. ### 5.1. Plugin mẫu cho từng họ - **Họ C — `SpinRecovery` / `BackUpRecovery`** (per-cycle, ROS-free): - Override `computeCommand`: sinh `Twist` theo pose + dt (xoay tới góc / lùi tới khoảng cách), đạt đích → `kSucceeded`. Đọc tham số từ `RecoveryConfig::params` (`spin_target_angle`, `spin_speed`, `backup_distance`, `backup_speed`...). - Dùng default `runBehavior` (không cần override). - **Họ B — `ClearCostmapRecovery`-like** (one-shot, cần context): - Override `runBehavior(ctx)`: gọi hành động clear qua một context mở rộng (`RecoveryContext` thêm hook clear — hoặc để ở adapter). Trả `kSucceeded`/`kFailed`, output `kNone`. `computeCommand` chỉ trả trạng thái. - Lưu ý: clear costmap thật cần costmap → phần đó nằm ở **adapter/caller**, không trong `recovery_core`. Plugin ở đây minh hoạ contract, thao tác thật uỷ thác qua context. - **Họ A — `RegenPathRecovery`** (trả path): - Override để trả `RecoveryResult` với `output_type == kPath`, điền `robot_nav_msgs::Path` (một đoạn đường thoát hình học đơn giản, ví dụ cung lùi). one-shot hoặc per-cycle. > Các plugin này có thể đặt trong `recovery_core/plugins/` hoặc package riêng — **cần chốt** > (mục 6). Dù đặt đâu, chúng chỉ được include header của `recovery_core`, không thêm dependency > ROS vào package interface. ### 5.2. Cơ chế nạp — **Boost.DLL** (chốt, theo đúng convention workspace) Workspace nạp planner/recovery plugin bằng **Boost.DLL import_alias**, KHÔNG dùng pluginlib/ class_loader. Bằng chứng trong repo: - Plugin export: `robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp:37,330-337`. - Loader import: `move_base/src/move_base.cpp:22, 2016-2026` (`loadRecoveryBehaviors`). `recovery_core` (interface) **không** include Boost.DLL. Chỉ **plugin** export symbol và **loader/adapter** import. Interface chỉ cần `Ptr = std::shared_ptr` để khớp chữ ký `import_alias`. #### 5.2.1. Phía PLUGIN — export factory bằng `BOOST_DLL_ALIAS` Mỗi plugin cung cấp một **factory không tham số** trả `Ptr` (shared_ptr), rồi alias ra tên symbol dùng làm `type` trong YAML: ```cpp // spin_recovery.cpp #include #include namespace recovery_plugins { class SpinRecovery : public recovery_core::RecoveryBehavior { /* override ... */ public: static recovery_core::RecoveryBehavior::Ptr create() { // factory không tham số return std::make_shared(); } }; } // namespace recovery_plugins // Tên alias thứ 2 ("spin_recovery") chính là `type` trong YAML recovery_behaviors. BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery) ``` Quy ước (giống `ClearCostmapRecovery` → alias `ClearCostmapRecovery`): - Factory là `static Ptr create()` — **không tham số** (Boost.DLL alias yêu cầu signature `Ptr()`), cấu hình đi qua `configure()` sau khi tạo, không qua constructor. - Trả `std::shared_ptr` (KHÔNG `unique_ptr`) để khớp loader. - Tên alias = tên `type` mà YAML/loader sẽ dùng để tìm library + symbol. #### 5.2.2. Phía LOADER — `import_alias` (mẫu, thường nằm ở adapter/caller) ```cpp #include // path .so lấy qua PluginLoaderHelper::findLibraryPath(type) như move_base đang làm. auto loader = boost::dll::import_alias( path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations); recovery_core::RecoveryBehavior::Ptr behavior = loader(); // tạo instance std::string err; behavior->configure(config, &err); // rồi mới cấu hình ``` > `recovery_core` **không** viết loader này (nó thuộc caller/move_base/adapter). Nhưng > `docs/PLUGIN_GUIDE.md` sẽ ghi mẫu để người dùng plugin biết cách nạp. #### 5.2.3. CMake cho PLUGIN (không phải cho core) - `find_package(Boost REQUIRED COMPONENTS system filesystem)` (Boost.DLL cần `filesystem`; `system` theo pattern package cũ). Link `${Boost_LIBRARIES}` và `${CMAKE_DL_LIBS}` (dl). - `set_target_properties( PROPERTIES POSITION_INDEPENDENT_CODE ON)` — bắt buộc cho .so. - Build mỗi plugin thành **shared library** riêng; tên library + symbol khớp `type` YAML. - Install `.so` vào nơi `findLibraryPath` tìm được (theo convention `PluginLoaderHelper`). - **Không** cần `plugins.xml` (workspace này dùng Boost.DLL thuần, không pluginlib). ### 5.3. Adapter ROS (tuỳ chọn, package RIÊNG) Nếu cần chạy trong move_base thật: package adapter bọc plugin `recovery_core`, implement `robot_nav_core::RecoveryBehavior` (có costmap/tf), cấp `RecoveryContext` đọc TF/costmap, tiêu thụ Twist/Path. Giữ `recovery_core` sạch, không đổi. ### 5.4. Checklist Phase 3 - [ ] Chốt: plugin đặt trong `recovery_core/plugins/` hay package riêng (đề xuất: package riêng cho plugin cần costmap/tf; plugin thuần spin/backup có thể ở `recovery_core/plugins/`). - [ ] Mỗi họ A/B/C có ≥ 1 plugin mẫu implement interface, build ra **shared library** riêng. - [ ] Mỗi plugin có `static Ptr create()` + `BOOST_DLL_ALIAS(...::create, )`; tên alias = `type` YAML; trả `std::shared_ptr`. - [ ] CMake plugin: `find_package(Boost COMPONENTS system filesystem)`, link `${CMAKE_DL_LIBS}`, `POSITION_INDEPENDENT_CODE ON`, install `.so` đúng nơi loader tìm. - [ ] Test end-to-end (không ROS): `import_alias` nạp `.so` mẫu → `configure` → `runBehavior`/`computeCommand` với MockContext → kiểm output đúng họ (Twist / none / Path). - [ ] Cập nhật `docs/ARCHITECTURE.md`, `docs/PLUGIN_GUIDE.md` (kèm mẫu export + import). - [ ] **Kiểm chứng:** build plugin `.so` + test nạp Boost.DLL pass. --- ## 6. Rủi Ro & Điểm Cần Xác Nhận - **Kiểu output hợp nhất vs đa interface:** đề xuất một `RecoveryResult` mang cờ `output_type` (đơn giản, 1 base class). Phương án khác: template/đa base theo họ — phức tạp hơn. **Cần chốt.** - **`RecoveryContext` rộng tới đâu:** tối thiểu chỉ `getRobotPose`. Họ B (clear costmap) và họ A (regen path cần costmap) sẽ cần hook thêm — nên để ở **context mở rộng của adapter**, giữ context lõi nhỏ. **Cần chốt mức tối thiểu.** - **Vị trí plugin:** trong `recovery_core/plugins/` hay package riêng. Đề xuất: package riêng cho plugin cần costmap/tf; plugin thuần (spin/backup) có thể ở `recovery_core/plugins/`. - **Cơ chế nạp = Boost.DLL (đã chốt):** theo đúng convention workspace (`import_alias`, không pluginlib). Ràng buộc kéo theo: `Ptr = std::shared_ptr`; factory `static Ptr create()` không tham số + `BOOST_DLL_ALIAS`; cấu hình qua `configure()` sau khi tạo. Core interface KHÔNG phụ thuộc Boost.DLL — chỉ plugin và loader. - **`robot_nav_msgs` build dep:** xác nhận có CMake config/target để `find_package` (catkin) hoặc include-only (standalone). Là header msgs nên nhiều khả năng include-only. - **An toàn:** core không collision-check → ghi rõ `docs/SAFETY.md`; caller/adapter chịu trách nhiệm an toàn khi thực thi backup/spin. --- ## 7. Tóm Tắt 3 Phase | Phase | Kết quả | Kiểm chứng | |-------|---------|-----------| | 1 | Khung package interface: 4 header + stub, package.xml, CMake build lib rỗng | `cmake` + `make` OK | | 2 | Hoàn thiện phần chung của interface: types/config/validate + default `runBehavior`, contract test qua Mock | build + test pass | | 3 | Plugin mẫu cho 3 họ (Twist/none/Path), export `BOOST_DLL_ALIAS`, nạp qua `import_alias`, doc | build `.so` + test nạp Boost.DLL pass |