158 lines
6.2 KiB
C++
158 lines
6.2 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — cổng ra phía recovery.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#ifndef MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|
|
#define MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|
|
|
|
#include <cstddef>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <robot_geometry_msgs/PoseStamped.h>
|
|
#include <robot_geometry_msgs/Twist.h>
|
|
|
|
namespace robot
|
|
{
|
|
class NodeHandle;
|
|
}
|
|
|
|
namespace move_base2
|
|
{
|
|
|
|
/// @brief Lý do vào recovery. Behavior có thể dùng để chọn chiến lược khác nhau cho cùng một plugin.
|
|
enum class RecoveryTrigger
|
|
{
|
|
kPlanningFailed, ///< Không lập được plan trong thời gian cho phép.
|
|
kControllingFailed, ///< Không sinh được lệnh vận tốc hợp lệ trong thời gian cho phép.
|
|
kOscillation ///< Robot quẩn tại chỗ quá lâu.
|
|
};
|
|
|
|
const char* toString(RecoveryTrigger trigger);
|
|
|
|
/**
|
|
* @struct RecoveryRoutes
|
|
* @brief Các route recovery đã resolve từ tên instance YAML sang index trong @ref RecoveryPort.
|
|
*
|
|
* Registry/plugin chỉ biết danh sách behavior. Policy "lỗi nào thử behavior nào trước" thuộc
|
|
* move_base2, nên @ref RecoveryRunner parse `recovery/routes` sau khi registry đã nạp xong rồi
|
|
* chuyển tên instance thành index ở đây. State machine chỉ nhìn thấy index — giữ lõi thuần, không
|
|
* phụ thuộc YAML hay recovery_core.
|
|
*
|
|
* Ba vector để rỗng cùng lúc nghĩa là legacy fallback: mọi trigger dùng toàn bộ behavior theo thứ
|
|
* tự registry. Khi một route được khai thì cả ba route phải có ít nhất một index hợp lệ.
|
|
*/
|
|
struct RecoveryRoutes
|
|
{
|
|
std::vector<std::size_t> planning_failed;
|
|
std::vector<std::size_t> controlling_failed;
|
|
std::vector<std::size_t> oscillation;
|
|
|
|
const std::vector<std::size_t>& forTrigger(RecoveryTrigger trigger) const;
|
|
bool empty() const;
|
|
};
|
|
|
|
/**
|
|
* @enum RecoveryOutputKind
|
|
* @brief Behavior đó có lái robot hay không.
|
|
*
|
|
* Lõi cần biết điều này **trước** khi behavior chạy tick đầu tiên: nếu behavior không phát vận tốc
|
|
* (đợi, xoá costmap) thì nguồn vận tốc của cycle phải là `kNone`, không phải `kRecovery`. Coi mọi
|
|
* behavior là nguồn vận tốc khiến `VelocityArbiter` đổi nguồn hai lần cho mỗi lượt one-shot, mỗi
|
|
* lần chèn một cycle zero — vài chục ms cmd_vel = 0 không vì lý do gì.
|
|
*
|
|
* Khai riêng ở đây thay vì dùng `recovery_core::RecoveryOutputType`: lõi không được include
|
|
* recovery framework (D3). `RecoveryRunner` map 1:1 hai enum bằng `switch`.
|
|
*/
|
|
enum class RecoveryOutputKind
|
|
{
|
|
kNone, ///< Không phát output (đợi, xoá costmap).
|
|
kVelocity, ///< Phát Twist mỗi cycle (rotate, back up).
|
|
kPath ///< Sinh đường đi mới.
|
|
};
|
|
|
|
const char* toString(RecoveryOutputKind kind);
|
|
|
|
/**
|
|
* @struct RecoveryTick
|
|
* @brief Kết quả một control cycle của recovery, đã chuẩn hoá về ngôn ngữ của move_base2.
|
|
*
|
|
* Chuẩn hoá ở đây thay vì dùng thẳng kiểu của recovery framework là có chủ đích: lõi không được
|
|
* include recovery framework, nếu không chiều phụ thuộc sẽ khoá cứng move_base2 vào một hiện thực.
|
|
*
|
|
* Bất biến đọc: chỉ đọc @ref cmd khi @ref has_velocity, chỉ đọc @ref path khi @ref has_path. Một
|
|
* tick không bao giờ vừa có vận tốc vừa có path.
|
|
*/
|
|
struct RecoveryTick
|
|
{
|
|
enum class Status
|
|
{
|
|
kRunning,
|
|
kSucceeded,
|
|
kFailed
|
|
};
|
|
|
|
Status status = Status::kRunning;
|
|
|
|
bool has_velocity = false; ///< true -> @ref cmd hợp lệ.
|
|
robot_geometry_msgs::Twist cmd; ///< [m/s], [rad/s]. Dấu âm của linear.x nghĩa là lùi.
|
|
|
|
bool has_path = false; ///< true -> @ref path hợp lệ (họ recovery sinh lại đường đi).
|
|
std::vector<robot_geometry_msgs::PoseStamped> path;
|
|
|
|
std::string message; ///< Mô tả người-đọc-được, chỉ để log khi state đổi.
|
|
};
|
|
|
|
/**
|
|
* @class RecoveryPort
|
|
* @brief Cổng ra phía recovery. Tick-based: mỗi control cycle một lời gọi @ref update.
|
|
*
|
|
* Đây là khác biệt kiến trúc so với thế hệ 1, không phải đổi tên hàm. Recovery thế hệ 1 chạy
|
|
* blocking bên trong một lời gọi và không trả gì; recovery thế hệ 2 trả kết quả từng cycle và có
|
|
* thể phát vận tốc. Hệ quả bắt buộc: recovery phải được tick từ đúng thread đang sở hữu cmd_vel,
|
|
* không được có thread riêng — hai thread cùng phát vận tốc là hai bộ điều khiển tranh nhau.
|
|
*/
|
|
class RecoveryPort
|
|
{
|
|
public:
|
|
virtual ~RecoveryPort() = default;
|
|
|
|
/// @brief Nạp và cấu hình danh sách behavior. Gọi một lần lúc khởi tạo.
|
|
virtual bool configure(robot::NodeHandle& nh) = 0;
|
|
|
|
/// @brief Số behavior đã nạp được. 0 nghĩa là không có đường phục hồi nào.
|
|
virtual std::size_t behaviorCount() const = 0;
|
|
|
|
/**
|
|
* @brief Họ output của behavior thứ @p index — behavior đó có lái robot hay không.
|
|
*
|
|
* Được hỏi **trước** khi behavior chạy, kể cả ở cycle khởi động nó. Index sai phải trả
|
|
* @ref RecoveryOutputKind::kNone (giả định an toàn: không cấp quyền phát vận tốc cho thứ không
|
|
* biết là gì).
|
|
*/
|
|
virtual RecoveryOutputKind outputKind(std::size_t index) const = 0;
|
|
|
|
/**
|
|
* @brief Bắt đầu behavior thứ @p index.
|
|
* @return false nếu index sai hoặc behavior từ chối khởi động (ví dụ đã va chạm ngay tại chỗ).
|
|
*/
|
|
virtual bool start(std::size_t index, RecoveryTrigger trigger) = 0;
|
|
|
|
/// @brief Một control cycle. CHỈ được gọi sau khi @ref start trả true.
|
|
virtual RecoveryTick update() = 0;
|
|
|
|
/// @brief Yêu cầu dừng. Tick kế tiếp phải trả kết quả dừng an toàn.
|
|
virtual void cancel() = 0;
|
|
|
|
/// @brief Tên behavior thứ @p index; chuỗi rỗng nếu index sai.
|
|
virtual std::string behaviorName(std::size_t index) const = 0;
|
|
};
|
|
|
|
} // namespace move_base2
|
|
|
|
#endif // MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|