diff --git a/CMakeLists.txt b/CMakeLists.txt index 41b3c38..4564019 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,7 +195,6 @@ endif() # ======================================================== add_library(recovery_core SHARED src/recovery_types.cpp - src/recovery_config.cpp src/recovery_behavior.cpp ) diff --git a/PLAN.md b/PLAN.md index d16fdbb..70f7eeb 100644 --- a/PLAN.md +++ b/PLAN.md @@ -25,11 +25,10 @@ plugin recovery có thể trả trạng thái, velocity command hoặc path. - `RecoveryStatus` - `RecoveryOutputType` - `RecoveryResult` -- Cung cấp helper config chung: - - `RecoveryConfig::control_frequency` - - `RecoveryConfig::timeout` - - `validate()` - - `fromNodeHandle()` +- Cung cấp ngữ cảnh + mục tiêu runtime: + - `RecoveryContext` (tf/costmap/global_path) + - `RecoveryGoal` (angle/distance/target_pose/params) +- Param chung `timeout` do base đọc trong `configure()` (param riêng plugin đọc `onConfigure()`). - 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 @@ -157,18 +156,18 @@ recovery_core/ ├── PLAN.md ├── include/recovery_core/ │ ├── recovery_behavior.h -│ ├── recovery_config.h │ └── recovery_types.h ├── src/ │ ├── recovery_behavior.cpp -│ ├── recovery_config.cpp │ └── recovery_types.cpp +├── plugins/ +│ ├── clear_costmap_recovery.cpp +│ ├── rotate_recovery.cpp +│ ├── back_up_recovery.cpp +│ └── regen_path_recovery.cpp ├── test/ │ ├── CMakeLists.txt -│ ├── interface_contract_test.cpp -│ └── mock_behavior.h -├── examples/ -│ └── minimal_recovery.cpp +│ └── plugin_loader_contract_test.cpp └── docs/ ├── ARCHITECTURE.md ├── PLUGIN_GUIDE.md diff --git a/README.md b/README.md index 3feaeaf..5da58c4 100644 --- a/README.md +++ b/README.md @@ -13,21 +13,35 @@ Interface (base class) cho các hành vi **recovery** của navigation stack ROS - 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. +## Vòng đời hướng-goal + +```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(dt); // publish r.command; đọc r.progress / r.remaining / r.message +``` + +API công khai `configure/start/update/cancel` là non-virtual (base lo guard/timeout/elapsed/cancel); +plugin chỉ override hook `onConfigure()/onStart(goal)/onUpdate(dt)`. + ## Ba họ recovery -| Họ | Ví dụ | Output | Method chính | -|----|-------|--------|--------------| -| A | regen path (đường thoát) | `robot_nav_msgs::Path` | `runBehavior()` | -| B | clear costmap | không có (chỉ status) | `runBehavior()` | -| C | rotation / backup | `robot_geometry_msgs::Twist` mỗi cycle | `computeCommand(dt)` | +| 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(dt)` | -Cả 3 chia sẻ một `RecoveryResult` hợp nhất mang cờ `output_type`. +Cả 3 chia sẻ một `RecoveryResult` hợp nhất mang cờ `output_type` + rich feedback +(`progress`/`remaining`/`elapsed`/`message`). ## Cấu trúc ``` -include/recovery_core/ recovery_types.h, recovery_config.h, recovery_behavior.h -src/ phần chung của contract (types/config/default behavior) +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 diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index f289f0e..5065aed 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -8,34 +8,41 @@ về output** (path/vận tốc), đồng thời vẫn dùng global path/costmap ## Các thành phần -- `RecoveryBehavior` (interface): `initialize` (mirror nav_core) + `runBehavior` (one-shot) + - `computeCommand` (per-cycle) + `status`. -- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ. -- `RecoveryConfig`: param chung (control_frequency, timeout) + validate + đọc từ NodeHandle. +- `RecoveryBehavior` (interface, template-method): API non-virtual `configure`/`start`/`update`/ + `cancel`; plugin chỉ triển khai hook `onConfigure`/`onStart(goal)`/`onUpdate(dt)`. +- `RecoveryContext`: gói con trỏ ngữ cảnh (tf/costmap/global_path) truyền một lần qua `configure`. +- `RecoveryGoal`: mục tiêu RUNTIME mỗi lượt (angle/distance/target_pose/params). +- `RecoveryResult` / `RecoveryStatus` / `RecoveryOutputType`: hợp đồng output hợp nhất 3 họ + + rich feedback (`progress`/`remaining`/`elapsed`/`message`), thêm trạng thái `kCancelled`. - Plugin mẫu: - - `ClearCostmapRecovery`: clear layer costmap theo tên, trả no-output status. - - `RotateRecovery`: sinh `Twist.angular.z` theo chu kỳ tới khi đủ góc. - - `BackUpRecovery`: sinh `Twist.linear.x < 0` theo chu kỳ tới khi đủ khoảng lùi. + - `ClearCostmapRecovery`: clear layer costmap theo tên, one-shot no-output. + - `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. - `RegenPathRecovery`: trả lại `robot_nav_msgs::Path` từ `global_path` hiện tại. ## Luồng runtime ``` -initialize(name, tf, global_path, global, local) // 1 lần, đọc param qua NodeHandle +configure(name, ctx) // 1 lần: cache ctx, đọc config chung, onConfigure() │ - ├── one-shot (họ A/B): runBehavior() ──────────────► RecoveryResult{status, path|none} +start(goal) // mỗi lượt: chốt mục tiêu runtime, onStart() │ - └── per-cycle (họ C): loop { computeCommand(dt) } ─► RecoveryResult{status, velocity} - (caller publish command mỗi cycle tới khi status != kRunning) + ├── one-shot (họ A/B): update(dt) 1 lần ─────────► RecoveryResult{status, path|none, msg} + │ + └── per-cycle (họ C): loop { update(dt) } ──────► RecoveryResult{status, velocity, + (tới khi status != kRunning) progress, remaining} ``` ## Ghi chú thiết kế -- `computeCommand(dt)` lấy pose robot từ costmap/tf bên trong (nhất quán mirror nav_core), - không truyền pose qua tham số. -- Default `RecoveryBehavior::computeCommand(dt)` trả `RecoveryResult::Failed()` để họ A/B không - vô tình sinh command mù. -- `RecoveryConfig::validate()` từ chối `NaN/Inf`, `control_frequency <= 0`, `timeout < 0`. -- `RecoveryConfig::fromNodeHandle()` đọc param chung và thay giá trị invalid bằng default an toàn. +- Template-method: base xử lý guard vòng đời (configure→start→update), `dt<=0`, `timeout`, đo + `elapsed`, và `cancel` một chỗ; plugin không lặp lại các guard này. +- Mục tiêu là RUNTIME qua `RecoveryGoal` (không cố định trong config): cùng plugin phục vụ nhiều + yêu cầu góc/khoảng khác nhau. Field = 0 → dùng default plugin đọc ở `onConfigure()`. +- `onUpdate(dt)` 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`. +- Param chung duy nhất còn lại là `timeout` (s, 0 = không giới hạn), base đọc trong `configure()` + từ `~//timeout`; giá trị `NaN/Inf/<0` bị thay bằng 0. Param RIÊNG của plugin đọc trong + `onConfigure()`. - Interface KHÔNG include Boost.DLL; export/import là việc của plugin/loader. - Plugin mẫu có dùng Boost.DLL alias, nhưng core contract vẫn không biết loader/adaptor. diff --git a/docs/PLUGIN_GUIDE.md b/docs/PLUGIN_GUIDE.md index 02dae0a..161d1c5 100644 --- a/docs/PLUGIN_GUIDE.md +++ b/docs/PLUGIN_GUIDE.md @@ -9,18 +9,38 @@ Package hiện có 4 plugin mẫu dưới `plugins/`: ## Bước chung 1. Kế thừa `recovery_core::RecoveryBehavior`. -2. Override `initialize()` — đọc param riêng qua `robot::NodeHandle("~/" + name)`, cache - tf/global_path/costmap. -3. Override method theo họ (xem dưới) + `status()`. -4. Thêm factory `static Ptr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`. +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()`. +3. Override `onStart(goal)` + `onUpdate(dt)` theo họ (xem dưới). KHÔNG override + `configure/start/update/cancel` — base đã lo guard vòng đời/timeout/cancel/elapsed. +4. Thêm factory `static RecoveryBehaviorPtr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`. + +## Vòng đời (goal-driven) + +``` +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(dt) // mỗi cycle tới khi status != kRunning; base tick elapsed + guard + │ +[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 | `runBehavior()` | `RecoveryResult::PathOut(path, kSucceeded)` | -| B. none | `runBehavior()` | `RecoveryResult::Succeeded()` / `Failed()` | -| C. velocity | `computeCommand(dt)` | `RecoveryResult::Velocity(twist, kRunning|kSucceeded)` | +| A. path | `onUpdate()` (one-shot) | `RecoveryResult::PathOut(path, kSucceeded)` | +| B. none | `onUpdate()` (one-shot) | `RecoveryResult::Succeeded()` / `Failed()` | +| C. velocity | `onStart()` chốt goal + `onUpdate(dt)` 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) @@ -31,10 +51,11 @@ Package hiện có 4 plugin mẫu dưới `plugins/`: namespace recovery_plugins { class SpinRecovery : public recovery_core::RecoveryBehavior { public: - static recovery_core::RecoveryBehavior::Ptr create() { + static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { return std::make_shared(); } - // override initialize()/computeCommand()/runBehavior()/status()... + protected: + // override onConfigure()/onStart(goal)/onUpdate(dt)... }; } // namespace recovery_plugins @@ -46,10 +67,21 @@ BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery) ```cpp #include -auto loader = boost::dll::import_alias( +auto loader = boost::dll::import_alias( path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations); -recovery_core::RecoveryBehavior::Ptr behavior = loader(); -behavior->initialize(name, tf, global_path, global_costmap, local_costmap); +recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = loader(); + +recovery_core::RecoveryContext ctx; +ctx.tf = tf; ctx.global_path = global_path; +ctx.global_costmap = global_costmap; ctx.local_costmap = local_costmap; +behavior->configure(name, ctx); + +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(dt); // 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 diff --git a/docs/SAFETY.md b/docs/SAFETY.md index c0de7b3..0fa34ff 100644 --- a/docs/SAFETY.md +++ b/docs/SAFETY.md @@ -11,13 +11,14 @@ đú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). -- **Default per-cycle fail an toàn.** Behavior không override `computeCommand(dt)` sẽ nhận - `RecoveryResult::Failed()` thay vì velocity mặc định. +- **Guard vòng đời ở base.** `start()` trước `configure()`, hay `update()` trước `start()`, đều trả + `RecoveryResult::Failed()`. `dt <= 0`, `NaN`, `Inf`, và `timeout` do base chặn trước khi gọi + `onUpdate()`. Sau `cancel()`, base trả stop output (Twist 0) + `kCancelled`. ## Nguyên tắc cho plugin -- Guard `initialized_` và costmap/tf null trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`. -- Guard `dt <= 0`, `NaN`, `Inf` trước khi tính velocity. +- Guard costmap/tf null (lấy qua `ctx()`) trước khi thao tác; fail an toàn -> `RecoveryResult::Failed()`. +- Không cần tự guard `dt <= 0`/`NaN`/`Inf`/`timeout` — base đã lo; chỉ 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). diff --git a/include/recovery_core/recovery_behavior.h b/include/recovery_core/recovery_behavior.h index f6458f8..d502c29 100644 --- a/include/recovery_core/recovery_behavior.h +++ b/include/recovery_core/recovery_behavior.h @@ -4,8 +4,10 @@ * * recovery_core — interface (base class) cho recovery behaviors. * - * 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 (path / none / velocity). + * Vòng đời hợp nhất, hướng-goal cho cả 3 họ recovery (path / none / velocity): + * configure(name, ctx) -> start(goal) -> lặp update(dt) -> [cancel()] + * Base lo phần chung (guard init/dt/timeout/cancel, đo elapsed, đẩy feedback); plugin chỉ + * override các hook nhỏ onConfigure()/onStart()/onUpdate(). * * Author: DuongTD *********************************************************************/ @@ -27,72 +29,113 @@ namespace recovery_core /** * @class RecoveryBehavior - * @brief Interface cho mọi hành vi recovery không chạy roscpp/ROS master thật - * (dùng lớp ROS-like robot_*). + * @brief Interface hướng-goal cho mọi hành vi recovery (không chạy roscpp/ROS master thật). * - * Ba họ hành vi và method chính tương ứng: - * - Họ A (trả path) : override runBehavior() -> RecoveryResult::PathOut(...) - * - Họ B (không output) : override runBehavior() -> RecoveryResult::Succeeded()/Failed() - * - Họ C (trả vận tốc) : override computeCommand() -> RecoveryResult::Velocity(...) + * Thiết kế theo template-method: API công khai (configure/start/update/cancel) là NON-VIRTUAL + * và do base xử lý phần lặp lại; plugin chỉ triển khai các hook protected: + * - onConfigure() : đọc param riêng từ ctx()/NodeHandle (tuỳ chọn). + * - onStart(goal) : chốt mục tiêu lượt này (rad/m/pose), reset trạng thái tiến độ. + * - onUpdate(dt) : một "tick"; họ one-shot (path/clear) hoàn tất ngay lần đầu. * - * Vòng đời: initialize() một lần -> runBehavior() (one-shot) hoặc lặp computeCommand() - * (per-cycle) -> status(). Guard initialized_/costmap trước khi thao tác. + * Ba họ hành vi: + * - Họ A (trả path) : onUpdate() trả RecoveryResult::PathOut(...), kSucceeded ngay. + * - Họ B (không output) : onUpdate() làm việc rồi trả Succeeded()/Failed() ngay. + * - Họ C (trả vận tốc) : onUpdate() sinh Twist mỗi cycle tới khi đạt goal -> kSucceeded. + * + * Bất biến: start() chỉ hợp lệ sau configure(); update() chỉ chạy sau start(). Vi phạm -> + * RecoveryResult::Failed(). Base tự trả stop output + kCancelled sau khi cancel(). */ class RecoveryBehavior { public: /// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace - /// (boost::dll::import_alias(...)). + /// (boost::dll::import_alias(...)). using RecoveryBehaviorPtr = std::shared_ptr; virtual ~RecoveryBehavior() = default; - /** - * @brief Khởi tạo — mở rộng chữ ký robot_nav_core::RecoveryBehavior::initialize (thêm - * global_path). Chỉ chạy một lần; đọc param qua robot::NodeHandle("~/" + name); - * cache tf/costmap/global_path (không sở hữu). - * @param name Tên instance (dùng cho namespace param + log). - * @param tf Transform buffer (không sở hữu). - * @param global_path Đường đi toàn cục hiện tại (không sở hữu) — họ A regen path tham - * chiếu để tạo lại/né; có thể null nếu chưa có plan. - * @param global_costmap Costmap toàn cục (không sở hữu). - * @param local_costmap Costmap cục bộ (không sở hữu). - */ - virtual void initialize(std::string name, tf3::BufferCore* tf, - std::vector* global_path, - robot_costmap_2d::Costmap2DROBOT* global_costmap, - robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0; + // ------------------------------------------------------------------ + // API công khai — NON-VIRTUAL, base xử lý phần chung. + // ------------------------------------------------------------------ /** - * @brief Chạy hành vi kiểu ONE-SHOT (họ A regen path, họ B clear costmap). - * Trả kết quả cuối kèm output (path hoặc none). Guard chưa initialize/costmap null - * -> RecoveryResult::Failed(). + * @brief Cấu hình một lần: cache ngữ cảnh (không sở hữu), đọc config chung, gọi onConfigure(). + * @param name Tên instance (namespace param + log). + * @param ctx Ngữ cảnh môi trường (tf/costmap/global_path). Gọi lại lần 2 bị bỏ qua. */ - virtual RecoveryResult runBehavior() = 0; + void configure(const std::string& name, const RecoveryContext& ctx); /** - * @brief Sinh command kiểu PER-CYCLE (họ C rotation/backup). Lấy pose robot từ costmap/tf - * bên trong; caller lặp gọi mỗi control cycle và publish command. + * @brief Bắt đầu một lượt recovery với mục tiêu RUNTIME. + * @param goal Mục tiêu lượt này (góc/khoảng lùi/pose + override). Field 0 = dùng default plugin. + * @return Kết quả tick đầu (thường kRunning; kFailed nếu chưa configure hoặc goal không hợp lệ). + */ + RecoveryResult start(const RecoveryGoal& goal); + + /** + * @brief Một control cycle. Guard chưa start/dt<=0/timeout/cancel trước khi gọi onUpdate(). * @param dt Khoảng thời gian control cycle (s), > 0. - * @return RecoveryResult (thường output_type == kVelocity). - * - * Mặc định: coi như không hỗ trợ per-cycle và trả Failed() — họ A/B không cần override. */ - virtual RecoveryResult computeCommand(double dt); + RecoveryResult update(double dt); /** - * @brief Trạng thái hiện tại của lượt recovery. + * @brief Yêu cầu huỷ: update() kế tiếp trả stop output + kCancelled. */ - virtual RecoveryStatus status() const = 0; + void cancel(); - virtual std::string getNameRecoveryBehavior() const + /// @brief Trạng thái hiện tại của lượt recovery. + RecoveryStatus status() const + { + return status_; + } + + /// @brief Thời gian đã trôi từ start() (s). + double elapsed() const + { + return elapsed_; + } + + const std::string& name() const + { + return name_; + } + + /// Giữ tên cũ cho tương thích call-site loader. + std::string getNameRecoveryBehavior() const { return name_; } protected: RecoveryBehavior() = default; + + // ------------------------------------------------------------------ + // 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 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 Một tick. Họ one-shot trả kSucceeded/kFailed ngay; họ velocity trả kRunning tới goal. + virtual RecoveryResult onUpdate(double dt) = 0; + + // Truy cập cho plugin (chỉ đọc ngữ cảnh/goal). + const RecoveryContext& ctx() const { return ctx_; } + const RecoveryGoal& goal() const { return goal_; } + bool cancelRequested() const { return cancel_requested_; } + + RecoveryContext ctx_; + RecoveryGoal goal_; std::string name_; + double timeout_ = 0.0; ///< s — 0 nghĩa là không timeout; đọc param "~//timeout". + double elapsed_ = 0.0; + bool configured_ = false; + bool started_ = false; + bool cancel_requested_ = false; + RecoveryStatus status_ = RecoveryStatus::kIdle; }; } // namespace recovery_core diff --git a/include/recovery_core/recovery_config.h b/include/recovery_core/recovery_config.h deleted file mode 100644 index 720535d..0000000 --- a/include/recovery_core/recovery_config.h +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * recovery_core — config chung cho recovery behaviors. - * - * Author: DuongTD - *********************************************************************/ -#ifndef RECOVERY_CORE_RECOVERY_CONFIG_H_ -#define RECOVERY_CORE_RECOVERY_CONFIG_H_ - -#include - -// Forward declare để không kéo vào header interface. -namespace robot { class NodeHandle; } - -namespace recovery_core -{ - -/** - * @struct RecoveryConfig - * @brief Gói các tham số CHUNG cho vòng đời recovery. - * - * Tham số RIÊNG của từng hành vi (vd: spin_target_angle, backup_distance) do plugin tự đọc - * qua robot::NodeHandle trong initialize(); struct này chỉ giữ phần chung. - * - * Đơn vị: - * - control_frequency : Hz (tần số gọi computeCommand khi chạy per-cycle) - * - timeout : s (0 = không giới hạn thời gian) - */ -struct RecoveryConfig -{ - double control_frequency = 20.0; ///< Hz, > 0. - double timeout = 0.0; ///< s, >= 0; 0 nghĩa là không timeout. - - /** - * @brief Kiểm tra hợp lệ các tham số. - * @param error [out] Nếu != nullptr và invalid, ghi thông điệp lỗi. - * @return true nếu hợp lệ. - */ - bool validate(std::string* error) const; - - /** - * @brief Đọc config từ NodeHandle (có default, có validate + log cảnh báo nếu sai). - * @param nh NodeHandle đã trỏ tới namespace của behavior. - * @return RecoveryConfig đã điền (giá trị sai được thay bằng default). - */ - static RecoveryConfig fromNodeHandle(robot::NodeHandle& nh); -}; - -} // namespace recovery_core - -#endif // RECOVERY_CORE_RECOVERY_CONFIG_H_ diff --git a/include/recovery_core/recovery_types.h b/include/recovery_core/recovery_types.h index 1a0071f..b221e7d 100644 --- a/include/recovery_core/recovery_types.h +++ b/include/recovery_core/recovery_types.h @@ -9,9 +9,18 @@ #ifndef RECOVERY_CORE_RECOVERY_TYPES_H_ #define RECOVERY_CORE_RECOVERY_TYPES_H_ +#include +#include +#include + +#include #include #include +// Forward declare để không kéo header nặng vào contract type. +namespace tf3 { class BufferCore; } +namespace robot_costmap_2d { class Costmap2DROBOT; } + namespace recovery_core { @@ -21,10 +30,11 @@ namespace recovery_core */ enum class RecoveryStatus { - kIdle, ///< Chưa bắt đầu (sau initialize/reset). - kRunning, ///< Đang thực thi, cần tiếp tục gọi. - kSucceeded, ///< Hoàn thành thành công. - kFailed ///< Lỗi/không thể thực thi an toàn (trả stop output). + kIdle, ///< Chưa bắt đầu (sau configure/reset). + kRunning, ///< Đang thực thi, cần tiếp tục gọi update(). + kSucceeded, ///< Hoàn thành thành công (đạt goal). + kFailed, ///< Lỗi/không thể thực thi an toàn (trả stop output). + kCancelled ///< Bị caller huỷ giữa chừng (trả stop output). }; /** @@ -40,13 +50,59 @@ enum class RecoveryOutputType }; /** - * @struct RecoveryResult - * @brief Kết quả hợp nhất cho cả 3 họ recovery. + * @struct RecoveryContext + * @brief Ngữ cảnh môi trường cấp cho behavior một lần qua configure(). * - * Bất biến: chỉ đọc trường khớp với @ref output_type. + * 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* 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 + * @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào start(goal). + * + * Đâ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ể + * 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 + * default đã cấu hình của plugin". Đơn vị: angle [rad], distance [m]. + */ +struct RecoveryGoal +{ + double angle = 0.0; ///< rad — góc quay đích (họ rotate). 0 = dùng default plugin. + double distance = 0.0; ///< m — khoảng lùi đích (họ backup). 0 = dùng default plugin. + + robot_geometry_msgs::PoseStamped target_pose; ///< Pose đích (họ path/detour), tuỳ chọn. + bool has_target_pose = false; ///< true nếu target_pose hợp lệ. + + std::map params; ///< Override mở rộng theo từng plugin (vd tốc độ). + + /// @brief Đọc override double trong params, trả default nếu không có. + double param(const std::string& key, double fallback) const + { + const auto it = params.find(key); + return it == params.end() ? fallback : it->second; + } +}; + +/** + * @struct RecoveryResult + * @brief Kết quả hợp nhất + rich feedback cho cả 3 họ recovery. + * + * Bất biến output: chỉ đọc trường khớp với @ref output_type. * - kNone : bỏ qua command/path. * - kVelocity : dùng command; path để mặc định. * - 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 + * output_type. progress trong [0,1]; remaining theo đơn vị của goal (rad hoặc m). */ struct RecoveryResult { @@ -56,18 +112,30 @@ struct RecoveryResult robot_geometry_msgs::Twist command; ///< Hợp lệ khi output_type == kVelocity. robot_nav_msgs::Path path; ///< Hợp lệ khi output_type == kPath. + 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 elapsed = 0.0; ///< s — thời gian trôi từ start(). + std::string message; ///< Mô tả người-đọc-được (vd "rotated 1.20/1.57 rad"). + /// @brief Đang chạy, không output. static RecoveryResult Running(); /// @brief Thành công, không output. static RecoveryResult Succeeded(); /// @brief Thất bại, không output (caller nên dừng an toàn). static RecoveryResult Failed(); - /// @brief Output vận tốc kèm status (kRunning hoặc kSucceeded). + /// @brief Bị huỷ, không output (caller nên dừng an toàn). + static RecoveryResult Cancelled(); + /// @brief Output vận tốc kèm status (kRunning/kSucceeded/kFailed/kCancelled). static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command, RecoveryStatus status); /// @brief Output path kèm status. static RecoveryResult PathOut(const robot_nav_msgs::Path& path, RecoveryStatus status); + + /// @brief Gắn thêm feedback (fluent) — trả về chính nó để chain. + RecoveryResult& withProgress(double progress_value, double remaining_value); + /// @brief Gắn message (fluent). + RecoveryResult& withMessage(std::string text); }; } // namespace recovery_core diff --git a/plugins/back_up_recovery.cpp b/plugins/back_up_recovery.cpp index ab71e13..828e5b6 100644 --- a/plugins/back_up_recovery.cpp +++ b/plugins/back_up_recovery.cpp @@ -2,18 +2,16 @@ * * Software License Agreement (BSD License) * - * recovery_core — per-cycle backup recovery plugin. + * recovery_core — per-cycle backup recovery plugin (goal-driven). * * Author: DuongTD *********************************************************************/ #include -#include #include #include #include -#include #include #include @@ -22,129 +20,114 @@ namespace recovery_plugins { namespace { -robot_geometry_msgs::Twist zeroTwist() -{ - return robot_geometry_msgs::Twist(); -} - -bool validCycle(double dt) -{ - return std::isfinite(dt) && dt > 0.0; -} +constexpr double kDefaultBackupDistance = 0.5; // m. +constexpr double kDefaultLinearSpeed = 0.1; // m/s. } // namespace +/** + * @class BackUpRecovery + * @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal). + * + * goal.distance (m, > 0) là khoảng lùi lượt này; 0 nghĩa là dùng default configured. Tốc độ + * tuyến tính mặc định đọc từ param, có thể override qua goal.params["linear_speed"]. Mỗi + * update() trả Twist.linear.x < 0 kèm progress/remaining tới khi đủ khoảng -> kSucceeded. + */ class BackUpRecovery final : public recovery_core::RecoveryBehavior { public: BackUpRecovery() = default; - void initialize(std::string name, tf3::BufferCore* tf, - std::vector* global_path, - robot_costmap_2d::Costmap2DROBOT* global_costmap, - robot_costmap_2d::Costmap2DROBOT* local_costmap) override - { - if (initialized_) - { - robot::log_error("[recovery_core] BackUpRecovery '%s' initialized twice; ignoring.", - name_.c_str()); - return; - } - - name_ = std::move(name); - tf_ = tf; - global_path_ = global_path; - global_costmap_ = global_costmap; - local_costmap_ = local_costmap; - - robot::NodeHandle private_nh("~/" + name_); - config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh); - private_nh.param("backup_distance", backup_distance_, 0.5); - private_nh.param("linear_speed", linear_speed_, 0.1); - private_nh.param("require_costmap", require_costmap_, false); - - if (!std::isfinite(backup_distance_) || backup_distance_ <= 0.0) - { - robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.", - name_.c_str()); - backup_distance_ = 0.5; - } - if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0) - { - robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.", - name_.c_str()); - linear_speed_ = 0.1; - } - - initialized_ = true; - status_ = recovery_core::RecoveryStatus::kIdle; - } - - recovery_core::RecoveryResult runBehavior() override - { - status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning : - recovery_core::RecoveryStatus::kFailed; - return initialized_ ? recovery_core::RecoveryResult::Running() : - recovery_core::RecoveryResult::Failed(); - } - - recovery_core::RecoveryResult computeCommand(double dt) override - { - if (!initialized_ || !validCycle(dt) || (require_costmap_ && local_costmap_ == nullptr)) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), ); - } - - elapsed_ += dt; - if (config_.timeout > 0.0 && elapsed_ > config_.timeout) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); - } - - if (traveled_distance_ >= backup_distance_) - { - status_ = recovery_core::RecoveryStatus::kSucceeded; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); - } - - robot_geometry_msgs::Twist command; - command.linear.x = -std::abs(linear_speed_); - traveled_distance_ = std::min(backup_distance_, traveled_distance_ + std::abs(command.linear.x) * dt); - - if (traveled_distance_ >= backup_distance_) - { - status_ = recovery_core::RecoveryStatus::kSucceeded; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); - } - - status_ = recovery_core::RecoveryStatus::kRunning; - return recovery_core::RecoveryResult::Velocity(command, status_); - } - - recovery_core::RecoveryStatus status() const override - { - return status_; - } - static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { return std::make_shared(); } -private: - tf3::BufferCore* tf_ = nullptr; - std::vector* global_path_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr; - recovery_core::RecoveryConfig config_; - bool initialized_ = false; - recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle; +protected: + void onConfigure() override + { + robot::NodeHandle private_nh("~/" + name_); + private_nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance); + private_nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed); + private_nh.param("require_costmap", require_costmap_, false); - double backup_distance_ = 0.5; - double linear_speed_ = 0.1; + if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0) + { + robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.", + name_.c_str()); + default_backup_distance_ = kDefaultBackupDistance; + } + 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; + } + } + + recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override + { + // Costmap là bắt buộc? fail sớm trước khi xuất vận tốc lùi. + if (require_costmap_ && ctx().local_costmap == nullptr) + { + return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap"); + } + + backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0) + ? 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; + return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), + recovery_core::RecoveryStatus::kRunning) + .withProgress(0.0, backup_distance_) + .withMessage("backup start"); + } + + recovery_core::RecoveryResult onUpdate(double dt) override + { + if (traveled_distance_ >= backup_distance_) + { + return succeeded(); + } + + robot_geometry_msgs::Twist command; + command.linear.x = -std::abs(linear_speed_); + traveled_distance_ = + std::min(backup_distance_, traveled_distance_ + std::abs(command.linear.x) * dt); + + if (traveled_distance_ >= backup_distance_) + { + return succeeded(); + } + + return recovery_core::RecoveryResult::Velocity(command, + recovery_core::RecoveryStatus::kRunning) + .withProgress(traveled_distance_ / backup_distance_, + backup_distance_ - traveled_distance_) + .withMessage("backing up"); + } + +private: + recovery_core::RecoveryResult succeeded() + { + return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), + recovery_core::RecoveryStatus::kSucceeded) + .withProgress(1.0, 0.0) + .withMessage("backup complete"); + } + + double default_backup_distance_ = kDefaultBackupDistance; + double default_linear_speed_ = kDefaultLinearSpeed; bool require_costmap_ = false; - double elapsed_ = 0.0; + + double backup_distance_ = kDefaultBackupDistance; + double linear_speed_ = kDefaultLinearSpeed; double traveled_distance_ = 0.0; }; diff --git a/plugins/clear_costmap_recovery.cpp b/plugins/clear_costmap_recovery.cpp index 6c5401f..417116e 100644 --- a/plugins/clear_costmap_recovery.cpp +++ b/plugins/clear_costmap_recovery.cpp @@ -46,24 +46,14 @@ class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior public: ClearCostmapRecovery() = default; - void initialize(std::string name, tf3::BufferCore* tf, - std::vector* global_path, - robot_costmap_2d::Costmap2DROBOT* global_costmap, - robot_costmap_2d::Costmap2DROBOT* local_costmap) override + static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { - if (initialized_) - { - robot::log_error("[recovery_core] ClearCostmapRecovery '%s' initialized twice; ignoring.", - name_.c_str()); - return; - } - - name_ = std::move(name); - tf_ = tf; - global_path_ = global_path; - global_costmap_ = global_costmap; - local_costmap_ = local_costmap; + return std::make_shared(); + } +protected: + void onConfigure() override + { robot::NodeHandle private_nh("~/" + name_); private_nh.param("reset_distance", reset_distance_, 3.0); private_nh.param("invert_area_to_clear", invert_area_to_clear_, false); @@ -89,52 +79,37 @@ public: std::vector clearable_layers; private_nh.param("layer_names", clearable_layers, clearable_layers_default); clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end()); - - initialized_ = true; - status_ = recovery_core::RecoveryStatus::kIdle; } - recovery_core::RecoveryResult runBehavior() override + recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override { - if (!initialized_) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Failed(); - } + // One-shot: công việc thực hiện ở onUpdate() lần đầu. + return recovery_core::RecoveryResult::Running().withMessage("clear costmap start"); + } + recovery_core::RecoveryResult onUpdate(double /*dt*/) override + { bool ok = true; if (affected_maps_ == "global" || affected_maps_ == "both") { - ok = clear(global_costmap_) && ok; - if (ok && force_updating_ && global_costmap_ != nullptr) + ok = clear(ctx().global_costmap) && ok; + if (ok && force_updating_ && ctx().global_costmap != nullptr) { - global_costmap_->updateMap(); + ctx().global_costmap->updateMap(); } } if (affected_maps_ == "local" || affected_maps_ == "both") { - ok = clear(local_costmap_) && ok; - if (ok && force_updating_ && local_costmap_ != nullptr) + ok = clear(ctx().local_costmap) && ok; + if (ok && force_updating_ && ctx().local_costmap != nullptr) { - local_costmap_->updateMap(); + ctx().local_costmap->updateMap(); } } - status_ = ok ? recovery_core::RecoveryStatus::kSucceeded : - recovery_core::RecoveryStatus::kFailed; - return ok ? recovery_core::RecoveryResult::Succeeded() : - recovery_core::RecoveryResult::Failed(); - } - - recovery_core::RecoveryStatus status() const override - { - return status_; - } - - static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() - { - return std::make_shared(); + return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete") + : recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed"); } private: @@ -216,13 +191,6 @@ private: costmap->getOriginY() + costmap->getSizeInMetersY()); } - tf3::BufferCore* tf_ = nullptr; - std::vector* global_path_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr; - bool initialized_ = false; - recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle; - bool force_updating_ = false; double reset_distance_ = 3.0; bool invert_area_to_clear_ = false; diff --git a/plugins/regen_path_recovery.cpp b/plugins/regen_path_recovery.cpp index 007c7ab..2451800 100644 --- a/plugins/regen_path_recovery.cpp +++ b/plugins/regen_path_recovery.cpp @@ -2,81 +2,59 @@ * * Software License Agreement (BSD License) * - * recovery_core — path output recovery plugin. + * recovery_core — path output recovery plugin (goal-driven, one-shot). * * Author: DuongTD *********************************************************************/ #include -#include -#include - #include #include 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; - void initialize(std::string name, tf3::BufferCore* tf, - std::vector* global_path, - robot_costmap_2d::Costmap2DROBOT* global_costmap, - robot_costmap_2d::Costmap2DROBOT* local_costmap) override - { - if (initialized_) - { - robot::log_error("[recovery_core] RegenPathRecovery '%s' initialized twice; ignoring.", - name_.c_str()); - return; - } - - name_ = std::move(name); - tf_ = tf; - global_path_ = global_path; - global_costmap_ = global_costmap; - local_costmap_ = local_costmap; - - initialized_ = true; - status_ = recovery_core::RecoveryStatus::kIdle; - } - - recovery_core::RecoveryResult runBehavior() override - { - if (!initialized_ || global_path_ == nullptr || global_path_->empty()) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Failed(); - } - - robot_nav_msgs::Path path; - path.poses = *global_path_; - - status_ = recovery_core::RecoveryStatus::kSucceeded; - return recovery_core::RecoveryResult::PathOut(path, status_); - } - - recovery_core::RecoveryStatus status() const override - { - return status_; - } - static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { return std::make_shared(); } -private: - tf3::BufferCore* tf_ = nullptr; - std::vector* global_path_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr; - bool initialized_ = false; - recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle; +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(double /*dt*/) 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 diff --git a/plugins/rotate_recovery.cpp b/plugins/rotate_recovery.cpp index 022413a..ba90f94 100644 --- a/plugins/rotate_recovery.cpp +++ b/plugins/rotate_recovery.cpp @@ -2,18 +2,16 @@ * * Software License Agreement (BSD License) * - * recovery_core — per-cycle rotate recovery plugin. + * recovery_core — per-cycle rotate recovery plugin (goal-driven). * * Author: DuongTD *********************************************************************/ #include -#include #include #include #include -#include #include #include @@ -22,90 +20,76 @@ namespace recovery_plugins { namespace { -robot_geometry_msgs::Twist zeroTwist() -{ - return robot_geometry_msgs::Twist(); -} - -bool validCycle(double dt) -{ - return std::isfinite(dt) && dt > 0.0; -} +constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad. +constexpr double kDefaultAngularSpeed = 0.4; // rad/s. } // namespace +/** + * @class RotateRecovery + * @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal). + * + * goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ + * góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả + * Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command). + */ class RotateRecovery final : public recovery_core::RecoveryBehavior { public: RotateRecovery() = default; - void initialize(std::string name, tf3::BufferCore* tf, - std::vector* global_path, - robot_costmap_2d::Costmap2DROBOT* global_costmap, - robot_costmap_2d::Costmap2DROBOT* local_costmap) override + static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() { - if (initialized_) - { - robot::log_error("[recovery_core] RotateRecovery '%s' initialized twice; ignoring.", - name_.c_str()); - return; - } - - name_ = std::move(name); - tf_ = tf; - global_path_ = global_path; - global_costmap_ = global_costmap; - local_costmap_ = local_costmap; + return std::make_shared(); + } +protected: + void onConfigure() override + { robot::NodeHandle private_nh("~/" + name_); - config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh); - private_nh.param("target_angle", target_angle_, 1.57079632679); - private_nh.param("angular_speed", angular_speed_, 0.4); + private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle); + private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed); - if (!std::isfinite(target_angle_) || std::abs(target_angle_) <= 0.0) + if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0) { robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.", name_.c_str()); - target_angle_ = 1.57079632679; + default_target_angle_ = kDefaultTargetAngle; } - if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0) + 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()); - angular_speed_ = 0.4; + default_angular_speed_ = kDefaultAngularSpeed; } - - initialized_ = true; - status_ = recovery_core::RecoveryStatus::kIdle; } - recovery_core::RecoveryResult runBehavior() override + recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override { - status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning : - recovery_core::RecoveryStatus::kFailed; - return initialized_ ? recovery_core::RecoveryResult::Running() : - recovery_core::RecoveryResult::Failed(); + // Góc đích: goal.angle nếu hợp lệ, ngược lại default configured. + target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0) + ? goal.angle + : 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 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 computeCommand(double dt) override + recovery_core::RecoveryResult onUpdate(double dt) override { - if (!initialized_ || !validCycle(dt)) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); - } - - elapsed_ += dt; - if (config_.timeout > 0.0 && elapsed_ > config_.timeout) - { - status_ = recovery_core::RecoveryStatus::kFailed; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); - } - const double target = std::abs(target_angle_); + if (rotated_angle_ >= target) { - status_ = recovery_core::RecoveryStatus::kSucceeded; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); + return succeeded(target); } robot_geometry_msgs::Twist command; @@ -114,36 +98,29 @@ public: if (rotated_angle_ >= target) { - status_ = recovery_core::RecoveryStatus::kSucceeded; - return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_); + return succeeded(target); } - status_ = recovery_core::RecoveryStatus::kRunning; - return recovery_core::RecoveryResult::Velocity(command, status_); - } - - recovery_core::RecoveryStatus status() const override - { - return status_; - } - - static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() - { - return std::make_shared(); + return recovery_core::RecoveryResult::Velocity(command, + recovery_core::RecoveryStatus::kRunning) + .withProgress(rotated_angle_ / target, target - rotated_angle_) + .withMessage("rotating"); } private: - tf3::BufferCore* tf_ = nullptr; - std::vector* global_path_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr; - robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr; - recovery_core::RecoveryConfig config_; - bool initialized_ = false; - recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle; + recovery_core::RecoveryResult succeeded(double target) + { + return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(), + recovery_core::RecoveryStatus::kSucceeded) + .withProgress(1.0, 0.0) + .withMessage("rotate complete"); + } - double target_angle_ = 1.57079632679; - double angular_speed_ = 0.4; - double elapsed_ = 0.0; + double default_target_angle_ = kDefaultTargetAngle; + double default_angular_speed_ = kDefaultAngularSpeed; + + double target_angle_ = kDefaultTargetAngle; + double angular_speed_ = kDefaultAngularSpeed; double rotated_angle_ = 0.0; }; diff --git a/src/recovery_behavior.cpp b/src/recovery_behavior.cpp index f5b2f80..97e13ea 100644 --- a/src/recovery_behavior.cpp +++ b/src/recovery_behavior.cpp @@ -1,19 +1,118 @@ /********************************************************************* - * recovery_core — default impl cho RecoveryBehavior. + * recovery_core — phần chung (template-method) của RecoveryBehavior. * - * Chỉ chứa default computeCommand() (họ A/B không override). initialize()/runBehavior()/ - * status() là thuần ảo, do plugin cụ thể triển khai. + * configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời, timeout, đo + * elapsed, và xử lý cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate(). * * Author: DuongTD *********************************************************************/ #include +#include + +#include + namespace recovery_core { - -RecoveryResult RecoveryBehavior::computeCommand(double /*dt*/) +namespace { - return RecoveryResult::Failed(); +robot_geometry_msgs::Twist zeroTwist() +{ + return robot_geometry_msgs::Twist(); +} + +bool validCycle(double dt) +{ + return std::isfinite(dt) && dt > 0.0; +} +} // namespace + +void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx) +{ + if (configured_) + { + return; + } + + name_ = name; + ctx_ = ctx; + status_ = RecoveryStatus::kIdle; + + // Param chung duy nhất còn lại: timeout (s, 0 = không giới hạn). + robot::NodeHandle private_nh("~/" + name_); + private_nh.param("timeout", timeout_, 0.0); + if (!std::isfinite(timeout_) || timeout_ < 0.0) + { + robot::log_warning("[recovery_core] Invalid timeout for '%s'; using 0 (no timeout).", + name_.c_str()); + timeout_ = 0.0; + } + + onConfigure(); + + configured_ = true; +} + +RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal) +{ + if (!configured_) + { + status_ = RecoveryStatus::kFailed; + return RecoveryResult::Failed().withMessage("start() before configure()"); + } + + goal_ = goal; + elapsed_ = 0.0; + cancel_requested_ = false; + started_ = true; + status_ = RecoveryStatus::kRunning; + + return onStart(goal_); +} + +RecoveryResult RecoveryBehavior::update(double dt) +{ + if (!configured_ || !started_) + { + status_ = RecoveryStatus::kFailed; + return RecoveryResult::Failed().withMessage("update() before start()"); + } + + // Đã kết thúc: giữ nguyên trạng thái, không tick thêm. + if (status_ != RecoveryStatus::kRunning) + { + return RecoveryResult::Velocity(zeroTwist(), status_); + } + + if (cancel_requested_) + { + status_ = RecoveryStatus::kCancelled; + return RecoveryResult::Velocity(zeroTwist(), status_) + .withMessage("cancelled by caller"); + } + + if (!validCycle(dt)) + { + status_ = RecoveryStatus::kFailed; + return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("invalid dt"); + } + + elapsed_ += dt; + if (config_.timeout > 0.0 && elapsed_ > config_.timeout) + { + status_ = RecoveryStatus::kFailed; + return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("timeout"); + } + + RecoveryResult result = onUpdate(dt); + result.elapsed = elapsed_; + status_ = result.status; + return result; +} + +void RecoveryBehavior::cancel() +{ + cancel_requested_ = true; } } // namespace recovery_core diff --git a/src/recovery_config.cpp b/src/recovery_config.cpp deleted file mode 100644 index 5b32bba..0000000 --- a/src/recovery_config.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/********************************************************************* - * recovery_core — validate + đọc RecoveryConfig. - * - * Author: DuongTD - *********************************************************************/ -#include - -#include -#include - -#include - -namespace recovery_core -{ -namespace -{ - -constexpr double kDefaultControlFrequency = 20.0; -constexpr double kDefaultTimeout = 0.0; - -void appendError(std::string* error, const std::string& message) -{ - if (error == nullptr) - { - return; - } - - if (!error->empty()) - { - *error += "; "; - } - *error += message; -} - -bool invalidControlFrequency(double value) -{ - return !std::isfinite(value) || value <= 0.0; -} - -bool invalidTimeout(double value) -{ - return !std::isfinite(value) || value < 0.0; -} - -} // namespace - -bool RecoveryConfig::validate(std::string* error) const -{ - if (error != nullptr) - { - error->clear(); - } - - bool valid = true; - if (invalidControlFrequency(control_frequency)) - { - appendError(error, "control_frequency must be finite and > 0 Hz"); - valid = false; - } - - if (invalidTimeout(timeout)) - { - appendError(error, "timeout must be finite and >= 0 s"); - valid = false; - } - - return valid; -} - -RecoveryConfig RecoveryConfig::fromNodeHandle(robot::NodeHandle& nh) -{ - RecoveryConfig config; - nh.param("control_frequency", config.control_frequency, kDefaultControlFrequency); - nh.param("timeout", config.timeout, kDefaultTimeout); - - std::string error; - if (config.validate(&error)) - { - return config; - } - - robot::log_warning("[recovery_core] Invalid common recovery config: %s. " - "Replacing invalid values with defaults.", - error.c_str()); - - if (invalidControlFrequency(config.control_frequency)) - { - config.control_frequency = kDefaultControlFrequency; - } - if (invalidTimeout(config.timeout)) - { - config.timeout = kDefaultTimeout; - } - - if (!config.validate(nullptr)) - { - return RecoveryConfig{}; - } - return config; -} - -} // namespace recovery_core diff --git a/src/recovery_types.cpp b/src/recovery_types.cpp index e738d1e..b0cf2d5 100644 --- a/src/recovery_types.cpp +++ b/src/recovery_types.cpp @@ -5,6 +5,8 @@ *********************************************************************/ #include +#include + namespace recovery_core { @@ -21,6 +23,7 @@ RecoveryResult RecoveryResult::Succeeded() RecoveryResult result; result.status = RecoveryStatus::kSucceeded; result.output_type = RecoveryOutputType::kNone; + result.progress = 1.0; return result; } @@ -32,6 +35,14 @@ RecoveryResult RecoveryResult::Failed() return result; } +RecoveryResult RecoveryResult::Cancelled() +{ + RecoveryResult result; + result.status = RecoveryStatus::kCancelled; + result.output_type = RecoveryOutputType::kNone; + return result; +} + RecoveryResult RecoveryResult::Velocity(const robot_geometry_msgs::Twist& command, RecoveryStatus status) { @@ -39,6 +50,10 @@ RecoveryResult RecoveryResult::Velocity(const robot_geometry_msgs::Twist& comman result.status = status; result.output_type = RecoveryOutputType::kVelocity; result.command = command; + if (status == RecoveryStatus::kSucceeded) + { + result.progress = 1.0; + } return result; } @@ -49,7 +64,24 @@ RecoveryResult RecoveryResult::PathOut(const robot_nav_msgs::Path& path, result.status = status; result.output_type = RecoveryOutputType::kPath; result.path = path; + if (status == RecoveryStatus::kSucceeded) + { + result.progress = 1.0; + } return result; } +RecoveryResult& RecoveryResult::withProgress(double progress_value, double remaining_value) +{ + progress = progress_value < 0.0 ? 0.0 : (progress_value > 1.0 ? 1.0 : progress_value); + remaining = remaining_value < 0.0 ? 0.0 : remaining_value; + return *this; +} + +RecoveryResult& RecoveryResult::withMessage(std::string text) +{ + message = std::move(text); + return *this; +} + } // namespace recovery_core diff --git a/test/plugin_loader_contract_test.cpp b/test/plugin_loader_contract_test.cpp index 1043494..2e2a281 100644 --- a/test/plugin_loader_contract_test.cpp +++ b/test/plugin_loader_contract_test.cpp @@ -152,7 +152,9 @@ void testLoadPlugins() // 4. Chuyển quyền giữ library vào storage sống lâu dài. libraries_.push_back(std::move(library)); - behavior->initialize(plugin.name, nullptr, &global_path_, nullptr, nullptr); + recovery_core::RecoveryContext ctx; + ctx.global_path = &global_path_; + behavior->configure(plugin.name, ctx); creators_.push_back(behavior); } catch (const std::exception& e) @@ -170,18 +172,28 @@ for(const auto& behavior : creators_) { if(behavior->getNameRecoveryBehavior() == "rotation_rc") { - const recovery_core::RecoveryResult first = behavior->computeCommand(0.1); + // 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(0.1); 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 by default"); + 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->computeCommand(0.1); + last = behavior->update(0.1); } expect(last.status == recovery_core::RecoveryStatus::kSucceeded, @@ -190,6 +202,39 @@ for(const auto& behavior : creators_) "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(0.1); + 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(0.1); + } + + 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"); } } } @@ -208,7 +253,8 @@ global_path_.push_back(pose2); { if(behavior->getNameRecoveryBehavior() == "regen_path_rc") { - const recovery_core::RecoveryResult result = behavior->runBehavior(); + behavior->start(recovery_core::RecoveryGoal()); + const recovery_core::RecoveryResult result = behavior->update(0.1); expect(result.status == recovery_core::RecoveryStatus::kSucceeded, "regen path must succeed with a non-empty global path"); @@ -234,6 +280,7 @@ int main() testRegenPathPlugin(); testRotatePlugin(); + testBackupPlugin(); std::cout << "[PASS] recovery_core plugin loader contract" << std::endl; return 0;