DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
e4f2823b176acbae3b84451c969a4e5c8462c032
recovery_core/PLAN.md
duongtd e4f2823b17 first commit
2026-07-06 23:33:56 +07:00

22 KiB
Raw Blame History

Kế Hoạch Xây Dựng Package recovery_core (Interface Thuần, Không Chạy ROS)

recovery_core là package ĐỊNH NGHĨA INTERFACE cho các hành vi recovery — đóng đúng vai trò như robot_nav_core cung cấp RecoveryBehavior, nhưng không chạy ROS và biểu đạt hợp đồng qua robot_geometry_msgs / robot_nav_msgs (header-only).

Bản thân package không chứa hành vi cụ thể (backup, spin, clear costmap, regen path). Các hành vi đó là plugin implement interface — làm ở Phase 3 hoặc ở package khác.

1. Bối Cảnh & Quyết Định Thiết Kế

1.1. recovery_core là gì

  • Là thư viện interface (giống robot_nav_core::RecoveryBehavior), nhưng ROS-free:
    • Không include robot/robot.h, không NodeHandle, không Costmap2DROBOT, không tf3::BufferCore.
    • Chỉ phụ thuộc kiểu dữ liệu header-only: robot_geometry_msgs (Twist, Pose2D, PoseStamped) và robot_nav_msgs (Path). Các kiểu này không kéo runtime ROS.
  • Cung cấp một base class thuần ảo đủ tổng quát để bao 3 họ recovery (mục 1.2), cùng các kiểu phụ trợ (status, result, config, context).

So với robot_nav_core::RecoveryBehavior (blocking runBehavior(), cần costmap + tf): recovery_core giữ tinh thần interface nhưng thay hạ tầng ROS bằng abstraction ROS-free và mở rộng để hành vi có thể trả về output (không chỉ chạy rồi thôi).

1.2. Ba họ recovery interface phải bao được

Họ Ví dụ Output đặc trưng
A. Trả v<><76> path tạo lại một đoạn đường thoát/né robot_nav_msgs::Path
B. Không output clear costmap, reset state chỉ trạng thái (SUCCEEDED/FAILED)
C. Trả về vận tốc rotation tại chỗ, backup (không ROS) robot_geometry_msgs::Twist theo cycle

Interface phải diễn đạt cả 3 mà không ép một hành vi phải điền output nó không dùng. → Dùng một RecoveryResult hợp nhất mang cờ loại output + các trường optional (status luôn có; twist, path chỉ hợp lệ khi cờ tương ứng bật).

1.3. Ranh giới trách nhiệm

recovery_core CHỈ cung cấp:

  • Base class RecoveryBehavior (thuần ảo) + vòng đời chuẩn.
  • Kiểu hợp đồng: RecoveryStatus, RecoveryOutputType, RecoveryResult, RecoveryConfig, RecoveryContext (abstraction ROS-free thay cho tf/costmap/publisher).
  • Không thuật toán hành vi cụ thể, không collision-check, không I/O, không vòng lặp thời gian.

Caller / plugin chịu trách nhiệm: cấp pose (qua RecoveryContext), tiêu thụ output (Twist → cmd_vel, Path → planner), đảm bảo an toàn.

1.4. Vị trí & chuẩn

  • Đặt tại .../Navigations/Libraries/recovery_core (cùng cấp robot_clear_costmap_recovery).
  • C++17, namespace recovery_core, guard RECOVERY_CORE_<FILE>_H_.
  • Identifiers/comments tiếng Anh; tài liệu .md tiếng Việt.
  • Build catkin + standalone (theo pattern robot_clear_costmap_recovery).
  • Phần lớn là header-only (interface) — .cpp chỉ cho helper/validate không inline.

2. Thiết Kế Interface (bản chốt ở Phase 2, nháp ở đây)

2.1. Kiểu hợp đồng

