recovery_core/PLAN.md

PLAN - recovery_core

recovery_core là package interface cho recovery behavior trong navigation stack ROS-like T800. Package này không chứa thuật toán recovery cụ thể; nó định nghĩa contract chung để các plugin recovery có thể trả trạng thái, velocity command hoặc path.

1. Mục Tiêu

• Cung cấp base class recovery_core::RecoveryBehavior cho các hành vi recovery.
• Dựa trên pattern robot_nav_core::RecoveryBehavior, nhưng mở rộng output để bao được 3 nhóm:
  • recovery không output, ví dụ clear/reset costmap;
  • recovery sinh vận tốc theo từng cycle, ví dụ rotate/backup;
  • recovery sinh path, ví dụ regen path hoặc detour path.
• Dùng stack ROS-like trong workspace (robot_*, tf3, robot_costmap_2d), không phụ thuộc roscpp hoặc ROS master thật.
• Giữ core sạch: plugin trả RecoveryResult; caller/adapter quyết định publish command, thay path, clear service hoặc request replan.

2. Ranh Giới Thiết Kế

2.1. recovery_core chịu trách nhiệm

• Định nghĩa interface RecoveryBehavior.
• Định nghĩa result contract:
  • RecoveryStatus
  • RecoveryOutputType
  • RecoveryResult
• 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

• Không implement recovery cụ thể như clear costmap, rotate, backup, regen path.
• Không publish cmd_vel.
• Không gọi ROS service hoặc action.
• Không tự collision-check velocity/path output.
• Không trực tiếp nạp plugin bằng Boost.DLL trong core interface.
• Không thay thế trực tiếp robot_nav_core::RecoveryBehavior trong move_base nếu chưa có adapter riêng.

3. Interface Contract Đã Chốt

3.1. Recovery status

enum class RecoveryStatus
{
  kIdle,
  kRunning,
  kSucceeded,
  kFailed
};

Ý nghĩa:

• kIdle: đã tạo/initialize nhưng chưa chạy.
• kRunning: behavior cần được gọi tiếp.
• kSucceeded: behavior hoàn thành.
• kFailed: behavior lỗi hoặc không thể tiếp tục an toàn.

3.2. Output type

enum class RecoveryOutputType
{
  kNone,
  kVelocity,
  kPath
};

Quy ước:

• kNone: chỉ đọc status.
• kVelocity: chỉ đọc command.
• kPath: chỉ đọc path.

3.3. Recovery result

struct RecoveryResult
{
  RecoveryStatus status = RecoveryStatus::kRunning;
  RecoveryOutputType output_type = RecoveryOutputType::kNone;

  robot_geometry_msgs::Twist command;
  robot_nav_msgs::Path path;

  static RecoveryResult Running();
  static RecoveryResult Succeeded();
  static RecoveryResult Failed();
  static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
                                 RecoveryStatus status);
  static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
                                RecoveryStatus status);
};

Factory phải giữ bất biến:

• Running/Succeeded/Failed dùng output_type = kNone.
• Velocity(...) dùng output_type = kVelocity.
• PathOut(...) dùng output_type = kPath.

3.4. Recovery behavior

class RecoveryBehavior
{
public:
  using Ptr = std::shared_ptr<RecoveryBehavior>;

  virtual ~RecoveryBehavior() = default;

  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;

  virtual RecoveryResult runBehavior() = 0;
  virtual RecoveryResult computeCommand(double dt);
  virtual RecoveryStatus status() const = 0;

protected:
  RecoveryBehavior() = default;
};

Điểm khác robot_nav_core::RecoveryBehavior:

• Có thêm global_path để behavior họ path có ngữ cảnh plan hiện tại.
• runBehavior() trả RecoveryResult thay vì void.
• Có computeCommand(dt) cho behavior per-cycle.

4. Ba Nhóm Recovery

Nhóm	Ví dụ	Method chính	Output
A. Path output	regen path, detour path	runBehavior()	RecoveryOutputType::kPath
B. No output	clear costmap, reset state	runBehavior()	RecoveryOutputType::kNone
C. Velocity output	rotate, backup	computeCommand(dt)	RecoveryOutputType::kVelocity

Caller/adapter chịu trách nhiệm tiêu thụ output:

• path output: thay local path hoặc request global/local replan;
• no output: tiếp tục navigation hoặc chuyển behavior kế tiếp;
• velocity output: publish command ngoài core, có safety gate trước khi gửi robot.

5. Package Layout

