DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

temporary storage 7/9/2026 19:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 19:41:20 +07:00
parent 915cf85cc5
commit e9394434ba
17 changed files with 658 additions and 566 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -195,7 +195,6 @@ endif()
# ========================================================
add_library(recovery_core SHARED
src/recovery_types.cpp
src/recovery_config.cpp
src/recovery_behavior.cpp
)

21
PLAN.md
View File

@@ -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

30
README.md
View File

@@ -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

41
docs/ARCHITECTURE.md
View File

@@ -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ừ `~/<name>/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.

56
docs/PLUGIN_GUIDE.md
View File

@@ -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<SpinRecovery>();
}
// 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 <boost/dll/import.hpp>
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(
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

9
docs/SAFETY.md
View File

@@ -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).

121
include/recovery_core/recovery_behavior.h
View File

@@ -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<recovery_core::RecoveryBehavior::Ptr()>(...)).
/// (boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(...)).
using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>;
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<robot_geometry_msgs::PoseStamped>* 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 "~/<name>/timeout".
double elapsed_ = 0.0;
bool configured_ = false;
bool started_ = false;
bool cancel_requested_ = false;
RecoveryStatus status_ = RecoveryStatus::kIdle;
};
} // namespace recovery_core

53
include/recovery_core/recovery_config.h
View File

@@ -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 <string>
// Forward declare để không kéo <robot/robot.h> 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_

84
include/recovery_core/recovery_types.h
View File

@@ -9,9 +9,18 @@
#ifndef RECOVERY_CORE_RECOVERY_TYPES_H_
#define RECOVERY_CORE_RECOVERY_TYPES_H_
#include <map>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_nav_msgs/Path.h>
// 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<robot_geometry_msgs::PoseStamped>* 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<std::string, double> 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

207
plugins/back_up_recovery.cpp
View File

@@ -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 <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_config.h>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
@@ -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<robot_geometry_msgs::PoseStamped>* 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<BackUpRecovery>();
}
private:
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* 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;
};

72
plugins/clear_costmap_recovery.cpp
View File

@@ -46,24 +46,14 @@ class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
public:
ClearCostmapRecovery() = default;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) 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<ClearCostmapRecovery>();
}
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<std::string> 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<ClearCostmapRecovery>();
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<robot_geometry_msgs::PoseStamped>* 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;

84
plugins/regen_path_recovery.cpp
View File

@@ -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 <recovery_core/recovery_behavior.h>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
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<robot_geometry_msgs::PoseStamped>* 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<RegenPathRecovery>();
}
private:
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* 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

143
plugins/rotate_recovery.cpp
View File

@@ -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 <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_config.h>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
@@ -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<robot_geometry_msgs::PoseStamped>* 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<RotateRecovery>();
}
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<RotateRecovery>();
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<robot_geometry_msgs::PoseStamped>* 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;
};

111
src/recovery_behavior.cpp
View File

@@ -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 <recovery_core/recovery_behavior.h>
#include <cmath>
#include <robot/robot.h>
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

102
src/recovery_config.cpp
View File

@@ -1,102 +0,0 @@
/*********************************************************************
* recovery_core — validate + đọc RecoveryConfig.
*
* Author: DuongTD
*********************************************************************/
#include <recovery_core/recovery_config.h>
#include <cmath>
#include <string>
#include <robot/robot.h>
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

32
src/recovery_types.cpp
View File

@@ -5,6 +5,8 @@
*********************************************************************/
#include <recovery_core/recovery_types.h>
#include <utility>
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

57
test/plugin_loader_contract_test.cpp
View File

@@ -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;