fix(ekf_localizer): correct the calculation of delay_step in updateMeasurementPose/Twist

[TaikiYamada4]

Description

In the measurementUpdatePose() and measurementUpdateTwist() of ekf_module.cpp, a variable delay_step, which is used for extracting previous states in the EKF, have been computed by the following.

const int delay_step = std::roundf(delay_time / dt);

Currently, the devisor dt is the measured lap time of the timer_callback, but since ekf_localizer is a single thread node, the lap time of the timer callback may be different than expected. Generally, the lap time gets once longer when other callbacks provoke, and then gets shorter to catch up. This results to incorrect extracting of previous states. Thanks to the damping strategy in the EKF, this bug hasn't been exposed as a critical outcome, but yet it should be solved.

To solve this, I added a variable to accumulated_delay_times_ to store the time how long the previous states in the EKF are old.
With this, we can search the state index that has the closest delay time to that of the measurement, which is the correct delay_step we want.

(Added on 2023/12/04)
I also added a feature to output a WARNING message, if the timer callback period (ekf_dt_) gets too long for some reason.

Related links

To debug these commits, I locally made a debug commit by my self.
https://github.com/TaikiYamada4/autoware.universe/tree/feat/debug_y_ekf_with_accumulated_delay_time

The PR below has the same objective, but since this PR's approach is more simple than the previous one, I'd rather like to merge this PR and the previous PR should be closed if so.
#5587

Tests performed

These changes have been tested through the rosbag replay simulation in the tutorial.
Plus, I have monitored the first element of variable y_ekf in measurementUpdatePose() which suffers from the mentioned bug. This is monitored in the same rosbag replay simulation.
(Extracting values of y_ekf is not included in this PR and done by my local branch)

Despite this value should behave in a continuous manner, there are spikes due to the bug mentioned here (Upper figure).
After applying the changes in this PR, they will disappear (Lower figure).

Notes for reviewers

Interface changes

None

Effects on system behavior

The predicted observation in the EKF should not look up to wrong previous states.
However, since the smoothing strategy in the EKF have been relieving the effects of the bug, it hardly makes any changes to the outputs in a macro manner.

Maybe this bug fix benefits the system when the timer callback in the ekf_localizer is unstable.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Attention: 22 lines in your changes are missing coverage. Please review.

Comparison is base (765a596) 15.32% compared to head (89b85b7) 44.10%.
Report is 165 commits behind head on main.

Files	Patch %	Lines
localization/ekf_localizer/src/ekf_module.cpp	40.00%	6 Missing and 6 partials ⚠️
localization/ekf_localizer/src/ekf_localizer.cpp	20.00%	3 Missing and 5 partials ⚠️
localization/ekf_localizer/src/warning_message.cpp	66.66%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #5691       +/-   ##
===========================================
+ Coverage   15.32%   44.10%   +28.78%     
===========================================
  Files        1721       25     -1696     
  Lines      118559      993   -117566     
  Branches    37995      531    -37464     
===========================================
- Hits        18169      438    -17731     
+ Misses      79657      123    -79534     
+ Partials    20733      432    -20301     

Flag	Coverage Δ
differential	44.10% <38.88%> (?)
total	?

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[TaikiYamada4]

I also added a feature to output a WARNING message, if the timer callback period (ekf_dt_) gets too long for some reason.