recovery_core/
├── CMakeLists.txt
├── package.xml
├── README.md
├── PLAN.md
├── include/recovery_core/
│   ├── recovery_behavior.h
│   └── recovery_types.h
├── src/
│   ├── recovery_behavior.cpp
│   └── recovery_types.cpp
├── plugins/
│   ├── clear_costmap_recovery.cpp
│   ├── rotate_recovery.cpp
│   ├── back_up_recovery.cpp
│   └── regen_path_recovery.cpp
├── test/
│   ├── CMakeLists.txt
│   └── plugin_loader_contract_test.cpp
└── docs/
    ├── ARCHITECTURE.md
    ├── PLUGIN_GUIDE.md
    └── SAFETY.md

6. Dependencies

Runtime/build dependencies:

• robot_costmap_2d
• robot_cpp
• robot_time
• robot_geometry_msgs
• robot_nav_msgs
• tf3
• Boost system thread

Không phụ thuộc:

• robot_nav_core
• roscpp
• pluginlib

7. Roadmap

Phase 1 - Package Skeleton

Trạng thái: done.

Mục tiêu:

• Tạo package skeleton.
• Tạo 3 header interface.
• Tạo 3 source stub compile được.
• Tạo README/docs/example/test stub.
• CMake hỗ trợ catkin và standalone.
• package.xml đúng dependency, không kéo robot_nav_core.

Acceptance:

• catkin_make --pkg recovery_core pass.
• Standalone cmake + make pass.
• Smoke test chạy được.
• package.xml parse được bằng catkin_pkg.
• README.md mô tả rõ đây là interface package, không phải behavior implementation.

Kết quả hiện tại:

• Catkin output: devel/lib/librecovery_core.so.
• Catkin test binary: devel/lib/recovery_core/recovery_core_interface_test.
• Standalone output: /tmp/recovery_core_phase1_build/librecovery_core.a.
• Standalone test binary: /tmp/recovery_core_phase1_build/test/recovery_core_interface_test.

Phase 2 - Core Contract Implementation

Trạng thái: done.

Mục tiêu:

• Hoàn thiện phần logic chung của interface, chưa viết recovery cụ thể.

Work items:

1. Implement RecoveryResult factories.
2. Implement RecoveryConfig::validate.
3. Implement RecoveryConfig::fromNodeHandle.
4. Giữ default RecoveryBehavior::computeCommand(dt) trả Failed().
5. Deferred: helper chạy loop cho behavior velocity chỉ thêm khi Phase 4 integration cần:
   • dùng control_frequency;
   • tôn trọng timeout;
   • không cấp phát/log trong loop.
6. Hoàn thiện MockBehavior.
7. Nâng interface_contract_test.cpp từ smoke test thành assertion test.
8. Cập nhật docs/ARCHITECTURE.md và docs/SAFETY.md theo contract thật.

Acceptance:

• RecoveryResult::Running() trả status = kRunning, output_type = kNone.
• RecoveryResult::Succeeded() trả status = kSucceeded, output_type = kNone.
• RecoveryResult::Failed() trả status = kFailed, output_type = kNone.
• Velocity(command, status) giữ command, set output_type = kVelocity.
• PathOut(path, status) giữ path, set output_type = kPath.
• Config reject NaN, inf, control_frequency <= 0, timeout < 0.
• Default computeCommand(dt) không sinh velocity mù.
• Test cover factory, config validation, default per-cycle behavior, mock lifecycle.

Verify commands:

xmllint --noout src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml
python3 -c "from catkin_pkg.package import parse_package; parse_package('src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml')"
catkin_make --pkg recovery_core
./devel/lib/recovery_core/recovery_core_interface_test
cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase2_build
make -C /tmp/recovery_core_phase2_build -j4
/tmp/recovery_core_phase2_build/test/recovery_core_interface_test

Phase 3 - Plugin Implementations

Trạng thái: done.

Mục tiêu:

• Viết plugin recovery thật implement recovery_core::RecoveryBehavior.
• Export bằng Boost.DLL đúng convention workspace.
• Test nạp plugin end-to-end.

Plugin đề xuất:

1. ClearCostmapRecovery
   • nhóm B, one-shot, no output;
   • tham khảo logic robot_clear_costmap_recovery;
   • trả RecoveryResult::Succeeded() hoặc Failed().
2. RotateRecovery
   • nhóm C, per-cycle velocity;
   • đọc target_angle, angular_speed, timeout;
   • dùng tích phân theo dt trong plugin mẫu; adapter production có thể thay bằng pose/tf;
   • trả zero command khi kết thúc hoặc fail.
3. BackUpRecovery
   • nhóm C, per-cycle velocity;
   • đọc backup_distance, linear_speed, timeout;
   • có require_costmap để fail nếu thiếu local costmap trước khi trả backward velocity.
4. RegenPathRecovery hoặc DetourPathRecovery
   • nhóm A, path output;
   • trả robot_nav_msgs::Path;
   • caller/adapter quyết định thay plan hay request replan.

Boost.DLL convention:

class RotateRecovery : public recovery_core::RecoveryBehavior
{
public:
  static recovery_core::RecoveryBehavior::Ptr create()
  {
    return std::make_shared<RotateRecovery>();
  }
};

BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, rotate_recovery)

Loader side:

auto loader = boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(
    path_so, type, boost::dll::load_mode::append_decorations);

recovery_core::RecoveryBehavior::Ptr behavior = loader();
behavior->initialize(name, tf, global_path, global_costmap, local_costmap);

Acceptance:

• Mỗi plugin build ra .so riêng.
• Mỗi plugin export factory không tham số, trả RecoveryBehavior::Ptr.
• Test nạp .so bằng boost::dll::import_alias.
• Test gọi initialize, runBehavior hoặc computeCommand.
• Output đúng nhóm recovery.
• Không plugin nào publish trực tiếp trong core logic.

Verify commands:

catkin_make --pkg recovery_core
./devel/lib/recovery_core/recovery_core_interface_test
./devel/lib/recovery_core/recovery_core_plugin_loader_test
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_interface_test
/tmp/recovery_core_phase3_build/test/recovery_core_plugin_loader_test

Phase 4 - Adapter / Integration

Trạng thái: pending.

Mục tiêu:

• Tích hợp recovery_core vào caller thật mà không làm core phụ thuộc ROS publish/service.

Phương án:

• Viết adapter riêng nếu cần tương thích robot_nav_core::RecoveryBehavior.
• Adapter chịu trách nhiệm:
  • load plugin;
  • gọi initialize;
  • gọi runBehavior hoặc loop computeCommand;
  • publish velocity nếu output là kVelocity;
  • thay path hoặc request replan nếu output là kPath;
  • áp safety stop nếu output failed/timeout.

Acceptance:

• Core vẫn không publish.
• Plugin vẫn chỉ trả RecoveryResult.
• Adapter có safety gate trước velocity command.
• Timeout và failure path luôn trả stop command hoặc abort rõ ràng.

8. Safety Requirements

• Behavior velocity phải trả stop command khi không chắc an toàn.
• Không publish command từ core/plugin nếu chưa qua adapter safety gate.
• dt <= 0, NaN, inf, missing tf/costmap/plan phải fail rõ ràng.
• Không log spam trong control loop.
• Không parse YAML hoặc cấp phát lớn trong mỗi cycle.
• Đơn vị phải rõ:
  • distance: meter;
  • angle: radian;
  • time: second;
  • linear velocity: m/s;
  • angular velocity: rad/s.

9. Definition Of Done

Package DoD

• package.xml hợp lệ.
• Catkin build pass.
• Standalone CMake build pass.
• Header install/export đúng.
• Test binary chạy được.
• README/docs mô tả đúng scope.

Interface DoD

• Contract status/output rõ ràng.
• Factory result đúng bất biến.
• Config validate đầy đủ.
• Default behavior fail an toàn.
• Test cover lifecycle và output invariant.

Plugin DoD

• Plugin không publish trực tiếp.
• Plugin không sở hữu raw pointer tf/costmap/global_path.
• Plugin guard initialized/null/invalid input.
• Plugin export Boost.DLL alias đúng.
• Loader test nạp được .so.

Integration DoD

• Adapter là nơi duy nhất có side effect publish/service/path replacement.
• Safety stop rõ ràng khi failed/timeout.
• Có log đủ ngữ cảnh, không spam loop.

10. Verification Baseline

Phase 1 đã được kiểm chứng bằng các lệnh:

xmllint --noout src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml
python3 -c "from catkin_pkg.package import parse_package; p=parse_package('src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core/package.xml'); print(p.name, p.version)"
catkin_make --pkg recovery_core
./devel/lib/recovery_core/recovery_core_interface_test
cmake -S src/AMR_T800/pnkx_nav_core/src/Navigations/Libraries/recovery_core -B /tmp/recovery_core_phase1_build
make -C /tmp/recovery_core_phase1_build -j4
/tmp/recovery_core_phase1_build/test/recovery_core_interface_test

Kỳ vọng chính:

• catkin_make --pkg recovery_core tạo devel/lib/librecovery_core.so.
• Standalone make tạo /tmp/recovery_core_phase1_build/librecovery_core.a.
• Contract test in interface contract test OK.

11. Open Decisions

• Plugin mẫu hiện nằm dưới recovery_core/plugins/; package riêng chỉ cần nếu muốn tách deploy.
• Có cần adapter tương thích robot_nav_core::RecoveryBehavior cho move_base hiện tại không.
• Có cần helper loop trong base class cho behavior velocity hay để caller tự quản loop.
• global_path nên là mutable pointer như hiện tại hay chuyển sang const std::vector<...>* nếu behavior không được phép sửa plan trực tiếp.