optimal & fix file cmake
This commit is contained in:
@@ -1,110 +1,160 @@
|
||||
# Hướng Dẫn Viết Plugin recovery_core
|
||||
# Viết một recovery behavior mới
|
||||
|
||||
Package hiện có 4 plugin mẫu dưới `plugins/`:
|
||||
- `clear_costmap_recovery` — nhóm B, one-shot, no output.
|
||||
- `rotate_recovery` — nhóm C, per-cycle velocity.
|
||||
- `back_up_recovery` — nhóm C, per-cycle velocity.
|
||||
- `regen_path_recovery` — nhóm A, path output.
|
||||
## 1. Chọn họ output
|
||||
|
||||
## Bước chung
|
||||
Quyết định đầu tiên và không đổi được về sau: `outputKind()`.
|
||||
|
||||
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()` 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(...)`.
|
||||
| Họ | Khi nào | Cổng bắt buộc trong context |
|
||||
|---|---|---|
|
||||
| `kNone` | Behavior không lái robot (đợi, xoá costmap, gọi thiết bị ngoài) | — |
|
||||
| `kVelocity` | Behavior tự lái từng cycle | `PoseProvider` + `CollisionChecker` |
|
||||
| `kPath` | Behavior sinh ra đường đi mới | `PlanProvider` |
|
||||
|
||||
## Vòng đời (goal-driven)
|
||||
Base kiểm cổng theo họ ngay ở `configure()`, nên một behavior họ velocity thiếu collision checker sẽ
|
||||
**không nạp được**, thay vì phát hiện lúc đang lái.
|
||||
|
||||
```
|
||||
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() // mỗi cycle tới khi status != kRunning; base guard vòng đời/cancel
|
||||
│
|
||||
[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 | `onUpdate()` (one-shot) | `RecoveryResult::PathOut(path, kSucceeded)` |
|
||||
| B. none | `onUpdate()` (one-shot) | `RecoveryResult::Succeeded()` / `Failed()` |
|
||||
| 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).
|
||||
|
||||
## Export bằng Boost.DLL (bắt buộc cho plugin)
|
||||
## 2. Khung plugin
|
||||
|
||||
```cpp
|
||||
#include <recovery_core/recovery_behavior.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <recovery_core/recovery_math.h>
|
||||
|
||||
namespace recovery_plugins {
|
||||
class SpinRecovery : public recovery_core::RecoveryBehavior {
|
||||
public:
|
||||
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() {
|
||||
return std::make_shared<SpinRecovery>();
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <robot/robot.h>
|
||||
|
||||
namespace recovery_plugins
|
||||
{
|
||||
namespace
|
||||
{
|
||||
constexpr double kDefaultLimit = 1.0; // [m] đơn vị ghi ngay tại chỗ khai báo
|
||||
}
|
||||
|
||||
class MyRecovery final : public recovery_core::RecoveryBehavior
|
||||
{
|
||||
public:
|
||||
MyRecovery() = default;
|
||||
|
||||
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
||||
{
|
||||
return std::make_shared<MyRecovery>();
|
||||
}
|
||||
protected:
|
||||
// override onConfigure()/onStart(goal)/onUpdate()...
|
||||
|
||||
recovery_core::RecoveryOutputType outputKind() const override
|
||||
{
|
||||
return recovery_core::RecoveryOutputType::kVelocity;
|
||||
}
|
||||
|
||||
protected:
|
||||
// nh ĐÃ được caller scope vào namespace param của instance này — đọc khoá phẳng.
|
||||
bool onConfigure(robot::NodeHandle& nh) override
|
||||
{
|
||||
nh.param("limit", limit_, kDefaultLimit);
|
||||
if (!std::isfinite(limit_) || limit_ <= 0.0)
|
||||
{
|
||||
robot::log_warning("[recovery_core] '%s': limit=%.3f không hợp lệ; dùng %.3f.",
|
||||
name().c_str(), limit_, kDefaultLimit);
|
||||
limit_ = kDefaultLimit;
|
||||
}
|
||||
return true; // false = không chạy được; registry bỏ behavior này và log đích danh
|
||||
}
|
||||
|
||||
// Chốt mục tiêu lượt này + kiểm điều kiện an toàn để khởi động. KHÔNG sinh tick ở đây.
|
||||
bool onStart(const recovery_core::RecoveryGoal& goal) override
|
||||
{
|
||||
if (!ctx().pose->getRobotPose(start_pose_))
|
||||
return false; // không biết robot ở đâu -> từ chối khởi động
|
||||
|
||||
target_ = goal.distance.value_or(limit_);
|
||||
return true;
|
||||
}
|
||||
|
||||
// dt là thời gian THẬT tính từ tick trước; tick đầu ngay sau start() có dt = 0.
|
||||
recovery_core::RecoveryResult onUpdate(const robot::Time& now, double dt) override
|
||||
{
|
||||
robot_geometry_msgs::PoseStamped pose;
|
||||
if (!ctx().pose->getRobotPose(pose))
|
||||
return stopResult(recovery_core::RecoveryStatus::kFailed).withMessage("mất pose robot");
|
||||
|
||||
const double done = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
|
||||
if (done >= target_)
|
||||
return stopResult(recovery_core::RecoveryStatus::kSucceeded).withProgress(1.0, 0.0);
|
||||
|
||||
robot_geometry_msgs::Twist cmd;
|
||||
// ... tính cmd, nhớ ramp theo acc_lim và kiểm collision ở pose dự đoán ...
|
||||
return recovery_core::RecoveryResult::Velocity(cmd, recovery_core::RecoveryStatus::kRunning)
|
||||
.withProgress(done / target_, target_ - done);
|
||||
}
|
||||
|
||||
// Tuỳ chọn: cơ hội giảm tốc thay vì nhảy thẳng về 0.
|
||||
recovery_core::RecoveryResult onCancel() override
|
||||
{
|
||||
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("cancelled");
|
||||
}
|
||||
|
||||
private:
|
||||
double limit_ = kDefaultLimit; ///< [m]
|
||||
double target_ = kDefaultLimit; ///< [m]
|
||||
double start_yaw_ = 0.0; ///< [rad]
|
||||
robot_geometry_msgs::PoseStamped start_pose_;
|
||||
};
|
||||
|
||||
} // namespace recovery_plugins
|
||||
|
||||
// alias = `type` dùng trong YAML recovery_behaviors.
|
||||
BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)
|
||||
BOOST_DLL_ALIAS(recovery_plugins::MyRecovery::create, MyRecovery)
|
||||
```
|
||||
|
||||
## Nạp phía loader (adapter/caller — không nằm trong recovery_core)
|
||||
Những gì **không** phải viết: guard `configured_`/`started_`, kiểm cancel, đo `elapsed`, ép
|
||||
`timeout`, đặt `output_type` cho stop output, kiểm NaN của lệnh vận tốc. Base làm hết — và giữ được
|
||||
vì toàn bộ state của nó là `private`.
|
||||
|
||||
```cpp
|
||||
#include <boost/dll/import.hpp>
|
||||
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(
|
||||
path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations);
|
||||
recovery_core::RecoveryBehavior::RecoveryBehaviorPtr behavior = loader();
|
||||
## 3. Đăng ký build
|
||||
|
||||
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(); // publish r.command; đọc r.progress/r.remaining/r.message
|
||||
}
|
||||
```cmake
|
||||
add_recovery_core_plugin(
|
||||
recovery_core_my_recovery
|
||||
plugins/my_recovery.cpp
|
||||
)
|
||||
```
|
||||
|
||||
Lưu ý: adapter/test phải giữ handle `.so` sống lâu hơn object plugin. Nếu library bị unload trong
|
||||
khi object plugin còn tồn tại, virtual call qua vtable của plugin có thể crash.
|
||||
Thêm tên target vào `catkin_package(LIBRARIES ...)` để consumer khác dùng lại được.
|
||||
|
||||
## CMake cho plugin
|
||||
## 4. Khai trong YAML
|
||||
|
||||
- `find_package(Boost REQUIRED COMPONENTS system filesystem)`
|
||||
- link `${Boost_LIBRARIES}`, `${CMAKE_DL_LIBS}`, `recovery_core`
|
||||
- `set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON)`
|
||||
- build shared library, tên library + symbol khớp `type`; install `.so` nơi loader tìm.
|
||||
```yaml
|
||||
recovery:
|
||||
behaviors:
|
||||
- {name: my_instance, type: MyRecovery} # thứ tự trong danh sách CHÍNH LÀ thứ tự thử
|
||||
my_instance:
|
||||
limit: 0.5 # [m]
|
||||
timeout: 10.0 # [s] base đọc; 0 = không giới hạn
|
||||
|
||||
## Test plugin
|
||||
|
||||
```bash
|
||||
catkin_make --pkg recovery_core
|
||||
./devel/lib/recovery_core/recovery_core_plugin_loader_test
|
||||
MyRecovery:
|
||||
library_path: librecovery_core_my_recovery # THIẾU KHOÁ NÀY LÀ LỖI PHỔ BIẾN NHẤT
|
||||
```
|
||||
|
||||
Standalone:
|
||||
Tên alias trong `BOOST_DLL_ALIAS` phải khớp `type`. Namespace param của instance là `<ns>/<name>` —
|
||||
registry dựng `NodeHandle` đó và truyền vào `configure()`; plugin **không** tự đi tìm config trên
|
||||
disk. Đó là lý do test chỉ cần trỏ vào cây config của mình là chạy được.
|
||||
|
||||
```bash
|
||||
cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase3_build
|
||||
make -C /tmp/recovery_core_phase3_build -j4
|
||||
/tmp/recovery_core_phase3_build/test/recovery_core_plugin_loader_test
|
||||
```
|
||||
Một plugin có thể có **nhiều instance** với tham số khác nhau — bộ mặc định dùng
|
||||
`ClearCostmapRecovery` hai lần (`conservative_reset` và `aggressive_reset`).
|
||||
|
||||
## 5. Test
|
||||
|
||||
Đặt trong `test/`, thêm tên vào danh sách `RECOVERY_CORE_TESTS` của `CMakeLists.txt` (dùng
|
||||
`catkin_add_gtest`, nên `ctest -R recovery_core` bắt được).
|
||||
|
||||
Dùng `recovery_test::VelocityRig` (costmap giả + pose giả + collision checker giả + đồng hồ giả) và
|
||||
nạp plugin **qua `RecoveryRegistry`** để đi đúng đường Boost.DLL mà runtime dùng — không link thẳng
|
||||
`.so` vào test.
|
||||
|
||||
Tối thiểu phải phủ:
|
||||
|
||||
- từ chối khởi động khi điều kiện an toàn không thoả;
|
||||
- mất pose giữa chừng → `kFailed` + lệnh dừng;
|
||||
- loop chạy chậm (dt gấp 5–20 lần nhịp thường) vẫn dừng đúng chỗ;
|
||||
- cancel → lệnh dừng;
|
||||
- param ngoài dải → dùng default, không nhận giá trị sai.
|
||||
|
||||
Cuối cùng, kiểm rằng test **fail được**: gỡ `.so` khỏi `devel/lib` rồi chạy lại. Phải đỏ. Bộ test cũ
|
||||
của gói này in `[PASS]` khi không nạp được plugin nào — đó là thứ phải tránh.
|
||||
|
||||
Reference in New Issue
Block a user