namespace recovery_core {

// Trạng thái tiến trình 1 lượt recovery.
enum class RecoveryStatus { kIdle, kRunning, kSucceeded, kFailed };

// Loại output mà hành vi sinh ra ở cycle hiện tại.
enum class RecoveryOutputType { kNone, kVelocity, kPath };

// Kết quả hợp nhất cho cả 3 họ. Chỉ đọc trường khớp với output_type.
struct RecoveryResult {
  RecoveryStatus status = RecoveryStatus::kRunning;
  RecoveryOutputType output_type = RecoveryOutputType::kNone;

  robot_geometry_msgs::Twist command;   // hợp lệ khi output_type == kVelocity
  robot_nav_msgs::Path   path;          // hợp lệ khi output_type == kPath

  // Tiện ích khởi tạo nhanh (định nghĩa trong .cpp hoặc inline):
  static RecoveryResult Running();
  static RecoveryResult Succeeded();
  static RecoveryResult Failed();
  static RecoveryResult Velocity(const robot_geometry_msgs::Twist&, RecoveryStatus);
  static RecoveryResult PathOut(const robot_nav_msgs::Path&, RecoveryStatus);
};

2.2. Context ROS-free (thay tf/costmap/publisher)

RecoveryContext là abstraction để plugin lấy trạng thái thế giới mà không biết ROS. recovery_core chỉ khai báo interface; caller cấp implementation cụ thể (đọc từ đâu là việc của caller).

// Interface tối thiểu; mở rộng dần khi có nhu cầu thực.
class RecoveryContext {
 public:
  virtual ~RecoveryContext() = default;
  // Pose robot hiện tại trong frame quy ước (rad, m). Trả false nếu không có.
  virtual bool getRobotPose(robot_geometry_msgs::Pose2D* out) const = 0;
  // (tuỳ chọn, cho họ cần) truy vấn cost tại điểm; mặc định không hỗ trợ.
  // virtual bool getCost(double x, double y, unsigned char* cost) const { return false; }
};

Ghi chú: interface base không ép hành vi phải dùng context (họ C rotation thuần có thể chỉ cần pose truyền vào computeCommand). Context là kênh mở rộng cho họ A/B cần hỏi thế giới.

2.3. Base class interface

class RecoveryBehavior {
 public:
  // shared_ptr để KHỚP cơ chế nạp Boost.DLL của workspace:
  //   boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(...)
  // (xem move_base.cpp loadRecoveryBehaviors + robot_nav_core::RecoveryBehavior::Ptr).
  using Ptr = std::shared_ptr<RecoveryBehavior>;
  virtual ~RecoveryBehavior() = default;

  // 1) Nạp + validate config. Không throw; trả false + set error nếu sai.
  virtual bool configure(const RecoveryConfig& config, std::string* error) = 0;

  // 2) Bắt đầu 1 lượt: chốt state khởi đầu, reset progress về kRunning.
  virtual void start(const robot_geometry_msgs::Pose2D& current_pose) = 0;

  // 3a) Kiểu PER-CYCLE (họ C, và họ A nếu sinh path dần): gọi mỗi control cycle.
  //     dt (s) > 0. Guard chưa configure/start -> Failed + output kNone.
  virtual RecoveryResult computeCommand(const robot_geometry_msgs::Pose2D& current_pose,
                                        double dt) = 0;

  // 3b) Kiểu ONE-SHOT/BLOCKING (họ B clear costmap, họ A regen path 1 lần):
  //     chạy trọn hành vi qua context, trả kết quả cuối. Mặc định: lặp computeCommand.
  virtual RecoveryResult runBehavior(RecoveryContext* ctx);   // có default impl

  virtual void reset() = 0;
  virtual RecoveryStatus status() const = 0;
  virtual const char* name() const = 0;

 protected:
  RecoveryBehavior() = default;
};

Vì sao có cả computeCommand lẫn runBehavior:

