Viết một recovery behavior mới

1. Chọn họ output

Quyết định đầu tiên và không đổi được về sau: outputKind().

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`

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.

2. Khung plugin

#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_math.h>

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

  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

BOOST_DLL_ALIAS(recovery_plugins::MyRecovery::create, MyRecovery)

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.

3. Đăng ký build

add_recovery_core_plugin(
  recovery_core_my_recovery
  plugins/my_recovery.cpp
)

Thêm tên target vào catkin_package(LIBRARIES ...) để consumer khác dùng lại được.

4. Khai trong 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

MyRecovery:
  library_path: librecovery_core_my_recovery  # THIẾU KHOÁ NÀY LÀ LỖI PHỔ BIẾN NHẤT

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.

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.

6.0 KiB Raw Blame History Unescape Escape