Hướng Dẫn Viết Plugin recovery_core

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.

Bước chung

Kế thừa recovery_core::RecoveryBehavior.
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().
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.
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	`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)

#include <recovery_core/recovery_behavior.h>
#include <boost/dll/alias.hpp>

namespace recovery_plugins {
class SpinRecovery : public recovery_core::RecoveryBehavior {
 public:
  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create() {
    return std::make_shared<SpinRecovery>();
  }
 protected:
  // override onConfigure()/onStart(goal)/onUpdate(dt)...
};
}  // namespace recovery_plugins

// alias = `type` dùng trong YAML recovery_behaviors.
BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)

Nạp phía loader (adapter/caller — không nằm trong recovery_core)

#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();

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 khi object plugin còn tồn tại, virtual call qua vtable của plugin có thể crash.

CMake cho plugin

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.

Test plugin

catkin_make --pkg recovery_core
./devel/lib/recovery_core/recovery_core_plugin_loader_test

Standalone:

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

4.4 KiB Raw Blame History