  • Họ C (rotation/backup) tự nhiên là per-cycle → override computeCommand.
  • Họ B (clear costmap) tự nhiên là one-shot → override runBehavior (dùng ctx), computeCommand chỉ trả trạng thái.
  • Họ A (regen path) có thể one-shot (runBehavior trả Path) hoặc per-cycle tuỳ plugin.
  • runBehavior có default implementation trong recovery_core (lặp computeCommand tới khi khác kRunning) để plugin per-cycle không phải viết lại; plugin one-shot thì override.

2.4. Config

struct RecoveryConfig {
  std::string type;                 // "back_up" | "spin" | "clear_costmap" | "regen_path" ...
  double control_frequency = 20.0;  // Hz, dùng cho default runBehavior loop (dt = 1/f)
  double timeout = 0.0;             // s, 0 = không timeout
  std::map<std::string, double> params;   // tham số riêng của từng plugin
  // validate(): control_frequency > 0, timeout >= 0, hữu hạn. Trả false + message.
  bool validate(std::string* error) const;
};

Package interface không biết tham số riêng của backup/spin — để plugin tự đọc từ params. recovery_core chỉ validate phần chung.

3. PHASE 1 — Dựng Khung Package (KHÔNG code triển khai)

Mục tiêu: tạo cây thư mục + file stub (header có guard/namespace/Doxygen, .cpp thân rỗng compile được), package.xml, CMakeLists.txt build ra lib rỗng. Package phải configure CMake thành công.

3.1. Cây thư mục mục tiêu

recovery_core/
├── PLAN.md                            # tài liệu này
├── README.md                          # phạm vi: INTERFACE, không ROS, 3 họ recovery
├── package.xml                        # depend: robot_geometry_msgs, robot_nav_msgs
├── CMakeLists.txt                     # catkin + standalone; lib chủ yếu header, .cpp cho helper
├── include/
│   └── recovery_core/
│       ├── recovery_types.h           # enum Status, OutputType; struct RecoveryResult
│       ├── recovery_config.h          # struct RecoveryConfig + validate()
│       ├── recovery_context.h         # interface RecoveryContext (ROS-free)
│       └── recovery_behavior.h        # base class RecoveryBehavior (interface chính)
├── src/
│   ├── recovery_types.cpp             # (stub) factory helper Running()/Failed()/...
│   ├── recovery_config.cpp            # (stub) validate()
│   └── recovery_behavior.cpp          # (stub) default runBehavior() loop
├── test/
│   ├── CMakeLists.txt                 # khai báo test target (tuỳ chọn)
│   ├── interface_contract_test.cpp    # (stub) dùng 1 MockBehavior kiểm vòng đời
│   └── mock_behavior.h                # (stub) implement tối thiểu để test interface
├── examples/
│   └── minimal_plugin.cpp             # (stub) ví dụ 1 plugin per-cycle bé xíu, không ROS
└── docs/
    ├── ARCHITECTURE.md                # sơ đồ interface + luồng runBehavior/computeCommand
    ├── PLUGIN_GUIDE.md                # hướng dẫn viết plugin cho từng họ A/B/C
    └── SAFETY.md                      # cảnh báo: không collision-check trong core

3.2. Quy ước nội dung stub (Phase 1)

  • Header: license/author ngắn, guard RECOVERY_CORE_*_H_, include tối thiểu (<robot_geometry_msgs/Twist.h>, <robot_geometry_msgs/Pose2D.h>, <robot_nav_msgs/Path.h> khi cần), khai báo đầy đủ chữ ký + Doxygen public API, thân hàm non-inline để trong .cpp.
  • Source: thân tối thiểu // TODO(phase-2): implement + return {} để compile.
  • package.xml: format 2, <depend>robot_geometry_msgs</depend>, <depend>robot_nav_msgs</depend>, buildtool catkin. Không depend costmap/tf/robot_cpp.
  • CMakeLists.txt: khung catkin+standalone của robot_clear_costmap_recovery nhưng chỉ giữ 2 msgs depend; tạo library recovery_core từ các .cpp (kể cả khi phần lớn header-only, vẫn build 1 lib nhỏ cho validate/helper). Install header + export target 2 chế độ.

3.3. Checklist Phase 1

