optimal & fix file cmake
This commit is contained in:
92
include/recovery_core/adapters/costmap_collision_checker.h
Normal file
92
include/recovery_core/adapters/costmap_collision_checker.h
Normal file
@@ -0,0 +1,92 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — CollisionChecker dựng trên Costmap2DROBOT.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_
|
||||
#define RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/Point.h>
|
||||
|
||||
#include <recovery_core/recovery_context.h>
|
||||
|
||||
namespace robot_costmap_2d { class Costmap2DROBOT; }
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapCollisionChecker
|
||||
* @brief Cost của footprint tại một pose giả định, quét trên costmap hiện hành.
|
||||
*
|
||||
* Mã trả về khớp **chính xác** `nav_test_harness::FakeCollisionChecker` để code tiêu thụ không đổi
|
||||
* cách đọc giữa test và runtime:
|
||||
* - `-1` một điểm của footprint nằm ngoài bản đồ;
|
||||
* - `-2` một điểm rơi vào `LETHAL_OBSTACLE` hoặc `INSCRIBED_INFLATED_OBSTACLE`;
|
||||
* - `-3` một điểm rơi vào `NO_INFORMATION` (chưa biết → coi là không đi được);
|
||||
* - `>= 0` cost lớn nhất dọc biên footprint.
|
||||
*
|
||||
* @note Cố ý **không** bọc `robot_base_local_planner::CostmapModel`: lớp đó cache một tham chiếu
|
||||
* costmap trong constructor (đúng thứ CLAUDE.md cấm), và mã lỗi của nó ngược với harness
|
||||
* (-1 lethal / -3 ngoài bản đồ), nên bọc nó sẽ làm runtime và test bất đồng đúng ở chỗ khó
|
||||
* thấy nhất.
|
||||
*
|
||||
* @warning Chỉ quét **biên** đa giác footprint, không quét lòng trong — giống `CostmapModel`. Ở
|
||||
* runtime inflation layer làm vật cản nở ra nên biên luôn chạm; trong test thì kịch bản
|
||||
* phải tự đặt vật cản chạm biên.
|
||||
*
|
||||
* Con trỏ costmap **non-owning**; lớp này không cache `Costmap2D*` bên trong mà lấy lại mỗi lần gọi
|
||||
* — costmap có thể bị thay giữa hai cycle, và cache chính là nguyên nhân lỗi double-free đã ghi
|
||||
* nhận trong workspace.
|
||||
*/
|
||||
class CostmapCollisionChecker final : public CollisionChecker
|
||||
{
|
||||
public:
|
||||
/// Mã lỗi, khớp `nav_test_harness::FakeCollisionChecker`.
|
||||
static constexpr double kOutsideMap = -1.0;
|
||||
static constexpr double kLethal = -2.0;
|
||||
static constexpr double kUnknown = -3.0;
|
||||
|
||||
explicit CostmapCollisionChecker(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
|
||||
: costmap_(costmap)
|
||||
{
|
||||
}
|
||||
|
||||
/// @brief Trỏ lại vào costmap hiện hành. Không sở hữu.
|
||||
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
|
||||
{
|
||||
costmap_ = costmap;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Ép dùng footprint này thay cho footprint của costmap.
|
||||
* @param footprint Đa giác trong frame robot [m]. Rỗng = quay lại dùng footprint của costmap.
|
||||
*/
|
||||
void setFootprintOverride(std::vector<robot_geometry_msgs::Point> footprint);
|
||||
|
||||
/// @brief Bước lấy mẫu dọc cạnh footprint [m]. Bỏ qua giá trị <= 0.
|
||||
void setSampleStep(double step_m);
|
||||
|
||||
double footprintCost(double x, double y, double theta) const override;
|
||||
|
||||
private:
|
||||
/// @return cost tại một điểm toàn cục, hoặc mã lỗi âm.
|
||||
double pointCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double wx, double wy) const;
|
||||
|
||||
/// @return cost lớn nhất dọc đoạn thẳng, hoặc mã lỗi âm đầu tiên gặp phải.
|
||||
double lineCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double x0, double y0, double x1,
|
||||
double y1) const;
|
||||
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; ///< non-owning
|
||||
std::vector<robot_geometry_msgs::Point> footprint_override_;
|
||||
double sample_step_ = 0.025; ///< [m]
|
||||
};
|
||||
|
||||
} // namespace recovery_core
|
||||
|
||||
#endif // RECOVERY_CORE_ADAPTERS_COSTMAP_COLLISION_CHECKER_H_
|
||||
54
include/recovery_core/adapters/costmap_pose_provider.h
Normal file
54
include/recovery_core/adapters/costmap_pose_provider.h
Normal file
@@ -0,0 +1,54 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — PoseProvider dựng trên Costmap2DROBOT.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_
|
||||
#define RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_
|
||||
|
||||
#include <recovery_core/recovery_context.h>
|
||||
|
||||
namespace robot_costmap_2d { class Costmap2DROBOT; }
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
|
||||
/**
|
||||
* @class CostmapPoseProvider
|
||||
* @brief Lấy pose robot qua `Costmap2DROBOT::getRobotPose` (đã gồm tra TF + transform_tolerance).
|
||||
*
|
||||
* Con trỏ costmap là **non-owning** và có thể bị thay giữa hai lượt recovery — dùng @ref setCostmap
|
||||
* để caller làm mới trước mỗi `start()`/`update()` thay vì giữ một bản cache trong context.
|
||||
*/
|
||||
class CostmapPoseProvider final : public PoseProvider
|
||||
{
|
||||
public:
|
||||
explicit CostmapPoseProvider(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
|
||||
: costmap_(costmap)
|
||||
{
|
||||
}
|
||||
|
||||
/// @brief Trỏ lại vào costmap hiện hành. Không sở hữu.
|
||||
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
|
||||
{
|
||||
costmap_ = costmap;
|
||||
}
|
||||
|
||||
/**
|
||||
* @copydoc PoseProvider::getRobotPose
|
||||
*
|
||||
* Trả false khi chưa có costmap hoặc TF không cho ra pose — hai trường hợp đều nghĩa là "không
|
||||
* biết robot ở đâu", và plugin phải dừng an toàn.
|
||||
*/
|
||||
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override;
|
||||
|
||||
private:
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; ///< non-owning
|
||||
};
|
||||
|
||||
} // namespace recovery_core
|
||||
|
||||
#endif // RECOVERY_CORE_ADAPTERS_COSTMAP_POSE_PROVIDER_H_
|
||||
@@ -4,10 +4,8 @@
|
||||
*
|
||||
* recovery_core — interface (base class) cho recovery behaviors.
|
||||
*
|
||||
* 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() -> [cancel()]
|
||||
* Base lo phần chung (guard init/cancel, đẩy feedback); plugin chỉ
|
||||
* override các hook nhỏ onConfigure()/onStart()/onUpdate().
|
||||
* Vòng đời tick-based, hướng-goal:
|
||||
* configure(name, ctx, nh) -> start(goal, now) -> lặp update(now) -> [cancel()]
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
@@ -16,12 +14,11 @@
|
||||
|
||||
#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 <robot/node_handle.h>
|
||||
#include <robot/time.h>
|
||||
|
||||
#include <recovery_core/recovery_context.h>
|
||||
#include <recovery_core/recovery_types.h>
|
||||
|
||||
namespace recovery_core
|
||||
@@ -29,60 +26,82 @@ namespace recovery_core
|
||||
|
||||
/**
|
||||
* @class RecoveryBehavior
|
||||
* @brief Interface hướng-goal cho mọi hành vi recovery (không chạy roscpp/ROS master thật).
|
||||
* @brief Interface hướng-goal, tick-based cho mọi hành vi recovery.
|
||||
*
|
||||
* 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() : một "tick"; họ one-shot (path/clear) hoàn tất ngay lần đầu.
|
||||
* Template-method: API công khai (`configure`/`start`/`update`/`cancel`) là NON-VIRTUAL và base giữ
|
||||
* toàn bộ bất biến; plugin chỉ triển khai hook protected. Khác bản trước ở chỗ base **thật sự** giữ
|
||||
* được bất biến — mọi state là `private`, plugin không ghi vào đượ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.
|
||||
* Base chịu trách nhiệm:
|
||||
* - guard vòng đời (`start` sau `configure`, `update` sau `start`);
|
||||
* - kiểm ngữ cảnh bắt buộc theo @ref outputKind (họ velocity phải có pose + collision checker);
|
||||
* - validate goal (NaN/Inf) trước khi giao cho plugin;
|
||||
* - đo `elapsed` và ép `timeout`;
|
||||
* - cưỡng chế `output_type` của mọi kết quả thuộc `{outputKind(), kNone}`;
|
||||
* - sinh **stop output đúng họ** khi guard/cancel/timeout — họ velocity nhận Twist 0 tường minh,
|
||||
* họ khác nhận `kNone` (base không bịa ra output vận tốc cho behavior không lái).
|
||||
*
|
||||
* 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().
|
||||
* Plugin chịu trách nhiệm: đọc param riêng, chốt mục tiêu lượt này, và sinh một tick an toàn.
|
||||
*
|
||||
* @note Không thread-safe. Recovery được tick từ đúng thread sở hữu cmd_vel; hai thread cùng phát
|
||||
* vận tốc là hai bộ điều khiển tranh nhau.
|
||||
*/
|
||||
class RecoveryBehavior
|
||||
{
|
||||
public:
|
||||
/// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace
|
||||
/// (boost::dll::import_alias<recovery_core::RecoveryBehavior::RecoveryBehaviorPtr()>(...)).
|
||||
/// shared_ptr để khớp cơ chế nạp Boost.DLL của workspace.
|
||||
using RecoveryBehaviorPtr = std::shared_ptr<RecoveryBehavior>;
|
||||
|
||||
virtual ~RecoveryBehavior() = default;
|
||||
|
||||
RecoveryBehavior(const RecoveryBehavior&) = delete;
|
||||
RecoveryBehavior& operator=(const RecoveryBehavior&) = delete;
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// API công khai — NON-VIRTUAL, base xử lý phần chung.
|
||||
// API công khai — NON-VIRTUAL.
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
/**
|
||||
* @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.
|
||||
* @brief Cấu hình một lần.
|
||||
* @param name Tên instance, dùng cho log.
|
||||
* @param ctx Các cổng môi trường. Base kiểm cổng bắt buộc theo @ref outputKind.
|
||||
* @param nh NodeHandle **đã được caller scope sẵn vào namespace param của instance này**.
|
||||
* Plugin không tự dựng NodeHandle từ disk — nếu tự dựng thì không test được mà không
|
||||
* có cây config thật, và đó là lý do bộ test cũ phải đọc config production.
|
||||
* @return false nếu thiếu cổng bắt buộc, param không hợp lệ, hoặc gọi lần thứ hai.
|
||||
*/
|
||||
void configure(const std::string& name, const RecoveryContext& ctx);
|
||||
bool configure(const std::string& name, const RecoveryContext& ctx, robot::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @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ệ).
|
||||
* @brief Bắt đầu một lượt recovery.
|
||||
* @param goal Mục tiêu lượt này. Trường không đặt = dùng default của plugin.
|
||||
* @param now Thời điểm hiện tại — mốc cho `elapsed` và `timeout`.
|
||||
* @return false nếu chưa configure, goal không hợp lệ, hoặc plugin từ chối khởi động (ví dụ cung
|
||||
* sẽ quay đã bị chặn). **Không sinh tick ở đây** — tick chỉ ra từ `update()`.
|
||||
*/
|
||||
RecoveryResult start(const RecoveryGoal& goal);
|
||||
bool start(const RecoveryGoal& goal, const robot::Time& now);
|
||||
|
||||
/**
|
||||
* @brief Một control cycle. Guard chưa start/cancel trước khi gọi onUpdate().
|
||||
* @brief Một control cycle.
|
||||
* @param now Thời điểm hiện tại. Base tự tính `dt = now - lần update trước`.
|
||||
*
|
||||
* Gọi sau khi lượt đã kết thúc thì trả lại đúng trạng thái kết thúc kèm stop output, không tick
|
||||
* thêm. Gọi khi chưa `start()` thì trả `kFailed` + stop output.
|
||||
*/
|
||||
RecoveryResult update();
|
||||
RecoveryResult update(const robot::Time& now);
|
||||
|
||||
/**
|
||||
* @brief Yêu cầu huỷ: update() kế tiếp trả stop output + kCancelled.
|
||||
* @brief Yêu cầu dừng. Tick kế tiếp trả stop output + `kCancelled`.
|
||||
*
|
||||
* @note Trong `move_base2`, state machine không tick recovery sau khi cancel (state chuyển sang
|
||||
* `CANCELLING`, nơi mọi nguồn vận tốc bị khoá), nên `kCancelled` thường không bao giờ đến
|
||||
* tay caller. Đường này vẫn phải đúng: nó là hàng rào cho host khác và cho tương lai.
|
||||
*/
|
||||
void cancel();
|
||||
|
||||
/// @brief Trạng thái hiện tại của lượt recovery.
|
||||
/// @brief Họ output của behavior. Khai một lần; base dùng để cưỡng chế bất biến.
|
||||
virtual RecoveryOutputType outputKind() const = 0;
|
||||
|
||||
RecoveryStatus status() const
|
||||
{
|
||||
return status_;
|
||||
@@ -93,10 +112,16 @@ public:
|
||||
return name_;
|
||||
}
|
||||
|
||||
/// Giữ tên cũ cho tương thích call-site loader.
|
||||
std::string getNameRecoveryBehavior() const
|
||||
/// @brief [s] thời gian trôi từ `start()`. 0 nếu chưa start.
|
||||
double elapsed() const
|
||||
{
|
||||
return name_;
|
||||
return elapsed_;
|
||||
}
|
||||
|
||||
/// @brief [s] trần thời gian một lượt; 0 = không giới hạn.
|
||||
double timeout() const
|
||||
{
|
||||
return timeout_;
|
||||
}
|
||||
|
||||
protected:
|
||||
@@ -106,23 +131,79 @@ protected:
|
||||
// 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 Đọc param riêng của plugin từ @p nh (đã scope sẵn).
|
||||
* @return false nếu param không hợp lệ tới mức không chạy được. Sai nhẹ thì log + dùng default.
|
||||
*/
|
||||
virtual bool onConfigure(robot::NodeHandle& nh) = 0;
|
||||
|
||||
/// @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 Chốt mục tiêu lượt này, reset bộ đếm tiến độ, kiểm điều kiện an toàn để khởi động.
|
||||
* @return false = từ chối khởi động; base đặt status về kFailed.
|
||||
*/
|
||||
virtual bool 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() = 0;
|
||||
/**
|
||||
* @brief Một tick.
|
||||
* @param now Thời điểm hiện tại.
|
||||
* @param dt [s] khoảng cách tới tick trước, đo **thật** chứ không phải chu kỳ cấu hình.
|
||||
* Luôn >= 0; tick đầu sau `start()` có dt = 0.
|
||||
*
|
||||
* @warning Không được block quá một phần nhỏ của chu kỳ control — nó chạy trên thread phát
|
||||
* cmd_vel. Không parse file, không cấp phát lớn, không gọi việc nặng của costmap.
|
||||
*/
|
||||
virtual RecoveryResult onUpdate(const robot::Time& now, 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_; }
|
||||
/**
|
||||
* @brief Cơ hội phát lệnh giảm tốc trước khi dừng hẳn.
|
||||
*
|
||||
* Default trả stop output ngay. Plugin họ velocity đang chạy nhanh nên override để ramp về 0 thay
|
||||
* vì nhảy bậc — dù `VelocityArbiter` phía ngoài có clamp gia tốc, kế hoạch dừng là việc của
|
||||
* plugin, không phải của hàng rào cuối.
|
||||
*/
|
||||
virtual RecoveryResult onCancel();
|
||||
|
||||
// ------------------------------------------------------------------
|
||||
// Truy cập chỉ-đọc cho plugin.
|
||||
// ------------------------------------------------------------------
|
||||
|
||||
const RecoveryContext& ctx() const
|
||||
{
|
||||
return ctx_;
|
||||
}
|
||||
|
||||
const RecoveryGoal& goal() const
|
||||
{
|
||||
return goal_;
|
||||
}
|
||||
|
||||
bool cancelRequested() const
|
||||
{
|
||||
return cancel_requested_;
|
||||
}
|
||||
|
||||
/// @brief Stop output đúng họ của behavior này, kèm @p status.
|
||||
RecoveryResult stopResult(RecoveryStatus status) const;
|
||||
|
||||
private:
|
||||
/// Kiểm ngữ cảnh có đủ cổng bắt buộc cho họ output này không.
|
||||
bool validateContext() const;
|
||||
|
||||
/// Kiểm goal không mang NaN/Inf và distance > 0 nếu được đặt.
|
||||
bool validateGoal(const RecoveryGoal& goal) const;
|
||||
|
||||
/// Ép bất biến output + gắn elapsed lên kết quả plugin trả về.
|
||||
RecoveryResult finalize(RecoveryResult result) const;
|
||||
|
||||
RecoveryContext ctx_;
|
||||
RecoveryGoal goal_;
|
||||
std::string name_;
|
||||
|
||||
robot::Time start_time_;
|
||||
robot::Time last_update_;
|
||||
double elapsed_ = 0.0; ///< [s]
|
||||
double timeout_ = 0.0; ///< [s], 0 = không giới hạn
|
||||
|
||||
bool configured_ = false;
|
||||
bool started_ = false;
|
||||
bool cancel_requested_ = false;
|
||||
|
||||
126
include/recovery_core/recovery_context.h
Normal file
126
include/recovery_core/recovery_context.h
Normal file
@@ -0,0 +1,126 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — ngữ cảnh môi trường cấp cho recovery behavior.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef RECOVERY_CORE_RECOVERY_CONTEXT_H_
|
||||
#define RECOVERY_CORE_RECOVERY_CONTEXT_H_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
|
||||
// Forward declare: contract không kéo header costmap vào mọi plugin.
|
||||
namespace robot_costmap_2d { class Costmap2DROBOT; }
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
|
||||
/**
|
||||
* @enum RecoveryTrigger
|
||||
* @brief Vì sao caller vào recovery.
|
||||
*
|
||||
* Khai riêng ở đây thay vì dùng `move_base2::RecoveryTrigger` vì chiều phụ thuộc là một chiều:
|
||||
* `move_base2` biết `recovery_core`, không được ngược lại. `RecoveryRunner` map 1:1 hai enum bằng
|
||||
* `switch`, nên compiler bắt được ngay khi một bên thêm giá trị mới.
|
||||
*/
|
||||
enum class RecoveryTrigger
|
||||
{
|
||||
kUnspecified, ///< Caller không nói lý do.
|
||||
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);
|
||||
|
||||
/**
|
||||
* @class PoseProvider
|
||||
* @brief Nguồn pose robot trong global frame.
|
||||
*
|
||||
* Runtime: bọc `Costmap2DROBOT::getRobotPose` (đã gồm tra TF và kiểm `transform_tolerance`).
|
||||
* Test: fake bơm pose theo kịch bản.
|
||||
*
|
||||
* @invariant Trả `false` nghĩa là **không biết robot đang ở đâu** — TF thiếu, TF quá hạn, hoặc
|
||||
* frame chưa sẵn sàng. Plugin PHẢI dừng an toàn, tuyệt đối không dùng pose cũ để đi
|
||||
* tiếp. @p pose không được ghi khi hàm trả `false`.
|
||||
*/
|
||||
class PoseProvider
|
||||
{
|
||||
public:
|
||||
virtual ~PoseProvider() = default;
|
||||
|
||||
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class CollisionChecker
|
||||
* @brief Kiểm va chạm theo footprint tại một pose giả định.
|
||||
*
|
||||
* @warning Bên gọi chỉ được kiểm `< 0`. **Không so sánh với một giá trị âm cụ thể**: các hiện thực
|
||||
* trong workspace không thống nhất mã lỗi (`robot_base_local_planner::CostmapModel` dùng
|
||||
* -1 cho lethal và -3 cho ngoài bản đồ; `nav_test_harness::FakeCollisionChecker` dùng
|
||||
* -1 cho ngoài bản đồ và -2 cho lethal). Mọi giá trị âm đều mang đúng một nghĩa:
|
||||
* **không đặt robot ở đây được**.
|
||||
*/
|
||||
class CollisionChecker
|
||||
{
|
||||
public:
|
||||
virtual ~CollisionChecker() = default;
|
||||
|
||||
/**
|
||||
* @param x,y Vị trí giả định trong global frame [m].
|
||||
* @param theta Hướng giả định [rad].
|
||||
* @return `< 0` nghĩa là không đặt được (va chạm / ngoài bản đồ / unknown);
|
||||
* `>= 0` là cost lớn nhất gặp phải dọc biên footprint.
|
||||
*/
|
||||
virtual double footprintCost(double x, double y, double theta) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class PlanProvider
|
||||
* @brief Nguồn global plan hiện hành.
|
||||
*
|
||||
* Cố ý là **accessor**, không phải con trỏ cache trong context: runtime dùng mô hình triple-buffer
|
||||
* và **xoay con trỏ** giữa ba bộ đệm plan, nên một con trỏ lấy lúc `configure()` sẽ trỏ vào bộ đệm
|
||||
* đang giữ vai trò scratch sau vài chu kỳ planner — không phải plan đang chạy.
|
||||
*/
|
||||
class PlanProvider
|
||||
{
|
||||
public:
|
||||
virtual ~PlanProvider() = default;
|
||||
|
||||
/**
|
||||
* @param[out] out Plan hiện hành trong global frame. Chỉ được ghi khi hàm trả `true`.
|
||||
* @return false nếu chưa có plan nào.
|
||||
*/
|
||||
virtual bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RecoveryContext
|
||||
* @brief Các cổng môi trường cấp cho behavior một lần qua `configure()`.
|
||||
*
|
||||
* Chỉ chứa **cổng**, không chứa dữ liệu động. Cổng nào bắt buộc là tuỳ họ output của behavior —
|
||||
* `RecoveryBehavior::configure()` kiểm và từ chối nếu thiếu, thay vì để plugin phát hiện lúc chạy.
|
||||
*
|
||||
* Con trỏ costmap là **non-owning và có thể bị thay** giữa hai lượt: caller (`RecoveryRunner`) làm
|
||||
* mới context trước mỗi `start()`/`update()`. Plugin không được cache `Costmap2D*` bên trong qua
|
||||
* các tick.
|
||||
*/
|
||||
struct RecoveryContext
|
||||
{
|
||||
const PoseProvider* pose = nullptr; ///< Bắt buộc cho họ velocity.
|
||||
const CollisionChecker* collision = nullptr; ///< Bắt buộc cho họ velocity.
|
||||
const PlanProvider* plan = nullptr; ///< Bắt buộc cho họ path.
|
||||
|
||||
robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr; ///< non-owning.
|
||||
robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr; ///< non-owning.
|
||||
};
|
||||
|
||||
} // namespace recovery_core
|
||||
|
||||
#endif // RECOVERY_CORE_RECOVERY_CONTEXT_H_
|
||||
91
include/recovery_core/recovery_math.h
Normal file
91
include/recovery_core/recovery_math.h
Normal file
@@ -0,0 +1,91 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — helper hình học dùng chung cho plugin.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef RECOVERY_CORE_RECOVERY_MATH_H_
|
||||
#define RECOVERY_CORE_RECOVERY_MATH_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/Quaternion.h>
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
|
||||
/// @brief Yaw [rad] của quaternion, trong (-pi, pi].
|
||||
inline double yawFromQuaternion(const robot_geometry_msgs::Quaternion& q)
|
||||
{
|
||||
const double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
|
||||
const double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
|
||||
return std::atan2(siny_cosp, cosy_cosp);
|
||||
}
|
||||
|
||||
/// @brief Yaw [rad] của một pose.
|
||||
inline double yawOf(const robot_geometry_msgs::PoseStamped& pose)
|
||||
{
|
||||
return yawFromQuaternion(pose.pose.orientation);
|
||||
}
|
||||
|
||||
/// @brief Đưa góc về (-pi, pi].
|
||||
inline double normalizeAngle(double angle)
|
||||
{
|
||||
while (angle > M_PI)
|
||||
{
|
||||
angle -= 2.0 * M_PI;
|
||||
}
|
||||
while (angle <= -M_PI)
|
||||
{
|
||||
angle += 2.0 * M_PI;
|
||||
}
|
||||
return angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Quãng đường robot đã đi **dọc theo một hướng cho trước**, tính từ pose xuất phát.
|
||||
*
|
||||
* Đây là cách đo tiến độ đúng cho họ velocity: hình chiếu delta pose lên hướng ban đầu. Tích phân
|
||||
* vận tốc *lệnh* thì bánh trượt hay bị chặn vẫn báo đi đủ quãng.
|
||||
*
|
||||
* @param current,start Hai pose trong cùng global frame.
|
||||
* @param heading Hướng chiếu [rad].
|
||||
* @return [m] Dương nghĩa là đã đi theo chiều @p heading; âm là đi ngược lại.
|
||||
*/
|
||||
inline double projectOntoHeading(const robot_geometry_msgs::PoseStamped& current,
|
||||
const robot_geometry_msgs::PoseStamped& start, double heading)
|
||||
{
|
||||
const double dx = current.pose.position.x - start.pose.position.x;
|
||||
const double dy = current.pose.position.y - start.pose.position.y;
|
||||
return dx * std::cos(heading) + dy * std::sin(heading);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Kẹp một lượng vận tốc theo trần gia tốc.
|
||||
* @param target Vận tốc mong muốn (độ lớn, >= 0).
|
||||
* @param current Vận tốc đang phát (độ lớn, >= 0).
|
||||
* @param acc_lim Trần gia tốc [đơn vị/s^2]; <= 0 nghĩa là không giới hạn.
|
||||
* @param dt [s] khoảng cách tới tick trước.
|
||||
* @return Độ lớn vận tốc được phép phát ở tick này.
|
||||
*/
|
||||
inline double rampToward(double target, double current, double acc_lim, double dt)
|
||||
{
|
||||
if (acc_lim <= 0.0 || dt <= 0.0)
|
||||
{
|
||||
return target;
|
||||
}
|
||||
|
||||
const double max_step = acc_lim * dt;
|
||||
if (target > current)
|
||||
{
|
||||
return std::min(target, current + max_step);
|
||||
}
|
||||
return std::max(target, current - max_step);
|
||||
}
|
||||
|
||||
} // namespace recovery_core
|
||||
|
||||
#endif // RECOVERY_CORE_RECOVERY_MATH_H_
|
||||
110
include/recovery_core/recovery_registry.h
Normal file
110
include/recovery_core/recovery_registry.h
Normal file
@@ -0,0 +1,110 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — nạp và giữ danh sách recovery behavior theo YAML.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef RECOVERY_CORE_RECOVERY_REGISTRY_H_
|
||||
#define RECOVERY_CORE_RECOVERY_REGISTRY_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
|
||||
#include <recovery_core/recovery_behavior.h>
|
||||
#include <recovery_core/recovery_context.h>
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
|
||||
/**
|
||||
* @class RecoveryRegistry
|
||||
* @brief Danh sách behavior **có thứ tự**, nạp từ YAML bằng Boost.DLL.
|
||||
*
|
||||
* Thứ tự chính là hành vi: caller thử behavior 0 trước, hỏng thì sang 1, hết danh sách thì abort.
|
||||
* Vì thế đây là `vector` chứ không phải bảng tra theo tên.
|
||||
*
|
||||
* Cấu hình mong đợi (xem `pnkx_nav_core/config/recovery_behaviors_params.yaml`):
|
||||
*
|
||||
* @code{.yaml}
|
||||
* recovery:
|
||||
* behaviors:
|
||||
* - {name: wait, type: WaitRecovery}
|
||||
* - {name: rotate, type: RotateRecovery}
|
||||
* wait:
|
||||
* wait_duration: 3.0
|
||||
* rotate:
|
||||
* angular_speed: 0.4
|
||||
*
|
||||
* WaitRecovery: { library_path: librecovery_core_wait_recovery }
|
||||
* RotateRecovery: { library_path: librecovery_core_rotate_recovery }
|
||||
* @endcode
|
||||
*
|
||||
* Khoá `library_path` là thứ hay quên nhất: thiếu nó thì plugin build xong vẫn báo "không tìm
|
||||
* thấy". Registry vì thế báo lỗi nêu **đích danh** khoá bị thiếu.
|
||||
*/
|
||||
class RecoveryRegistry
|
||||
{
|
||||
public:
|
||||
RecoveryRegistry() = default;
|
||||
~RecoveryRegistry();
|
||||
|
||||
RecoveryRegistry(const RecoveryRegistry&) = delete;
|
||||
RecoveryRegistry& operator=(const RecoveryRegistry&) = delete;
|
||||
|
||||
/**
|
||||
* @brief Nạp toàn bộ behavior khai trong `<ns>/behaviors`, theo đúng thứ tự khai báo.
|
||||
* @param nh NodeHandle gốc.
|
||||
* @param ns Namespace chứa danh sách (mặc định `recovery`).
|
||||
* @param ctx Ngữ cảnh cấp cho mọi behavior khi `configure()`.
|
||||
* @return false nếu có bất kỳ behavior nào không nạp được. Các behavior còn lại **vẫn** được giữ
|
||||
* (một đường phục hồi hỏng không nên xoá sạch các đường còn lại), và mỗi lỗi được log kèm
|
||||
* lý do cụ thể.
|
||||
*/
|
||||
bool loadFromConfig(robot::NodeHandle& nh, const std::string& ns, const RecoveryContext& ctx);
|
||||
|
||||
/**
|
||||
* @brief Thêm một behavior đã dựng sẵn vào cuối danh sách (test, hoặc behavior biên dịch thẳng
|
||||
* vào host). Caller tự chịu trách nhiệm đã `configure()` nó.
|
||||
* @return false nếu con trỏ null.
|
||||
*/
|
||||
bool registerBehavior(const RecoveryBehavior::RecoveryBehaviorPtr& behavior);
|
||||
|
||||
std::size_t size() const
|
||||
{
|
||||
return behaviors_.size();
|
||||
}
|
||||
|
||||
/// @brief Behavior thứ @p index, hoặc nullptr nếu index sai.
|
||||
RecoveryBehavior* at(std::size_t index) const;
|
||||
|
||||
/// @brief Tên behavior thứ @p index, hoặc chuỗi rỗng nếu index sai.
|
||||
std::string nameAt(std::size_t index) const;
|
||||
|
||||
void clear();
|
||||
|
||||
private:
|
||||
/// Nạp một behavior. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
|
||||
bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh,
|
||||
const RecoveryContext& ctx, const std::string& ns);
|
||||
|
||||
std::vector<RecoveryBehavior::RecoveryBehaviorPtr> behaviors_;
|
||||
|
||||
/**
|
||||
* Giữ factory của Boost.DLL sống đúng bằng vòng đời registry.
|
||||
*
|
||||
* Đây **không** phải biến thừa: factory nắm `shared_library` bên trong, thả nó ra là `.so` bị
|
||||
* unload trong khi các behavior tạo từ nó vẫn còn sống — vtable trỏ vào vùng nhớ đã gỡ. Thứ tự
|
||||
* huỷ trong `clear()` cũng vì lý do đó: behavior chết trước, factory chết sau.
|
||||
*/
|
||||
std::vector<std::function<RecoveryBehavior::RecoveryBehaviorPtr()>> factories_;
|
||||
};
|
||||
|
||||
} // namespace recovery_core
|
||||
|
||||
#endif // RECOVERY_CORE_RECOVERY_REGISTRY_H_
|
||||
@@ -2,7 +2,7 @@
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* recovery_core — kiểu hợp đồng output cho recovery behaviors.
|
||||
* recovery_core — kiểu hợp đồng goal/output cho recovery behaviors.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
@@ -10,16 +10,14 @@
|
||||
#define RECOVERY_CORE_RECOVERY_TYPES_H_
|
||||
|
||||
#include <map>
|
||||
#include <optional>
|
||||
#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; }
|
||||
#include <recovery_core/recovery_context.h>
|
||||
|
||||
namespace recovery_core
|
||||
{
|
||||
@@ -37,55 +35,47 @@ enum class RecoveryStatus
|
||||
kCancelled ///< Bị caller huỷ giữa chừng (trả stop output).
|
||||
};
|
||||
|
||||
const char* toString(RecoveryStatus status);
|
||||
|
||||
/**
|
||||
* @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.
|
||||
* @brief Họ output của behavior. Quyết định trường nào trong RecoveryResult đọc được.
|
||||
*
|
||||
* Đây **không** phải cờ tự do cho từng kết quả: mỗi plugin khai một lần qua
|
||||
* `RecoveryBehavior::outputKind()`, và base cưỡng chế mọi kết quả trả về phải khớp. Trước kia
|
||||
* `output_type` do plugin tự đặt mỗi tick nên caller không route theo nó đượ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.
|
||||
kNone, ///< Không output (vd: clear costmap, wait) — chỉ đọc status.
|
||||
kVelocity, ///< Output là Twist — họ rotate/backup.
|
||||
kPath ///< Output là Path — họ sinh lại đường đi.
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct RecoveryContext
|
||||
* @brief Ngữ cảnh môi trường cấp cho behavior một lần qua configure().
|
||||
*
|
||||
* 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ộ.
|
||||
};
|
||||
const char* toString(RecoveryOutputType kind);
|
||||
|
||||
/**
|
||||
* @struct RecoveryGoal
|
||||
* @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào start(goal).
|
||||
* @brief Mục tiêu RUNTIME cho một lượt recovery — caller truyền vào `start(goal, now)`.
|
||||
*
|
||||
* Đâ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().
|
||||
* Cùng một plugin phục vụ nhiều yêu cầu khác nhau mà không phải đổi config.
|
||||
*
|
||||
* 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].
|
||||
* `std::optional` chứ **không** dùng quy ước "0 nghĩa là dùng default": `angle = 0` là một yêu cầu
|
||||
* hợp lệ ("đừng quay") và phải phân biệt được với "caller không đặt". Bản cũ dùng sentinel 0 nên
|
||||
* một góc tính ra ~0 từ hình học bị âm thầm thay bằng π/2 — robot quay 90° mà không log gì.
|
||||
*/
|
||||
struct RecoveryGoal
|
||||
{
|
||||
double angle = 0.0; ///< rad — góc quay đích. 0 = dùng default plugin.
|
||||
double velocity = 0.0; ///< m/s — tốc độ di chuyển. 0 = dùng default plugin.
|
||||
double distance = 0.0; ///< m — khoảng lùi đích. 0 = dùng default plugin.
|
||||
RecoveryTrigger trigger = RecoveryTrigger::kUnspecified; ///< Vì sao vào recovery.
|
||||
|
||||
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::optional<double> angle; ///< [rad] có dấu, + là ngược chiều kim đồng hồ.
|
||||
std::optional<double> distance; ///< [m] > 0, độ dài quãng đi (backup: quãng lùi).
|
||||
|
||||
std::map<std::string, double> params; ///< Override mở rộng theo từng plugin (vd tốc độ).
|
||||
std::optional<robot_geometry_msgs::PoseStamped> target_pose; ///< Pose đích (họ path).
|
||||
|
||||
/// @brief Đọc override double trong params, trả default nếu không có.
|
||||
std::map<std::string, double> params; ///< Override mở rộng theo từng plugin.
|
||||
|
||||
/// @brief Đọc override double trong params, trả @p fallback nếu không có.
|
||||
double param(const std::string& key, double fallback) const
|
||||
{
|
||||
const auto it = params.find(key);
|
||||
@@ -95,45 +85,61 @@ struct RecoveryGoal
|
||||
|
||||
/**
|
||||
* @struct RecoveryResult
|
||||
* @brief Kết quả hợp nhất + rich feedback cho cả 3 họ recovery.
|
||||
* @brief Kết quả một tick, hợp nhất cho cả ba họ + feedback cho caller giám sát.
|
||||
*
|
||||
* 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.
|
||||
* Bất biến output được **base cưỡng chế**, không chỉ ghi trong comment: `output_type` luôn thuộc
|
||||
* `{outputKind(), kNone}`. Đọc dữ liệu qua @ref velocity / @ref pathOut để không thể đọc nhầm
|
||||
* trường của họ khác.
|
||||
*
|
||||
* 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).
|
||||
* Feedback (`progress`/`remaining`/`elapsed`/`message`) luôn hợp lệ, độc lập với `output_type`.
|
||||
* `progress` trong [0,1]; `remaining` theo đơn vị của goal (rad hoặc m); `elapsed` [s] do base ghi.
|
||||
*/
|
||||
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.
|
||||
robot_geometry_msgs::Twist command; ///< [m/s],[rad/s]. Chỉ hợp lệ khi output_type == kVelocity.
|
||||
robot_nav_msgs::Path path; ///< Chỉ 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").
|
||||
double elapsed = 0.0; ///< [s] thời gian trôi từ start(). Base ghi, plugin không phải đặt.
|
||||
std::string message; ///< Mô tả người-đọc-được, dùng để log khi state đổi.
|
||||
|
||||
/// @brief Twist nếu kết quả này thực sự là output vận tốc, ngược lại nullptr.
|
||||
const robot_geometry_msgs::Twist* velocity() const
|
||||
{
|
||||
return output_type == RecoveryOutputType::kVelocity ? &command : nullptr;
|
||||
}
|
||||
|
||||
/// @brief Path nếu kết quả này thực sự là output path, ngược lại nullptr.
|
||||
const robot_nav_msgs::Path* pathOut() const
|
||||
{
|
||||
return output_type == RecoveryOutputType::kPath ? &path : nullptr;
|
||||
}
|
||||
|
||||
/// @brief true nếu lượt recovery đã kết thúc (không cần tick thêm).
|
||||
bool terminal() const
|
||||
{
|
||||
return status != RecoveryStatus::kRunning;
|
||||
}
|
||||
|
||||
/// @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).
|
||||
/// @brief Thất bại, không output (caller dừng an toàn).
|
||||
static RecoveryResult Failed();
|
||||
/// @brief Bị huỷ, không output (caller nên dừng an toàn).
|
||||
/// @brief Bị huỷ, không output (caller dừng an toàn).
|
||||
static RecoveryResult Cancelled();
|
||||
/// @brief Output vận tốc kèm status (kRunning/kSucceeded/kFailed/kCancelled).
|
||||
/// @brief Output vận tốc kèm status.
|
||||
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);
|
||||
static RecoveryResult PathOut(const robot_nav_msgs::Path& path, RecoveryStatus status);
|
||||
|
||||
/// @brief Gắn thêm feedback (fluent) — trả về chính nó để chain.
|
||||
/// @brief Gắn thêm feedback (fluent). progress bị kẹp về [0,1], remaining về >= 0.
|
||||
RecoveryResult& withProgress(double progress_value, double remaining_value);
|
||||
/// @brief Gắn message (fluent).
|
||||
RecoveryResult& withMessage(std::string text);
|
||||
|
||||
Reference in New Issue
Block a user