fix(ekf_localizer): correct the calculation of delay_step in updateMe…

…asurementPose/Twist (autowarefoundation#5691)

* Added X_delay_times_ to obtain the accumulated lap times of the timer callback.
Added find_closest_index function to easily use the X_delay_times_.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Added a concept/variable of accumulated_delay_time_ and store every lap time of timer callback.
The delay_step will be calculated from it, and the bug of calculating delay_step should be gone.
Besides, removed dt in measurementUpdatePose/Twist since it is not needed.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Fixed dt to ekf_dt_ in predictUpdateFrequency()

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Removed temporary debug stuff

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Fixed code style pointed out from pre-commit.ci

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Fixed typo

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Removed variable ekf_rate_ which is currently unused.
Added warnings when the ekf_dt_ is too large.

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

* Changed threshold of delay time so that to look the most last (or largest) value of accumulated_delay_times_

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* Correct the warning messages of diag_info to look up the last (or largest) value of accumulated_delay_times_

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: TaikiYamada4 <taiki.yamada@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -155,7 +155,6 @@ class EKFLocalizer : public rclcpp::Node
 
   const HyperParameters params_;
 
-  double ekf_rate_;
   double ekf_dt_;
 
   /* process noise variance for discrete model */

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -30,6 +30,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 
 #include <memory>
+#include <vector>
 
 struct EKFDiagnosticInfo
 {
@@ -76,12 +77,15 @@ class EKFModule
   std::array<double, 36> getCurrentPoseCovariance() const;
   std::array<double, 36> getCurrentTwistCovariance() const;
 
+  size_t find_closest_delay_time_index(double target_value) const;
+  void accumulate_delay_time(const double dt);
+
   void predictWithDelay(const double dt);
   bool measurementUpdatePose(
-    const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr,
+    const PoseWithCovariance & pose, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & pose_diag_info);
   bool measurementUpdateTwist(
-    const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr,
+    const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
   geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay(
     const PoseWithCovariance & pose, const double delay_time);
@@ -91,6 +95,7 @@ class EKFModule
 
   std::shared_ptr<Warning> warning_;
   const int dim_x_;
+  std::vector<double> accumulated_delay_times_;
   const HyperParameters params_;
 };
 

localization/ekf_localizer/include/ekf_localizer/warning_message.hpp

@@ -17,10 +17,9 @@
 
 #include <string>
 
-std::string poseDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt);
+std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold);
 std::string twistDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt);
+  const double delay_time, const double delay_time_threshold);
 std::string poseDelayTimeWarningMessage(const double delay_time);
 std::string twistDelayTimeWarningMessage(const double delay_time);
 std::string mahalanobisWarningMessage(const double distance, const double max_distance);

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -44,7 +44,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
 : rclcpp::Node(node_name, node_options),
   warning_(std::make_shared<Warning>(this)),
   params_(this),
-  ekf_rate_(params_.ekf_rate),
   ekf_dt_(params_.ekf_dt),
   pose_queue_(params_.pose_smoothing_steps),
   twist_queue_(params_.twist_smoothing_steps)
@@ -109,9 +108,22 @@ void EKFLocalizer::updatePredictFrequency()
     if (get_clock()->now() < *last_predict_time_) {
       warning_->warn("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);
+      /* Measure dt */
+      ekf_dt_ = (get_clock()->now() - *last_predict_time_).seconds();
+      DEBUG_INFO(
+        get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_);
+
+      if (ekf_dt_ > 10.0) {
+        ekf_dt_ = 10.0;
+        RCLCPP_WARN(
+          get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_);
+      } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) {
+        RCLCPP_WARN(
+          get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_);
+      }
+
+      /* Register dt and accumulate time delay */
+      ekf_module_->accumulate_delay_time(ekf_dt_);
 
       /* Update discrete proc_cov*/
       proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
@@ -165,7 +177,7 @@ void EKFLocalizer::timerCallback()
     const size_t n = pose_queue_.size();
     for (size_t i = 0; i < n; ++i) {
       const auto pose = pose_queue_.pop_increment_age();
-      bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_);
+      bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_);
       if (is_updated) {
         pose_is_updated = true;
 
@@ -200,8 +212,7 @@ void EKFLocalizer::timerCallback()
     const size_t n = twist_queue_.size();
     for (size_t i = 0; i < n; ++i) {
       const auto twist = twist_queue_.pop_increment_age();
-      bool is_updated =
-        ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_);
+      bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_);
       if (is_updated) {
         twist_is_updated = true;
       }

localization/ekf_localizer/src/ekf_module.cpp

@@ -38,6 +38,7 @@
 EKFModule::EKFModule(std::shared_ptr<Warning> warning, const HyperParameters params)
 : warning_(std::move(warning)),
   dim_x_(6),  // x, y, yaw, yaw_bias, vx, wz
+  accumulated_delay_times_(params.extend_state_step, 1.0E15),
   params_(params)
 {
   Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);
@@ -131,6 +132,48 @@ double EKFModule::getYawBias() const
   return kalman_filter_.getLatestX()(IDX::YAWB);
 }
 