  • Tạo đủ cây thư mục 3.1.
  • 4 header có guard, namespace recovery_core, Doxygen; khai báo khớp mục 2.
  • 3 .cpp compile với thân stub.
  • package.xml chỉ depend robot_geometry_msgs + robot_nav_msgs.
  • CMakeLists.txt configure OK cả catkin lẫn standalone; build ra librecovery_core rỗng.
  • README.md nêu rõ: đây là INTERFACE, không ROS, 3 họ recovery, không hành vi cụ thể.
  • Kiểm chứng: cmake + make thành công.

4. PHASE 2 — Triển Khai Interface (điền logic phần thuộc về core)

Core là interface nên "triển khai" ở đây = hoàn thiện phần chung mà base cung cấp, KHÔNG phải viết hành vi cụ thể. Cụ thể:

4.1. Nội dung triển khai

  1. recovery_types: định nghĩa các factory RecoveryResult::Running/Succeeded/Failed/ Velocity/PathOut, đảm bảo set đúng output_type + status. Bất biến: khi output_type==kNone thì command/path để mặc định (không rác).
  2. recovery_config::validate: kiểm control_frequency > 0, timeout >= 0, hữu hạn; type không rỗng. Trả false + message rõ. Không throw.
  3. recovery_behavior.cpp — default runBehavior(ctx):
    • Guard ctx == nullptr → Failed.
    • Lấy pose qua ctx->getRobotPose; nếu fail → Failed.
    • dt = 1 / control_frequency; lặp computeCommand(pose, dt) tới khi status khác kRunning hoặc vượt timeout (nếu > 0) → khi timeout trả Failed.
    • Mỗi vòng cập nhật pose qua ctx (mô phỏng caller thật). Trả result cuối.
    • Không cấp phát trong vòng lặp; không log spam (chỉ log khi đổi status).
  4. Bất biến & guard chung (document + test):
    • Chưa configure thành công → mọi call trả Failed.
    • dt <= 0 hoặc pose NaN/Inf → Failed, output kNone.

4.2. Kiểm thử interface (không cần plugin thật)

  • test/mock_behavior.h: MockBehavior per-cycle đơn giản (đếm N cycle rồi kSucceeded, trả kVelocity) + MockContext cấp pose cố định.
  • interface_contract_test.cpp:
    • configure sai → false + message.
    • start → status()==kRunning.
    • computeCommand tiến trình đúng, đạt đích → kSucceeded.
    • default runBehavior chạy hết vòng đời qua MockContext, tôn trọng timeout.
    • guard: chưa configure, dt<=0, ctx null, pose NaN.

4.3. Checklist Phase 2

  • RecoveryResult factories set đúng cờ; có test bất biến output.
  • RecoveryConfig::validate từ chối mọi input sai với message rõ.
  • default runBehavior đúng: loop, timeout, guard ctx/pose; không alloc trong loop.
  • MockBehavior/MockContext + contract test pass.
  • docs/PLUGIN_GUIDE.md mô tả cách 1 plugin họ A/B/C override method nào.
  • Kiểm chứng: build + test pass.

5. PHASE 3 — Tạo Recovery Plugin (implement interface)

Mục tiêu: viết các hành vi cụ thể implement recovery_core::RecoveryBehavior, mỗi họ ít nhất một ví dụ, + cơ chế nạp theo string.

5.1. Plugin mẫu cho từng họ

