fix(ekf_localizer): rosbag clock jump bug

Signed-off-by: YamatoAndo <yamato.ando@gmail.com>
autowarefoundation · Apr 6, 2022 · ca2c576 · ca2c576
1 parent 90ee0ae
commit ca2c576
Showing 1 changed file with 14 additions and 9 deletions.
diff --git 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<const rclcpp::Time>(get_clock()->now());
 }