63 lines
1.9 KiB
C++
63 lines
1.9 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — path output recovery plugin (goal-driven, one-shot).
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_plugins
|
|
{
|
|
|
|
/**
|
|
* @class RegenPathRecovery
|
|
* @brief Họ A (path output), one-shot: trả lại robot_nav_msgs::Path từ global_path hiện tại.
|
|
*
|
|
* Hoàn tất ngay ở lần update() đầu. Guard chưa configure/global_path null hoặc rỗng -> Failed().
|
|
*/
|
|
class RegenPathRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
RegenPathRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<RegenPathRecovery>();
|
|
}
|
|
|
|
protected:
|
|
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
|
|
{
|
|
// One-shot: công việc thực hiện ở onUpdate() lần đầu, giữ start() gọn.
|
|
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
|
|
recovery_core::RecoveryStatus::kRunning)
|
|
.withMessage("regen path start");
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate(double /*dt*/) override
|
|
{
|
|
const auto* global_path = ctx().global_path;
|
|
if (global_path == nullptr || global_path->empty())
|
|
{
|
|
return recovery_core::RecoveryResult::Failed().withMessage("no global path to regenerate");
|
|
}
|
|
|
|
robot_nav_msgs::Path path;
|
|
path.poses = *global_path;
|
|
|
|
return recovery_core::RecoveryResult::PathOut(path, recovery_core::RecoveryStatus::kSucceeded)
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("regen path complete");
|
|
}
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)
|