  • Họ C — SpinRecovery / BackUpRecovery (per-cycle, ROS-free):
    • Override computeCommand: sinh Twist theo pose + dt (xoay tới góc / lùi tới khoảng cách), đạt đích → kSucceeded. Đọc tham số từ RecoveryConfig::params (spin_target_angle, spin_speed, backup_distance, backup_speed...).
    • Dùng default runBehavior (không cần override).
  • Họ B — ClearCostmapRecovery-like (one-shot, cần context):
    • Override runBehavior(ctx): gọi hành động clear qua một context mở rộng (RecoveryContext thêm hook clear — hoặc để ở adapter). Trả kSucceeded/kFailed, output kNone. computeCommand chỉ trả trạng thái.
    • Lưu ý: clear costmap thật cần costmap → phần đó nằm ở adapter/caller, không trong recovery_core. Plugin ở đây minh hoạ contract, thao tác thật uỷ thác qua context.
  • Họ A — RegenPathRecovery (trả path):
    • Override để trả RecoveryResult với output_type == kPath, điền robot_nav_msgs::Path (một đoạn đường thoát hình học đơn giản, ví dụ cung lùi). one-shot hoặc per-cycle.

Các plugin này có thể đặt trong recovery_core/plugins/ hoặc package riêng — cần chốt (mục 6). Dù đặt đâu, chúng chỉ được include header của recovery_core, không thêm dependency ROS vào package interface.

5.2. Cơ chế nạp — Boost.DLL (chốt, theo đúng convention workspace)

Workspace nạp planner/recovery plugin bằng Boost.DLL import_alias, KHÔNG dùng pluginlib/ class_loader. Bằng chứng trong repo:

  • Plugin export: robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp:37,330-337.
  • Loader import: move_base/src/move_base.cpp:22, 2016-2026 (loadRecoveryBehaviors).

recovery_core (interface) không include Boost.DLL. Chỉ plugin export symbol và loader/adapter import. Interface chỉ cần Ptr = std::shared_ptr<RecoveryBehavior> để khớp chữ ký import_alias<recovery_core::RecoveryBehavior::Ptr()>.

5.2.1. Phía PLUGIN — export factory bằng BOOST_DLL_ALIAS

Mỗi plugin cung cấp một factory không tham số trả Ptr (shared_ptr), rồi alias ra tên symbol dùng làm type trong YAML:

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

namespace recovery_plugins {

class SpinRecovery : public recovery_core::RecoveryBehavior { /* override ... */
 public:
  static recovery_core::RecoveryBehavior::Ptr create() {   // factory không tham số
    return std::make_shared<SpinRecovery>();
  }
};

}  // namespace recovery_plugins

// Tên alias thứ 2 ("spin_recovery") chính là `type` trong YAML recovery_behaviors.
BOOST_DLL_ALIAS(recovery_plugins::SpinRecovery::create, spin_recovery)

Quy ước (giống ClearCostmapRecovery → alias ClearCostmapRecovery):

  • Factory là static Ptr create() — không tham số (Boost.DLL alias yêu cầu signature Ptr()), cấu hình đi qua configure() sau khi tạo, không qua constructor.
  • Trả std::shared_ptr (KHÔNG unique_ptr) để khớp loader.
  • Tên alias = tên type mà YAML/loader sẽ dùng để tìm library + symbol.

5.2.2. Phía LOADER — import_alias (mẫu, thường nằm ở adapter/caller)

#include <boost/dll/import.hpp>
// path .so lấy qua PluginLoaderHelper::findLibraryPath(type) như move_base đang làm.
auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
    path_so, /*symbol=*/type, boost::dll::load_mode::append_decorations);
recovery_core::RecoveryBehavior::Ptr behavior = loader();   // tạo instance
std::string err;
behavior->configure(config, &err);                          // rồi mới cấu hình

recovery_core không viết loader này (nó thuộc caller/move_base/adapter). Nhưng docs/PLUGIN_GUIDE.md sẽ ghi mẫu để người dùng plugin biết cách nạp.

5.2.3. CMake cho PLUGIN (không phải cho core)

  • find_package(Boost REQUIRED COMPONENTS system filesystem) (Boost.DLL cần filesystem; system theo pattern package cũ). Link ${Boost_LIBRARIES} và ${CMAKE_DL_LIBS} (dl).
  • set_target_properties(<plugin> PROPERTIES POSITION_INDEPENDENT_CODE ON) — bắt buộc cho .so.
  • Build mỗi plugin thành shared library riêng; tên library + symbol khớp type YAML.
  • Install .so vào nơi findLibraryPath tìm được (theo convention PluginLoaderHelper).
  • Không cần plugins.xml (workspace này dùng Boost.DLL thuần, không pluginlib).

