optimal & fix file cmake
This commit is contained in:
@@ -1,79 +1,167 @@
|
||||
/*********************************************************************
|
||||
* recovery_core — phần chung (template-method) của RecoveryBehavior.
|
||||
*
|
||||
* configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời và xử lý
|
||||
* cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
|
||||
* 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
|
||||
{
|
||||
robot_geometry_msgs::Twist zeroTwist()
|
||||
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 robot_geometry_msgs::Twist();
|
||||
return !value.has_value() || std::isfinite(*value);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx)
|
||||
bool RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx,
|
||||
robot::NodeHandle& nh)
|
||||
{
|
||||
if (configured_)
|
||||
{
|
||||
return;
|
||||
// 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;
|
||||
|
||||
onConfigure();
|
||||
|
||||
configured_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal)
|
||||
bool RecoveryBehavior::start(const RecoveryGoal& goal, const robot::Time& now)
|
||||
{
|
||||
if (!configured_)
|
||||
{
|
||||
status_ = RecoveryStatus::kFailed;
|
||||
return RecoveryResult::Failed().withMessage("start() before configure()");
|
||||
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;
|
||||
started_ = true;
|
||||
start_time_ = now;
|
||||
last_update_ = now;
|
||||
elapsed_ = 0.0;
|
||||
status_ = RecoveryStatus::kRunning;
|
||||
started_ = true;
|
||||
|
||||
RecoveryResult result = onStart(goal_);
|
||||
status_ = result.status;
|
||||
return result;
|
||||
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()
|
||||
RecoveryResult RecoveryBehavior::update(const robot::Time& now)
|
||||
{
|
||||
if (!configured_ || !started_)
|
||||
{
|
||||
status_ = RecoveryStatus::kFailed;
|
||||
return RecoveryResult::Failed().withMessage("update() before start()");
|
||||
return stopResult(RecoveryStatus::kFailed).withMessage("update() before start()");
|
||||
}
|
||||
|
||||
// Đã kết thúc: giữ nguyên trạng thái, không tick thêm.
|
||||
if (status_ != RecoveryStatus::kRunning)
|
||||
{
|
||||
return RecoveryResult::Velocity(zeroTwist(), status_);
|
||||
// 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_)
|
||||
{
|
||||
status_ = RecoveryStatus::kCancelled;
|
||||
return RecoveryResult::Velocity(zeroTwist(), status_)
|
||||
.withMessage("cancelled by caller");
|
||||
RecoveryResult result = finalize(onCancel());
|
||||
status_ = result.status;
|
||||
return result;
|
||||
}
|
||||
|
||||
RecoveryResult result = onUpdate();
|
||||
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;
|
||||
}
|
||||
@@ -83,4 +171,126 @@ 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
|
||||
|
||||
Reference in New Issue
Block a user