DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

temporary storage 7/9/2026 19:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 19:41:20 +07:00
parent 915cf85cc5
commit e9394434ba
17 changed files with 658 additions and 566 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
include/recovery_core/recovery_behavior.h
View File

@@ -4,8 +4,10 @@
*
* 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).
* Vòng đời hợp nhất, hướng-goal cho cả 3 họ recovery (path / none / velocity):
* configure(name, ctx) -> start(goal) -> lặp update(dt) -> [cancel()]
* Base lo phần chung (guard init/dt/timeout/cancel, đo elapsed, đẩy feedback); plugin chỉ
* override các hook nhỏ onConfigure()/onStart()/onUpdate().
*
* Author: DuongTD
*********************************************************************/
@@ -27,72 +29,113 @@ 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_*).
* @brief Interface hướng-goal cho mọi hành vi recovery (không chạy roscpp/ROS master thật).
*
* 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(...)
* Thiết kế theo template-method: API công khai (configure/start/update/cancel) là NON-VIRTUAL
* và do base xử lý phần lặp lại; plugin chỉ triển khai các hook protected:
* - onConfigure() : đọc param riêng từ ctx()/NodeHandle (tuỳ chọn).
* - onStart(goal) : chốt mục tiêu lượt này (rad/m/pose), reset trạng thái tiến độ.
* - onUpdate(dt) : một "tick"; họ one-shot (path/clear) hoàn tất ngay lần đầu.
*
* 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.
* Ba họ hành vi:
* - Họ A (trả path) : onUpdate() trả RecoveryResult::PathOut(...), kSucceeded ngay.
* - Họ B (không output) : onUpdate() làm việc rồi trả Succeeded()/Failed() ngay.
* - Họ C (trả vận tốc) : onUpdate() sinh Twist mỗi cycle tới khi đạt goal -> kSucceeded.
*
* Bất biến: start() chỉ hợp lệ sau configure(); update() chỉ chạy sau start(). Vi phạm ->
* RecoveryResult::Failed(). Base tự trả stop output + kCancelled sau khi cancel().
*/
class RecoveryBehavior
{
public:
/// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace
/// (boost::dll::import_alias<recovery_core::RecoveryBehavior::Ptr()>(...)).
/// (boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(...)).
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;
// ------------------------------------------------------------------
// API công khai — NON-VIRTUAL, base xử lý phần chung.
// ------------------------------------------------------------------
/**
* @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().
* @brief Cấu hình một lần: cache ngữ cảnh (không sở hữu), đọc config chung, gọi onConfigure().
* @param name Tên instance (namespace param + log).
* @param ctx Ngữ cảnh môi trường (tf/costmap/global_path). Gọi lại lần 2 bị bỏ qua.
*/
virtual RecoveryResult runBehavior() = 0;
void configure(const std::string& name, const RecoveryContext& ctx);
/**
* @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.
* @brief Bắt đầu một lượt recovery với mục tiêu RUNTIME.
* @param goal Mục tiêu lượt này (góc/khoảng lùi/pose + override). Field 0 = dùng default plugin.
* @return Kết quả tick đầu (thường kRunning; kFailed nếu chưa configure hoặc goal không hợp lệ).
*/
RecoveryResult start(const RecoveryGoal& goal);
/**
* @brief Một control cycle. Guard chưa start/dt<=0/timeout/cancel trước khi gọi onUpdate().
* @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);
RecoveryResult update(double dt);
/**
* @brief Trạng thái hiện tại của lượt recovery.
* @brief Yêu cầu huỷ: update() kế tiếp trả stop output + kCancelled.
*/
virtual RecoveryStatus status() const = 0;
void cancel();
virtual std::string getNameRecoveryBehavior() const
/// @brief Trạng thái hiện tại của lượt recovery.
RecoveryStatus status() const
{
return status_;
}
/// @brief Thời gian đã trôi từ start() (s).
double elapsed() const
{
return elapsed_;
}
const std::string& name() const
{
return name_;
}
/// Giữ tên cũ cho tương thích call-site loader.
std::string getNameRecoveryBehavior() const
{
return name_;
}
protected:
RecoveryBehavior() = default;
// ------------------------------------------------------------------
// Hook cho plugin.
// ------------------------------------------------------------------
/// @brief Đọc param riêng (qua ctx()/NodeHandle) sau khi base cache ngữ cảnh. Tuỳ chọn.
virtual void onConfigure() {}
/// @brief Chốt mục tiêu lượt này; reset bộ đếm tiến độ nội bộ. Trả tick khởi đầu.
virtual RecoveryResult onStart(const RecoveryGoal& goal) = 0;
/// @brief Một tick. Họ one-shot trả kSucceeded/kFailed ngay; họ velocity trả kRunning tới goal.
virtual RecoveryResult onUpdate(double dt) = 0;
// Truy cập cho plugin (chỉ đọc ngữ cảnh/goal).
const RecoveryContext& ctx() const { return ctx_; }
const RecoveryGoal& goal() const { return goal_; }
bool cancelRequested() const { return cancel_requested_; }
RecoveryContext ctx_;
RecoveryGoal goal_;
std::string name_;
double timeout_ = 0.0; ///< s — 0 nghĩa là không timeout; đọc param "~/<name>/timeout".
double elapsed_ = 0.0;
bool configured_ = false;
bool started_ = false;
bool cancel_requested_ = false;
RecoveryStatus status_ = RecoveryStatus::kIdle;
};
} // namespace recovery_core