5.3. Adapter ROS (tuỳ chọn, package RIÊNG)

Nếu cần chạy trong move_base thật: package adapter bọc plugin recovery_core, implement robot_nav_core::RecoveryBehavior (có costmap/tf), cấp RecoveryContext đọc TF/costmap, tiêu thụ Twist/Path. Giữ recovery_core sạch, không đổi.

5.4. Checklist Phase 3

  • Chốt: plugin đặt trong recovery_core/plugins/ hay package riêng (đề xuất: package riêng cho plugin cần costmap/tf; plugin thuần spin/backup có thể ở recovery_core/plugins/).
  • Mỗi họ A/B/C có ≥ 1 plugin mẫu implement interface, build ra shared library riêng.
  • Mỗi plugin có static Ptr create() + BOOST_DLL_ALIAS(...::create, <type>); tên alias = type YAML; trả std::shared_ptr.
  • CMake plugin: find_package(Boost COMPONENTS system filesystem), link ${CMAKE_DL_LIBS}, POSITION_INDEPENDENT_CODE ON, install .so đúng nơi loader tìm.
  • Test end-to-end (không ROS): import_alias<recovery_core::RecoveryBehavior::Ptr()> nạp .so mẫu → configure → runBehavior/computeCommand với MockContext → kiểm output đúng họ (Twist / none / Path).
  • Cập nhật docs/ARCHITECTURE.md, docs/PLUGIN_GUIDE.md (kèm mẫu export + import).
  • Kiểm chứng: build plugin .so + test nạp Boost.DLL pass.

6. Rủi Ro & Điểm Cần Xác Nhận

  • Kiểu output hợp nhất vs đa interface: đề xuất một RecoveryResult mang cờ output_type (đơn giản, 1 base class). Phương án khác: template/đa base theo họ — phức tạp hơn. Cần chốt.
  • RecoveryContext rộng tới đâu: tối thiểu chỉ getRobotPose. Họ B (clear costmap) và họ A (regen path cần costmap) sẽ cần hook thêm — nên để ở context mở rộng của adapter, giữ context lõi nhỏ. Cần chốt mức tối thiểu.
  • Vị trí plugin: trong recovery_core/plugins/ hay package riêng. Đề xuất: package riêng cho plugin cần costmap/tf; plugin thuần (spin/backup) có thể ở recovery_core/plugins/.
  • Cơ chế nạp = Boost.DLL (đã chốt): theo đúng convention workspace (import_alias, không pluginlib). Ràng buộc kéo theo: Ptr = std::shared_ptr; factory static Ptr create() không tham số + BOOST_DLL_ALIAS; cấu hình qua configure() sau khi tạo. Core interface KHÔNG phụ thuộc Boost.DLL — chỉ plugin và loader.
  • robot_nav_msgs build dep: xác nhận có CMake config/target để find_package (catkin) hoặc include-only (standalone). Là header msgs nên nhiều khả năng include-only.
  • An toàn: core không collision-check → ghi rõ docs/SAFETY.md; caller/adapter chịu trách nhiệm an toàn khi thực thi backup/spin.

7. Tóm Tắt 3 Phase

Phase Kết quả Kiểm chứng
1 Khung package interface: 4 header + stub, package.xml, CMake build lib rỗng cmake + make OK
2 Hoàn thiện phần chung của interface: types/config/validate + default runBehavior, contract test qua Mock build + test pass
3 Plugin mẫu cho 3 họ (Twist/none/Path), export BOOST_DLL_ALIAS, nạp qua import_alias, doc build .so + test nạp Boost.DLL pass
Reference in New Issue View Git Blame Copy Permalink