diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 9c5cb34eab8d6..fa5d9137bee99 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -116,15 +116,20 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti void EKFLocalizer::updatePredictFrequency() { if (last_predict_time_) { - ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); - DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); - ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); - - /* Update discrete proc_cov*/ - proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); - proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); - proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); - proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); + if (get_clock()->now() < *last_predict_time_) { + RCLCPP_WARN(get_logger(), "Detected jump back in time"); + } + else { + ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); + DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); + ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); + + /* Update discrete proc_cov*/ + proc_cov_vx_d_ = std::pow(proc_stddev_vx_c_ * ekf_dt_, 2.0); + proc_cov_wz_d_ = std::pow(proc_stddev_wz_c_ * ekf_dt_, 2.0); + proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c_ * ekf_dt_, 2.0); + proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c_ * ekf_dt_, 2.0); + } } last_predict_time_ = std::make_shared(get_clock()->now()); }