+size_t EKFModule::find_closest_delay_time_index(double target_value) const
+{
+  // If target_value is too large, return last index + 1
+  if (target_value > accumulated_delay_times_.back()) {
+    return accumulated_delay_times_.size();
+  }
+
+  auto lower = std::lower_bound(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value);
+
+  // If the lower bound is the first element, return its index.
+  // If the lower bound is beyond the last element, return the last index.
+  // If else, take the closest element.
+  if (lower == accumulated_delay_times_.begin()) {
+    return 0;
+  } else if (lower == accumulated_delay_times_.end()) {
+    return accumulated_delay_times_.size() - 1;
+  } else {
+    // Compare the target with the lower bound and the previous element.
+    auto prev = lower - 1;
+    bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value);
+
+    // Return the index of the closer element.
+    return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev)
+                             : std::distance(accumulated_delay_times_.begin(), lower);
+  }
+}
+
+void EKFModule::accumulate_delay_time(const double dt)
+{
+  // Shift the delay times to the right.
+  std::copy_backward(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1,
+    accumulated_delay_times_.end());
+
+  // Add the new delay time to all elements.
+  accumulated_delay_times_.front() = 0.0;
+  for (auto & delay_time : accumulated_delay_times_) {
+    delay_time += dt;
+  }
+}
+
 void EKFModule::predictWithDelay(const double dt)
 {
   const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();
@@ -147,8 +190,7 @@ void EKFModule::predictWithDelay(const double dt)
 }
 
 bool EKFModule::measurementUpdatePose(
-  const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr,
-  EKFDiagnosticInfo & pose_diag_info)
+  const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info)
 {
   if (pose.header.frame_id != params_.pose_frame_id) {
     warning_->warnThrottle(
@@ -170,14 +212,15 @@ bool EKFModule::measurementUpdatePose(
 
   delay_time = std::max(delay_time, 0.0);
 
-  const int delay_step = std::roundf(delay_time / dt);
+  const int delay_step = static_cast<int>(find_closest_delay_time_index(delay_time));
 
   pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time);
-  pose_diag_info.delay_time_threshold = params_.extend_state_step * dt;
+  pose_diag_info.delay_time_threshold = accumulated_delay_times_.back();
   if (delay_step >= params_.extend_state_step) {
     pose_diag_info.is_passed_delay_gate = false;
     warning_->warnThrottle(
-      poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000);
+      poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold),
+      2000);
     return false;
   }
 
@@ -243,7 +286,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela
 }
 
 bool EKFModule::measurementUpdateTwist(
-  const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr,
+  const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
   EKFDiagnosticInfo & twist_diag_info)
 {
   if (twist.header.frame_id != "base_link") {
@@ -262,14 +305,16 @@ bool EKFModule::measurementUpdateTwist(
   }
   delay_time = std::max(delay_time, 0.0);
 
-  const int delay_step = std::roundf(delay_time / dt);
+  const int delay_step = static_cast<int>(find_closest_delay_time_index(delay_time));
 
   twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time);
-  twist_diag_info.delay_time_threshold = params_.extend_state_step * dt;
+  twist_diag_info.delay_time_threshold = accumulated_delay_times_.back();
   if (delay_step >= params_.extend_state_step) {
     twist_diag_info.is_passed_delay_gate = false;
     warning_->warnThrottle(
-      twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000);
+      twistDelayStepWarningMessage(
+        twist_diag_info.delay_time, twist_diag_info.delay_time_threshold),
+      2000);
     return false;
   }
 

localization/ekf_localizer/src/warning_message.cpp

@@ -18,22 +18,20 @@
 
 #include <string>
 
-std::string poseDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt)
+std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold)
 {
   const std::string s =
     "Pose delay exceeds the compensation limit, ignored. "
-    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
-  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
 }
 
-std::string twistDelayStepWarningMessage(
-  const double delay_time, const int extend_state_step, const double ekf_dt)
+std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold)
 {
   const std::string s =
     "Twist delay exceeds the compensation limit, ignored. "
-    "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]";
-  return fmt::format(s, delay_time, extend_state_step * ekf_dt);
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
 }
 
 std::string poseDelayTimeWarningMessage(const double delay_time)

localization/ekf_localizer/test/test_warning_message.cpp

@@ -21,17 +21,17 @@
 TEST(PoseDelayStepWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(
-    poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(),
+    poseDelayStepWarningMessage(6.0, 4.0).c_str(),
     "Pose delay exceeds the compensation limit, ignored. "
-    "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]");
+    "delay: 6.000[s], limit: 4.000[s]");
 }
 
 TEST(TwistDelayStepWarningMessage, SmokeTest)
 {
   EXPECT_STREQ(
-    twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(),
+    twistDelayStepWarningMessage(10.0, 6.0).c_str(),
     "Twist delay exceeds the compensation limit, ignored. "
-    "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]");
+    "delay: 10.000[s], limit: 6.000[s]");
 }
 
 TEST(PoseDelayTimeWarningMessage, SmokeTest)