fix(ekf_localizer): rosbag clock jump bug (tier4#646)

* fix(ekf_localizer): rosbag clock jump bug

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -116,15 +116,19 @@ 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<const rclcpp::Time>(get_clock()->now());
 }