temporary storage

include/recovery_core/recovery_behavior.h

@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — interface (base class) cho recovery behaviors.
+ *
+ * Mô phỏng robot_nav_core::RecoveryBehavior (giữ chữ ký initialize với tf + costmap),
+ * nhưng tổng quát hoá output để bao 3 họ recovery (path / none / velocity).
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_BEHAVIOR_H_
+#define RECOVERY_CORE_RECOVERY_BEHAVIOR_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <tf3/buffer_core.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+
+#include <recovery_core/recovery_types.h>
+
+namespace recovery_core
+{
+
+/**
+ * @class RecoveryBehavior
+ * @brief Interface cho mọi hành vi recovery không chạy roscpp/ROS master thật
+ *        (dùng lớp ROS-like robot_*).
+ *
+ * Ba họ hành vi và method chính tương ứng:
+ *  - Họ A (trả path)      : override runBehavior()   -> RecoveryResult::PathOut(...)
+ *  - Họ B (không output)  : override runBehavior()   -> RecoveryResult::Succeeded()/Failed()
+ *  - Họ C (trả vận tốc)   : override computeCommand() -> RecoveryResult::Velocity(...)
+ *
+ * Vòng đời: initialize() một lần -> runBehavior() (one-shot) hoặc lặp computeCommand()
+ * (per-cycle) -> status(). Guard initialized_/costmap trước khi thao tác.
+ */
+class RecoveryBehavior
+{
+public:
+  /// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace
+  /// (boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(...)).
+  using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>;
+
+  virtual ~RecoveryBehavior() = default;
+
+  /**
+   * @brief Khởi tạo — mở rộng chữ ký robot_nav_core::RecoveryBehavior::initialize (thêm
+   *        global_path). Chỉ chạy một lần; đọc param qua robot::NodeHandle("~/" + name);
+   *        cache tf/costmap/global_path (không sở hữu).
+   * @param name           Tên instance (dùng cho namespace param + log).
+   * @param tf             Transform buffer (không sở hữu).
+   * @param global_path    Đường đi toàn cục hiện tại (không sở hữu) — họ A regen path tham
+   *                       chiếu để tạo lại/né; có thể null nếu chưa có plan.
+   * @param global_costmap Costmap toàn cục (không sở hữu).
+   * @param local_costmap  Costmap cục bộ (không sở hữu).
+   */
+  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;
+
+  /**
+   * @brief Chạy hành vi kiểu ONE-SHOT (họ A regen path, họ B clear costmap).
+   *        Trả kết quả cuối kèm output (path hoặc none). Guard chưa initialize/costmap null
+   *        -> RecoveryResult::Failed().
+   */
+  virtual RecoveryResult runBehavior() = 0;
+
+  /**
+   * @brief Sinh command kiểu PER-CYCLE (họ C rotation/backup). Lấy pose robot từ costmap/tf
+   *        bên trong; caller lặp gọi mỗi control cycle và publish command.
+   * @param dt Khoảng thời gian control cycle (s), > 0.
+   * @return RecoveryResult (thường output_type == kVelocity).
+   *
+   * Mặc định: coi như không hỗ trợ per-cycle và trả Failed() — họ A/B không cần override.
+   */
+  virtual RecoveryResult computeCommand(double dt);
+
+  /**
+   * @brief Trạng thái hiện tại của lượt recovery.
+   */
+  virtual RecoveryStatus status() const = 0;
+
+  virtual std::string getNameRecoveryBehavior() const
+  {
+    return name_;
+  }
+
+protected:
+  RecoveryBehavior() = default;
+  std::string name_;
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_BEHAVIOR_H_

include/recovery_core/recovery_config.h

@@ -0,0 +1,53 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — config chung cho recovery behaviors.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_CONFIG_H_
+#define RECOVERY_CORE_RECOVERY_CONFIG_H_
+
+#include <string>
+
+// Forward declare để không kéo <robot/robot.h> vào header interface.
+namespace robot { class NodeHandle; }
+
+namespace recovery_core
+{
+
+/**
+ * @struct RecoveryConfig
+ * @brief Gói các tham số CHUNG cho vòng đời recovery.
+ *
+ * Tham số RIÊNG của từng hành vi (vd: spin_target_angle, backup_distance) do plugin tự đọc
+ * qua robot::NodeHandle trong initialize(); struct này chỉ giữ phần chung.
+ *
+ * Đơn vị:
+ *  - control_frequency : Hz  (tần số gọi computeCommand khi chạy per-cycle)
+ *  - timeout           : s   (0 = không giới hạn thời gian)
+ */
+struct RecoveryConfig
+{
+  double control_frequency = 20.0;  ///< Hz, > 0.
+  double timeout = 0.0;             ///< s, >= 0; 0 nghĩa là không timeout.
+
+  /**
+   * @brief Kiểm tra hợp lệ các tham số.
+   * @param error [out] Nếu != nullptr và invalid, ghi thông điệp lỗi.
+   * @return true nếu hợp lệ.
+   */
+  bool validate(std::string* error) const;
+
+  /**
+   * @brief Đọc config từ NodeHandle (có default, có validate + log cảnh báo nếu sai).
+   * @param nh NodeHandle đã trỏ tới namespace của behavior.
+   * @return RecoveryConfig đã điền (giá trị sai được thay bằng default).
+   */
+  static RecoveryConfig fromNodeHandle(robot::NodeHandle& nh);
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_CONFIG_H_

include/recovery_core/recovery_types.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — kiểu hợp đồng output cho recovery behaviors.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef RECOVERY_CORE_RECOVERY_TYPES_H_
+#define RECOVERY_CORE_RECOVERY_TYPES_H_
+
+#include <robot_geometry_msgs/Twist.h>
+#include <robot_nav_msgs/Path.h>
+
+namespace recovery_core
+{
+
+/**
+ * @enum RecoveryStatus
+ * @brief Trạng thái tiến trình của một lượt recovery.
+ */
+enum class RecoveryStatus
+{
+  kIdle,       ///< Chưa bắt đầu (sau initialize/reset).
+  kRunning,    ///< Đang thực thi, cần tiếp tục gọi.
+  kSucceeded,  ///< Hoàn thành thành công.
+  kFailed      ///< Lỗi/không thể thực thi an toàn (trả stop output).
+};
+
+/**
+ * @enum RecoveryOutputType
+ * @brief Loại output hành vi sinh ra ở kết quả hiện tại.
+ *        Quyết định trường nào trong RecoveryResult là hợp lệ để đọc.
+ */
+enum class RecoveryOutputType
+{
+  kNone,      ///< Không output (vd: clear costmap) — chỉ đọc status.
+  kVelocity,  ///< Output là command (Twist) — họ rotation/backup.
+  kPath       ///< Output là path — họ regen path.
+};
+
+/**
+ * @struct RecoveryResult
+ * @brief Kết quả hợp nhất cho cả 3 họ recovery.
+ *
+ * Bất biến: chỉ đọc trường khớp với @ref output_type.
+ *  - kNone     : bỏ qua command/path.
+ *  - kVelocity : dùng command; path để mặc định.
+ *  - kPath     : dùng path; command để mặc định.
+ */
+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.
+
+  /// @brief Đang chạy, không output.
+  static RecoveryResult Running();
+  /// @brief Thành công, không output.
+  static RecoveryResult Succeeded();
+  /// @brief Thất bại, không output (caller nên dừng an toàn).
+  static RecoveryResult Failed();
+  /// @brief Output vận tốc kèm status (kRunning hoặc kSucceeded).
+  static RecoveryResult Velocity(const robot_geometry_msgs::Twist& command,
+                                 RecoveryStatus status);
+  /// @brief Output path kèm status.
+  static RecoveryResult PathOut(const robot_nav_msgs::Path& path,
+                                RecoveryStatus status);
+};
+
+}  // namespace recovery_core
+
+#endif  // RECOVERY_CORE_RECOVERY_TYPES_H_