53
include/recovery_core/recovery_config.h
View File

@@ -1,53 +0,0 @@
/*********************************************************************
*
* 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_

84
include/recovery_core/recovery_types.h
View File

@@ -9,9 +9,18 @@
#ifndef RECOVERY_CORE_RECOVERY_TYPES_H_
#define RECOVERY_CORE_RECOVERY_TYPES_H_
#include <map>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_nav_msgs/Path.h>
// Forward declare để không kéo header nặng vào contract type.
namespace tf3 { class BufferCore; }
namespace robot_costmap_2d { class Costmap2DROBOT; }
namespace recovery_core
{
@@ -21,10 +30,11 @@ namespace recovery_core
*/
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).
kIdle, ///< Chưa bắt đầu (sau configure/reset).
kRunning, ///< Đang thực thi, cần tiếp tục gọi update().
kSucceeded, ///< Hoàn thành thành công (đạt goal).
kFailed, ///< Lỗi/không thể thực thi an toàn (trả stop output).
kCancelled ///< Bị caller huỷ giữa chừng (trả stop output).
};
/**
@@ -40,13 +50,59 @@ enum class RecoveryOutputType
};
/**
* @struct RecoveryResult
* @brief Kết quả hợp nhất cho cả 3 họ recovery.
* @struct RecoveryContext
* @brief Ngữ cảnh môi trường cấp cho behavior một lần qua configure().
*
* Bất biến: chỉ đọc trường khớp với @ref output_type.
* Gói các con trỏ KHÔNG sở hữu (tf/costmap/global_path) thay cho danh sách tham số dài của
* initialize() cũ. Thêm field mới ở đây không phá vỡ chữ ký configure() của mọi plugin.
*/
struct RecoveryContext
{
tf3::BufferCore* tf = nullptr; ///< Transform buffer.
std::vector<robot_geometry_msgs::PoseStamped>* global_path = nullptr; ///< Plan hiện tại.
robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr; ///< Costmap toàn cục.
robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr; ///< Costmap cục bộ.
};
/**
* @struct RecoveryGoal
* @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào start(goal).
*
* Đây là điểm cốt lõi giúp behavior "thông minh" hơn: cùng một plugin, mỗi lượt caller có thể
* yêu cầu góc quay / khoảng lùi khác nhau, thay vì cố định trong config lúc configure().
*
* Quy ước dùng default: trường mang giá trị 0 (hoặc has_target_pose == false) nghĩa là "dùng
* default đã cấu hình của plugin". Đơn vị: angle [rad], distance [m].
*/
struct RecoveryGoal
{
double angle = 0.0; ///< rad — góc quay đích (họ rotate). 0 = dùng default plugin.
double distance = 0.0; ///< m — khoảng lùi đích (họ backup). 0 = dùng default plugin.
robot_geometry_msgs::PoseStamped target_pose; ///< Pose đích (họ path/detour), tuỳ chọn.
bool has_target_pose = false; ///< true nếu target_pose hợp lệ.
std::map<std::string, double> params; ///< Override mở rộng theo từng plugin (vd tốc độ).
/// @brief Đọc override double trong params, trả default nếu không có.
double param(const std::string& key, double fallback) const
{
const auto it = params.find(key);
return it == params.end() ? fallback : it->second;
}
};
/**
* @struct RecoveryResult
* @brief Kết quả hợp nhất + rich feedback cho cả 3 họ recovery.
*
* Bất biến output: 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.
*
* Feedback (progress/remaining/elapsed/message) luôn hợp lệ để caller giám sát/log, độc lập với
* output_type. progress trong [0,1]; remaining theo đơn vị của goal (rad hoặc m).
*/
struct RecoveryResult
{
@@ -56,18 +112,30 @@ struct RecoveryResult
robot_geometry_msgs::Twist command; ///< Hợp lệ khi output_type == kVelocity.
robot_nav_msgs::Path path; ///< Hợp lệ khi output_type == kPath.
double progress = 0.0; ///< [0,1] tiến độ tới goal.
double remaining = 0.0; ///< Phần còn lại tới goal (rad hoặc m). >= 0.
double elapsed = 0.0; ///< s — thời gian trôi từ start().
std::string message; ///< Mô tả người-đọc-được (vd "rotated 1.20/1.57 rad").
/// @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).
/// @brief Bị huỷ, không output (caller nên dừng an toàn).
static RecoveryResult Cancelled();
/// @brief Output vận tốc kèm status (kRunning/kSucceeded/kFailed/kCancelled).
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);
/// @brief Gắn thêm feedback (fluent) — trả về chính nó để chain.
RecoveryResult& withProgress(double progress_value, double remaining_value);
/// @brief Gắn message (fluent).
RecoveryResult& withMessage(std::string text);
};
} // namespace recovery_core