From 17088a227d4f1683e3d75d9a6c504fc61b666e45 Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 9 Jul 2026 20:41:35 +0700 Subject: [PATCH] temporary storage 7/9/2026 20:41 --- PLAN.md | 34 ++++++++--------- README.md | 8 ++-- docs/ARCHITECTURE.md | 16 ++++---- docs/PLUGIN_GUIDE.md | 12 +++--- docs/SAFETY.md | 9 ++--- include/recovery_core/recovery_behavior.h | 21 +++-------- plugins/back_up_recovery.cpp | 15 ++++++-- plugins/clear_costmap_recovery.cpp | 2 +- plugins/regen_path_recovery.cpp | 2 +- plugins/rotate_recovery.cpp | 14 ++++++- src/recovery_behavior.cpp | 46 ++++------------------- test/plugin_loader_contract_test.cpp | 10 ++--- 12 files changed, 82 insertions(+), 107 deletions(-) diff --git a/PLAN.md b/PLAN.md index 70f7eeb..37dd4af 100644 --- a/PLAN.md +++ b/PLAN.md @@ -28,7 +28,7 @@ plugin recovery có thể trả trạng thái, velocity command hoặc path. - 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()`). +- 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 @@ -120,7 +120,7 @@ public: robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0; virtual RecoveryResult runBehavior() = 0; - virtual RecoveryResult computeCommand(double dt); + virtual RecoveryResult update(); virtual RecoveryStatus status() const = 0; protected: @@ -131,7 +131,7 @@ protected: Đ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ó `computeCommand(dt)` cho behavior per-cycle. +- Có `update()` cho behavior per-cycle. ## 4. Ba Nhóm Recovery @@ -139,7 +139,7 @@ protected: |------|-------|--------------|--------| | 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 | `computeCommand(dt)` | `RecoveryOutputType::kVelocity` | +| 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; @@ -228,10 +228,10 @@ Work items: 1. [x] Implement `RecoveryResult` factories. 2. [x] Implement `RecoveryConfig::validate`. 3. [x] Implement `RecoveryConfig::fromNodeHandle`. -4. [x] Giữ default `RecoveryBehavior::computeCommand(dt)` trả `Failed()`. +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 `control_frequency`; - - tôn trọng `timeout`; + - 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. @@ -243,8 +243,8 @@ Acceptance: - `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 reject `NaN`, `inf`, `control_frequency <= 0`, `timeout < 0`. -- Default `computeCommand(dt)` không sinh velocity mù. +- 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: @@ -275,12 +275,12 @@ Plugin đề xuất: - [x] trả `RecoveryResult::Succeeded()` hoặc `Failed()`. 2. `RotateRecovery` - [x] nhóm C, per-cycle velocity; - - [x] đọc `target_angle`, `angular_speed`, `timeout`; - - [x] dùng tích phân theo `dt` trong plugin mẫu; adapter production có thể thay bằng pose/tf; + - [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`, `timeout`; + - [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; @@ -344,22 +344,22 @@ Phương án: - Adapter chịu trách nhiệm: - load plugin; - gọi `initialize`; - - gọi `runBehavior` hoặc loop `computeCommand`; + - 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/timeout. + - á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. -- Timeout và failure path luôn trả stop command hoặc abort rõ ràng. +- 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. -- `dt <= 0`, `NaN`, `inf`, missing tf/costmap/plan phải fail rõ ràng. +- 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õ: @@ -399,7 +399,7 @@ Acceptance: ### Integration DoD - Adapter là nơi duy nhất có side effect publish/service/path replacement. -- Safety stop rõ ràng khi failed/timeout. +- 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 diff --git a/README.md b/README.md index 5da58c4..abd6242 100644 --- a/README.md +++ b/README.md @@ -20,11 +20,11 @@ behavior->configure(name, ctx); // 1 lần: ctx = {tf, global_path, glo 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 + r = behavior->update(); // 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)`. +API công khai `configure/start/update/cancel` là non-virtual (base lo guard vòng đời/cancel); +plugin chỉ override hook `onConfigure()/onStart(goal)/onUpdate()`. ## Ba họ recovery @@ -32,7 +32,7 @@ plugin chỉ override hook `onConfigure()/onStart(goal)/onUpdate(dt)`. |----|-------|--------|-----------| | 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 | rotation / backup | `robot_geometry_msgs::Twist` mỗi cycle | `onStart(goal)` + `onUpdate()` | Cả 3 chia sẻ một `RecoveryResult` hợp nhất mang cờ `output_type` + rich feedback (`progress`/`remaining`/`elapsed`/`message`). diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 5065aed..9642c4e 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -9,7 +9,7 @@ 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, template-method): API non-virtual `configure`/`start`/`update`/ - `cancel`; plugin chỉ triển khai hook `onConfigure`/`onStart(goal)`/`onUpdate(dt)`. + `cancel`; plugin chỉ triển khai hook `onConfigure`/`onStart(goal)`/`onUpdate()`. - `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ọ + @@ -27,22 +27,20 @@ configure(name, ctx) // 1 lần: cache ctx, đọc config chung, onConf │ start(goal) // mỗi lượt: chốt mục tiêu runtime, onStart() │ - ├── one-shot (họ A/B): update(dt) 1 lần ─────────► RecoveryResult{status, path|none, msg} + ├── one-shot (họ A/B): update() 1 lần ───────────► RecoveryResult{status, path|none, msg} │ - └── per-cycle (họ C): loop { update(dt) } ──────► RecoveryResult{status, velocity, + └── per-cycle (họ C): loop { update() } ────────► RecoveryResult{status, velocity, (tới khi status != kRunning) progress, remaining} ``` ## Ghi chú thiết kế -- 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. +- Template-method: base xử lý guard vòng đời (configure→start→update) 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ố. +- `onUpdate()` 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()`. +- 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 161d1c5..17f3137 100644 --- a/docs/PLUGIN_GUIDE.md +++ b/docs/PLUGIN_GUIDE.md @@ -11,8 +11,8 @@ Package hiện có 4 plugin mẫu dưới `plugins/`: 1. Kế thừa `recovery_core::RecoveryBehavior`. 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. +3. Override `onStart(goal)` + `onUpdate()` theo họ (xem dưới). KHÔNG override + `configure/start/update/cancel` — base đã lo guard vòng đời/cancel. 4. Thêm factory `static RecoveryBehaviorPtr create()` **không tham số** + `BOOST_DLL_ALIAS(...)`. ## Vòng đời (goal-driven) @@ -22,7 +22,7 @@ configure(name, ctx) // 1 lần: cache ctx, đọc config chung, gọi onCo │ 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 +loop update() // mỗi cycle tới khi status != kRunning; base guard vòng đời/cancel │ [cancel()] // update() kế tiếp -> stop output + kCancelled ``` @@ -37,7 +37,7 @@ qua `goal.params` (vd `goal.params["angular_speed"] = 0.8`). |----|----------|--------| | 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)` | +| C. velocity | `onStart()` chốt goal + `onUpdate()` 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). @@ -55,7 +55,7 @@ class SpinRecovery : public recovery_core::RecoveryBehavior { return std::make_shared(); } protected: - // override onConfigure()/onStart(goal)/onUpdate(dt)... + // override onConfigure()/onStart(goal)/onUpdate()... }; } // namespace recovery_plugins @@ -80,7 +80,7 @@ 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 + r = behavior->update(); // publish r.command; đọc r.progress/r.remaining/r.message } ``` diff --git a/docs/SAFETY.md b/docs/SAFETY.md index 0fa34ff..c37d7dd 100644 --- a/docs/SAFETY.md +++ b/docs/SAFETY.md @@ -7,18 +7,17 @@ - **Không collision-check tự động.** Interface không kiểm tra va chạm khi sinh vận tốc (backup/spin) hay khi tạo path. Việc tránh va chạm là trách nhiệm của **plugin** (dùng costmap được cấp qua `initialize`) hoặc của **caller**. -- **Không quản lý vòng lặp thời gian thực.** Caller chịu trách nhiệm gọi `computeCommand(dt)` - đúng nhịp và publish command. +- **Không quản lý vòng lặp thời gian thực.** Caller chịu trách nhiệm gọi `update()` đú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). - **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`. + `RecoveryResult::Failed()`. Sau `cancel()`, base trả stop output (Twist 0) + `kCancelled`. ## Nguyên tắc cho plugin - 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. +- Không cần tự guard vòng đời/cancel — base đã lo; plugin 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 d502c29..d7aa293 100644 --- a/include/recovery_core/recovery_behavior.h +++ b/include/recovery_core/recovery_behavior.h @@ -5,8 +5,8 @@ * recovery_core — interface (base class) cho recovery behaviors. * * 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ỉ + * configure(name, ctx) -> start(goal) -> lặp update() -> [cancel()] + * Base lo phần chung (guard init/cancel, đẩy feedback); plugin chỉ * override các hook nhỏ onConfigure()/onStart()/onUpdate(). * * Author: DuongTD @@ -35,7 +35,7 @@ namespace recovery_core * 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. + * - onUpdate() : một "tick"; họ one-shot (path/clear) hoàn tất ngay lần đầu. * * Ba họ hành vi: * - Họ A (trả path) : onUpdate() trả RecoveryResult::PathOut(...), kSucceeded ngay. @@ -73,10 +73,9 @@ public: 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. + * @brief Một control cycle. Guard chưa start/cancel trước khi gọi onUpdate(). */ - RecoveryResult update(double dt); + RecoveryResult update(); /** * @brief Yêu cầu huỷ: update() kế tiếp trả stop output + kCancelled. @@ -89,12 +88,6 @@ public: return status_; } - /// @brief Thời gian đã trôi từ start() (s). - double elapsed() const - { - return elapsed_; - } - const std::string& name() const { return name_; @@ -120,7 +113,7 @@ protected: 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; + virtual RecoveryResult onUpdate() = 0; // Truy cập cho plugin (chỉ đọc ngữ cảnh/goal). const RecoveryContext& ctx() const { return ctx_; } @@ -130,8 +123,6 @@ protected: 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; diff --git a/plugins/back_up_recovery.cpp b/plugins/back_up_recovery.cpp index 828e5b6..9e98a05 100644 --- a/plugins/back_up_recovery.cpp +++ b/plugins/back_up_recovery.cpp @@ -22,6 +22,7 @@ namespace { constexpr double kDefaultBackupDistance = 0.5; // m. constexpr double kDefaultLinearSpeed = 0.1; // m/s. +constexpr double kDefaultControlPeriod = 0.1; // s per update tick. } // namespace /** @@ -48,6 +49,7 @@ protected: 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("control_period", control_period_, kDefaultControlPeriod); private_nh.param("require_costmap", require_costmap_, false); if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0) @@ -62,6 +64,12 @@ protected: name_.c_str()); default_linear_speed_ = kDefaultLinearSpeed; } + if (!std::isfinite(control_period_) || control_period_ <= 0.0) + { + robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.", + name_.c_str()); + control_period_ = kDefaultControlPeriod; + } } recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override @@ -89,7 +97,7 @@ protected: .withMessage("backup start"); } - recovery_core::RecoveryResult onUpdate(double dt) override + recovery_core::RecoveryResult onUpdate() override { if (traveled_distance_ >= backup_distance_) { @@ -98,8 +106,8 @@ protected: 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); + traveled_distance_ = std::min( + backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_); if (traveled_distance_ >= backup_distance_) { @@ -124,6 +132,7 @@ private: double default_backup_distance_ = kDefaultBackupDistance; double default_linear_speed_ = kDefaultLinearSpeed; + double control_period_ = kDefaultControlPeriod; bool require_costmap_ = false; double backup_distance_ = kDefaultBackupDistance; diff --git a/plugins/clear_costmap_recovery.cpp b/plugins/clear_costmap_recovery.cpp index 417116e..9648c29 100644 --- a/plugins/clear_costmap_recovery.cpp +++ b/plugins/clear_costmap_recovery.cpp @@ -87,7 +87,7 @@ protected: return recovery_core::RecoveryResult::Running().withMessage("clear costmap start"); } - recovery_core::RecoveryResult onUpdate(double /*dt*/) override + recovery_core::RecoveryResult onUpdate() override { bool ok = true; if (affected_maps_ == "global" || affected_maps_ == "both") diff --git a/plugins/regen_path_recovery.cpp b/plugins/regen_path_recovery.cpp index 2451800..33679b0 100644 --- a/plugins/regen_path_recovery.cpp +++ b/plugins/regen_path_recovery.cpp @@ -40,7 +40,7 @@ protected: .withMessage("regen path start"); } - recovery_core::RecoveryResult onUpdate(double /*dt*/) override + recovery_core::RecoveryResult onUpdate() override { const auto* global_path = ctx().global_path; if (global_path == nullptr || global_path->empty()) diff --git a/plugins/rotate_recovery.cpp b/plugins/rotate_recovery.cpp index ba90f94..f519ea7 100644 --- a/plugins/rotate_recovery.cpp +++ b/plugins/rotate_recovery.cpp @@ -22,6 +22,7 @@ namespace { constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad. constexpr double kDefaultAngularSpeed = 0.4; // rad/s. +constexpr double kDefaultControlPeriod = 0.1; // s per update tick. } // namespace /** @@ -48,6 +49,7 @@ protected: robot::NodeHandle private_nh("~/" + name_); private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle); private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed); + private_nh.param("control_period", control_period_, kDefaultControlPeriod); if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0) { @@ -61,6 +63,12 @@ protected: name_.c_str()); default_angular_speed_ = kDefaultAngularSpeed; } + if (!std::isfinite(control_period_) || control_period_ <= 0.0) + { + robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.", + name_.c_str()); + control_period_ = kDefaultControlPeriod; + } } recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override @@ -83,7 +91,7 @@ protected: .withMessage("rotate start"); } - recovery_core::RecoveryResult onUpdate(double dt) override + recovery_core::RecoveryResult onUpdate() override { const double target = std::abs(target_angle_); @@ -94,7 +102,8 @@ protected: robot_geometry_msgs::Twist command; command.angular.z = std::copysign(angular_speed_, target_angle_); - rotated_angle_ = std::min(target, rotated_angle_ + std::abs(command.angular.z) * dt); + rotated_angle_ = + std::min(target, rotated_angle_ + std::abs(command.angular.z) * control_period_); if (rotated_angle_ >= target) { @@ -118,6 +127,7 @@ private: double default_target_angle_ = kDefaultTargetAngle; double default_angular_speed_ = kDefaultAngularSpeed; + double control_period_ = kDefaultControlPeriod; double target_angle_ = kDefaultTargetAngle; double angular_speed_ = kDefaultAngularSpeed; diff --git a/src/recovery_behavior.cpp b/src/recovery_behavior.cpp index 97e13ea..3657c7d 100644 --- a/src/recovery_behavior.cpp +++ b/src/recovery_behavior.cpp @@ -1,17 +1,13 @@ /********************************************************************* * recovery_core — phần chung (template-method) của RecoveryBehavior. * - * 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(). + * configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời và xử lý + * cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate(). * * Author: DuongTD *********************************************************************/ #include -#include - -#include - namespace recovery_core { namespace @@ -20,11 +16,6 @@ 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) @@ -38,16 +29,6 @@ void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& 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; @@ -62,15 +43,16 @@ RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal) } goal_ = goal; - elapsed_ = 0.0; cancel_requested_ = false; started_ = true; status_ = RecoveryStatus::kRunning; - return onStart(goal_); + RecoveryResult result = onStart(goal_); + status_ = result.status; + return result; } -RecoveryResult RecoveryBehavior::update(double dt) +RecoveryResult RecoveryBehavior::update() { if (!configured_ || !started_) { @@ -91,21 +73,7 @@ RecoveryResult RecoveryBehavior::update(double dt) .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_; + RecoveryResult result = onUpdate(); status_ = result.status; return result; } diff --git a/test/plugin_loader_contract_test.cpp b/test/plugin_loader_contract_test.cpp index 2e2a281..66a6977 100644 --- a/test/plugin_loader_contract_test.cpp +++ b/test/plugin_loader_contract_test.cpp @@ -180,7 +180,7 @@ for(const auto& behavior : creators_) expect(started.status == recovery_core::RecoveryStatus::kRunning, "rotate must be running right after start"); - const recovery_core::RecoveryResult first = behavior->update(0.1); + const recovery_core::RecoveryResult first = behavior->update(); expect(first.status == recovery_core::RecoveryStatus::kRunning, "rotate first cycle must be running"); expect(first.output_type == recovery_core::RecoveryOutputType::kVelocity, @@ -193,7 +193,7 @@ for(const auto& behavior : creators_) recovery_core::RecoveryResult last = first; for (int i = 0; i < 100 && last.status == recovery_core::RecoveryStatus::kRunning; ++i) { - last = behavior->update(0.1); + last = behavior->update(); } expect(last.status == recovery_core::RecoveryStatus::kSucceeded, @@ -219,7 +219,7 @@ for(const auto& behavior : creators_) goal.distance = 0.3; behavior->start(goal); - const recovery_core::RecoveryResult first = behavior->update(0.1); + const recovery_core::RecoveryResult first = behavior->update(); 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"); @@ -227,7 +227,7 @@ for(const auto& behavior : creators_) recovery_core::RecoveryResult last = first; for (int i = 0; i < 1000 && last.status == recovery_core::RecoveryStatus::kRunning; ++i) { - last = behavior->update(0.1); + last = behavior->update(); } expect(last.status == recovery_core::RecoveryStatus::kSucceeded, @@ -254,7 +254,7 @@ global_path_.push_back(pose2); if(behavior->getNameRecoveryBehavior() == "regen_path_rc") { behavior->start(recovery_core::RecoveryGoal()); - const recovery_core::RecoveryResult result = behavior->update(0.1); + const recovery_core::RecoveryResult result = behavior->update(); expect(result.status == recovery_core::RecoveryStatus::kSucceeded, "regen path must succeed with a non-empty global path");