297 lines
9.0 KiB
C++
297 lines
9.0 KiB
C++
/*********************************************************************
|
|
* recovery_core — phần chung (template-method) của RecoveryBehavior.
|
|
*
|
|
* Base giữ toàn bộ bất biến vòng đời, thời gian, và họ output; plugin chỉ triển khai hook.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <cmath>
|
|
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_core
|
|
{
|
|
namespace
|
|
{
|
|
constexpr double kDefaultTimeout = 0.0; ///< [s] 0 = không giới hạn.
|
|
constexpr double kMaxTimeout = 600.0; ///< [s] trần vệ sinh cho param cấu hình sai.
|
|
|
|
/// Một giá trị optional hợp lệ phải hữu hạn.
|
|
bool finiteIfSet(const std::optional<double>& value)
|
|
{
|
|
return !value.has_value() || std::isfinite(*value);
|
|
}
|
|
} // namespace
|
|
|
|
bool RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx,
|
|
robot::NodeHandle& nh)
|
|
{
|
|
if (configured_)
|
|
{
|
|
// Gọi lại với ctx khác là lỗi lập trình của caller. Bản cũ im lặng return, nên caller không bao
|
|
// giờ biết context thứ hai đã bị bỏ đi.
|
|
robot::log_error("[recovery_core] '%s': configure() called twice, ignored.", name_.c_str());
|
|
return false;
|
|
}
|
|
|
|
if (name.empty())
|
|
{
|
|
robot::log_error("[recovery_core] configure() with an empty instance name.");
|
|
return false;
|
|
}
|
|
|
|
name_ = name;
|
|
ctx_ = ctx;
|
|
|
|
if (!validateContext())
|
|
{
|
|
name_.clear();
|
|
ctx_ = RecoveryContext();
|
|
return false;
|
|
}
|
|
|
|
nh.param("timeout", timeout_, kDefaultTimeout);
|
|
if (!std::isfinite(timeout_) || timeout_ < 0.0 || timeout_ > kMaxTimeout)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': timeout=%.3f s outside [0, %.0f]; using 0 (no "
|
|
"limit).", name_.c_str(), timeout_, kMaxTimeout);
|
|
timeout_ = kDefaultTimeout;
|
|
}
|
|
|
|
if (!onConfigure(nh))
|
|
{
|
|
robot::log_error("[recovery_core] '%s': onConfigure() failed.", name_.c_str());
|
|
name_.clear();
|
|
ctx_ = RecoveryContext();
|
|
return false;
|
|
}
|
|
|
|
status_ = RecoveryStatus::kIdle;
|
|
configured_ = true;
|
|
return true;
|
|
}
|
|
|
|
bool RecoveryBehavior::start(const RecoveryGoal& goal, const robot::Time& now)
|
|
{
|
|
if (!configured_)
|
|
{
|
|
robot::log_error("[recovery_core] start() before configure().");
|
|
return false;
|
|
}
|
|
|
|
if (started_ && status_ == RecoveryStatus::kRunning && !cancel_requested_)
|
|
{
|
|
// Không từ chối: sau cancel(), state machine có thể start lượt mới mà lượt cũ chưa kịp về
|
|
// terminal (nó không tick recovery ở state CANCELLING). Nhưng vẫn phải báo, vì nếu KHÔNG phải
|
|
// đường cancel thì đây là caller đang bỏ dở một behavior đang lái robot.
|
|
robot::log_warning("[recovery_core] '%s': start() while the previous run is still going — "
|
|
"resetting.",
|
|
name_.c_str());
|
|
}
|
|
|
|
if (!validateGoal(goal))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
goal_ = goal;
|
|
cancel_requested_ = false;
|
|
start_time_ = now;
|
|
last_update_ = now;
|
|
elapsed_ = 0.0;
|
|
status_ = RecoveryStatus::kRunning;
|
|
started_ = true;
|
|
|
|
if (!onStart(goal_))
|
|
{
|
|
status_ = RecoveryStatus::kFailed;
|
|
robot::log_warning("[recovery_core] '%s': refused to start (trigger=%s).", name_.c_str(),
|
|
toString(goal_.trigger));
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::update(const robot::Time& now)
|
|
{
|
|
if (!configured_ || !started_)
|
|
{
|
|
status_ = RecoveryStatus::kFailed;
|
|
return stopResult(RecoveryStatus::kFailed).withMessage("update() before start()");
|
|
}
|
|
|
|
if (status_ != RecoveryStatus::kRunning)
|
|
{
|
|
// Lượt đã kết thúc: giữ nguyên kết luận, không tick thêm.
|
|
return stopResult(status_);
|
|
}
|
|
|
|
// dt đo THẬT. Đây là điểm sửa cốt lõi so với bản cũ: bản cũ tích phân vận tốc lệnh nhân với
|
|
// control_period lấy từ config, nên control loop chạy chậm là robot đi quá quãng yêu cầu.
|
|
double dt = (now - last_update_).toSec();
|
|
if (!std::isfinite(dt) || dt < 0.0)
|
|
{
|
|
// Đồng hồ đi lùi (đổi nguồn thời gian, hoặc sim reset). Coi như không có thời gian trôi thay vì
|
|
// tích phân một dt âm vào tiến độ.
|
|
robot::log_warning("[recovery_core] '%s': dt=%.6f s is invalid, treated as 0.", name_.c_str(),
|
|
dt);
|
|
dt = 0.0;
|
|
}
|
|
last_update_ = now;
|
|
elapsed_ = (now - start_time_).toSec();
|
|
if (!std::isfinite(elapsed_) || elapsed_ < 0.0)
|
|
{
|
|
elapsed_ = 0.0;
|
|
}
|
|
|
|
if (cancel_requested_)
|
|
{
|
|
RecoveryResult result = finalize(onCancel());
|
|
status_ = result.status;
|
|
return result;
|
|
}
|
|
|
|
if (timeout_ > 0.0 && elapsed_ >= timeout_)
|
|
{
|
|
status_ = RecoveryStatus::kFailed;
|
|
return stopResult(RecoveryStatus::kFailed)
|
|
.withMessage("exceeded the timeout of " + std::to_string(timeout_) + " s");
|
|
}
|
|
|
|
RecoveryResult result = finalize(onUpdate(now, dt));
|
|
status_ = result.status;
|
|
return result;
|
|
}
|
|
|
|
void RecoveryBehavior::cancel()
|
|
{
|
|
cancel_requested_ = true;
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::onCancel()
|
|
{
|
|
return stopResult(RecoveryStatus::kCancelled).withMessage("cancelled by caller");
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::stopResult(RecoveryStatus status) const
|
|
{
|
|
RecoveryResult result;
|
|
result.status = status;
|
|
result.elapsed = elapsed_;
|
|
|
|
if (outputKind() == RecoveryOutputType::kVelocity)
|
|
{
|
|
// Họ velocity: phát lệnh dừng TƯỜNG MINH. Caller đang lấy cmd_vel từ behavior này nên "không
|
|
// output" và "output vận tốc 0" là hai chuyện khác nhau.
|
|
result.output_type = RecoveryOutputType::kVelocity;
|
|
result.command = robot_geometry_msgs::Twist();
|
|
}
|
|
else
|
|
{
|
|
// Họ khác: KHÔNG bịa ra output vận tốc. Bản cũ trả Velocity(zero) cho mọi họ, nên một behavior
|
|
// clear-costmap báo cáo mình phát vận tốc.
|
|
result.output_type = RecoveryOutputType::kNone;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
bool RecoveryBehavior::validateContext() const
|
|
{
|
|
const RecoveryOutputType kind = outputKind();
|
|
|
|
if (kind == RecoveryOutputType::kVelocity)
|
|
{
|
|
if (ctx_.pose == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': the velocity family requires a PoseProvider — "
|
|
"progress must be measured from a real pose, not dead-reckoned.",
|
|
name_.c_str());
|
|
return false;
|
|
}
|
|
if (ctx_.collision == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': the velocity family requires a CollisionChecker — "
|
|
"driving blind is not allowed.", name_.c_str());
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (kind == RecoveryOutputType::kPath && ctx_.plan == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': the path family requires a PlanProvider.",
|
|
name_.c_str());
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool RecoveryBehavior::validateGoal(const RecoveryGoal& goal) const
|
|
{
|
|
if (!finiteIfSet(goal.angle) || !finiteIfSet(goal.distance))
|
|
{
|
|
robot::log_error("[recovery_core] '%s': goal contains NaN/Inf.", name_.c_str());
|
|
return false;
|
|
}
|
|
|
|
if (goal.distance.has_value() && *goal.distance <= 0.0)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': goal.distance=%.3f m must be > 0.", name_.c_str(),
|
|
*goal.distance);
|
|
return false;
|
|
}
|
|
|
|
for (const auto& entry : goal.params)
|
|
{
|
|
if (!std::isfinite(entry.second))
|
|
{
|
|
robot::log_error("[recovery_core] '%s': goal.params['%s'] is not finite.", name_.c_str(),
|
|
entry.first.c_str());
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
RecoveryResult RecoveryBehavior::finalize(RecoveryResult result) const
|
|
{
|
|
const RecoveryOutputType kind = outputKind();
|
|
|
|
if (result.output_type != kind && result.output_type != RecoveryOutputType::kNone)
|
|
{
|
|
// Plugin trả sai họ. Hạ về kNone thay vì tin theo: caller route bằng output_type, nên một họ
|
|
// sai ở đây là caller đọc nhầm trường.
|
|
robot::log_error("[recovery_core] '%s': returned output_type='%s' but outputKind()='%s'; "
|
|
"downgraded to 'none'.", name_.c_str(), toString(result.output_type), toString(kind));
|
|
result.output_type = RecoveryOutputType::kNone;
|
|
result.command = robot_geometry_msgs::Twist();
|
|
result.path = robot_nav_msgs::Path();
|
|
}
|
|
|
|
if (result.output_type == RecoveryOutputType::kVelocity)
|
|
{
|
|
const robot_geometry_msgs::Twist& cmd = result.command;
|
|
if (!std::isfinite(cmd.linear.x) || !std::isfinite(cmd.linear.y) ||
|
|
!std::isfinite(cmd.linear.z) || !std::isfinite(cmd.angular.x) ||
|
|
!std::isfinite(cmd.angular.y) || !std::isfinite(cmd.angular.z))
|
|
{
|
|
// NaN/Inf lọt ra cmd_vel là lỗi không được phép đi tiếp: đổi thành lệnh dừng + kFailed.
|
|
robot::log_error("[recovery_core] '%s': velocity command contains NaN/Inf — forcing a stop.",
|
|
name_.c_str());
|
|
RecoveryResult stop = stopResult(RecoveryStatus::kFailed);
|
|
stop.message = "velocity command is not finite";
|
|
return stop;
|
|
}
|
|
}
|
|
|
|
result.elapsed = elapsed_;
|
|
return result;
|
|
}
|
|
|
|
} // namespace recovery_core
|