104 lines
3.4 KiB
C++
104 lines
3.4 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — đợi tại chỗ, không phát output.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <cmath>
|
|
#include <string>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_plugins
|
|
{
|
|
namespace
|
|
{
|
|
constexpr double kDefaultWaitDuration = 3.0; // [s]
|
|
constexpr double kMaxWaitDuration = 300.0; // [s] trần vệ sinh cho param cấu hình sai.
|
|
} // namespace
|
|
|
|
/**
|
|
* @class WaitRecovery
|
|
* @brief Đứng yên một khoảng thời gian rồi báo thành công.
|
|
*
|
|
* Đây là recovery **an toàn nhất** trong bộ default và nên đứng đầu danh sách: nó không di chuyển,
|
|
* không cần pose, không cần collision check. Với AMR/AGV trong kho, phần lớn tình huống chặn đường
|
|
* là vật cản động (người, xe khác) — đợi vài giây rồi lập plan lại giải quyết được đa số, trong khi
|
|
* mọi behavior khác đều bắt robot cử động trong lúc chưa biết chuyện gì đang xảy ra.
|
|
*
|
|
* Thời lượng: `goal.params["wait_duration"]` cho lượt này, ngược lại param `wait_duration`.
|
|
*/
|
|
class WaitRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
WaitRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<WaitRecovery>();
|
|
}
|
|
|
|
recovery_core::RecoveryOutputType outputKind() const override
|
|
{
|
|
return recovery_core::RecoveryOutputType::kNone;
|
|
}
|
|
|
|
protected:
|
|
bool onConfigure(robot::NodeHandle& nh) override
|
|
{
|
|
nh.param("wait_duration", default_wait_duration_, kDefaultWaitDuration);
|
|
default_wait_duration_ = sanitizeDuration(default_wait_duration_, kDefaultWaitDuration);
|
|
return true;
|
|
}
|
|
|
|
bool onStart(const recovery_core::RecoveryGoal& goal) override
|
|
{
|
|
wait_duration_ = sanitizeDuration(goal.param("wait_duration", default_wait_duration_),
|
|
default_wait_duration_);
|
|
return true;
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
|
|
{
|
|
// elapsed() do base đo bằng đồng hồ thật, nên loop chạy chậm không làm sai thời lượng đợi.
|
|
const double waited = elapsed();
|
|
|
|
if (waited >= wait_duration_)
|
|
{
|
|
return recovery_core::RecoveryResult::Succeeded()
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("wait complete");
|
|
}
|
|
|
|
return recovery_core::RecoveryResult::Running()
|
|
.withProgress(waited / wait_duration_, wait_duration_ - waited)
|
|
.withMessage("waiting");
|
|
}
|
|
|
|
private:
|
|
double sanitizeDuration(double value, double fallback) const
|
|
{
|
|
if (!std::isfinite(value) || value <= 0.0 || value > kMaxWaitDuration)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': wait_duration=%.3f s outside (0, %.0f]; using %.3f "
|
|
"s.",
|
|
name().c_str(), value, kMaxWaitDuration, fallback);
|
|
return fallback;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
double default_wait_duration_ = kDefaultWaitDuration; ///< [s]
|
|
double wait_duration_ = kDefaultWaitDuration; ///< [s] mục tiêu lượt này
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::WaitRecovery::create, WaitRecovery)
|