feat(imu_corrector): changed publication algorithm and content of /diagnostics in gyro_bias_estimator

[TaikiYamada4]

Description

Change of publication algorithm

gyro_bias_estimator has been publishing /diagnostics topic through DiagnosticsUpdater.
However, there is also a publication of /diagnostics by force_update() in the timer_callback that makes extra publication.
In addition, this force_update() will be prevented if specific conditions match in timer_callback which leads to inconsistency on publication of /diagnostics.
To solve this problem, I implemented gyro_bias_estimator to publish /diagnostics only through DiagnosticsUpdater.
Plus, since the publication period set to the updater_ had been default (1.0 second), I revised it to be the same to the period of timer_callback.

Change of diagnostic content

I have modified the content of the diagnostic to satisfy the following.

1. Removed key "gyro_bias" since it is a vague concept, and moved most of it to the message field of the /diagnostics.
2. Keep the size of /diagnostics by setting the bias values as NaN when it is not initialized.
3. Add key "is_updated" and "supplement" to inform whether the gyro bias is updated and why if not.

The rqt_runtime_monitor will be like this

Related links

None

Tests performed

I've test this code via rosbag replay simulation tutorial.

Notes for reviewers

I've just set the publication period of /diagnostics same to the timer callback period.
If this doesn't suit the purpose please tell me.

Interface changes

None

Effects on system behavior

• /diagnostics from gyro_bias_estimator should be more periodical than before
• The content of the /diagnostics will be changed as mentioned in Change of diagnostic content

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.

[SakodaShintaro]

Thank you for the pull request!

(1) I think it would be good to be able to set separate values for the processing timer callback period and the diagnostics period.

(2) How about calculating the values output by the diagnostics in the timer callback and organizing them into a struct?

(3) I think it would be good if the summary statuses in the diagnostics clearly corresponded to the branches of the if statement in update_diagnostics.

(4) It would be good to execute force_update at the end of the process timer callback.

(5) Please ensure that is_bias_updated_ is not redundant by showing the exact same state as other variables, and consider removing it if it is.

[codecov]

Codecov Report

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

Comparison is base (f9ed5b0) 14.58% compared to head (d34a6c9) 14.58%.
Report is 4 commits behind head on main.

Files	Patch %	Lines
sensing/imu_corrector/src/gyro_bias_estimator.cpp	0.00%	44 Missing ⚠️
sensing/imu_corrector/src/gyro_bias_estimator.hpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6139      +/-   ##
==========================================
- Coverage   14.58%   14.58%   -0.01%     
==========================================
  Files        1878     1879       +1     
  Lines      128023   128052      +29     
  Branches    37468    37468              
==========================================
  Hits        18673    18673              
- Misses      88376    88405      +29     
  Partials    20974    20974              

Flag	Coverage Δ		*Carryforward flag
differential	19.54% <0.00%> (?)
total	14.58% <ø> (+<0.01%)	⬆️	Carriedforward from f9ed5b0

*This pull request uses carry forward flags. Click here to find out more.

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

[TaikiYamada4]

Regarding to the reviewers feedbacks, I've made these additional changes.

• Make the timer period and diagnostics period independent.
• Make a struct diagnostics_info_ to put diagnostics information together. This makes the update_diagnostics function clear.
• Added validate_gyro_bias() function since the timer_callback gets too long to read.
• Removed is_updated_ and supplement_, and let all string information go to the summary to make things simple.

I believe this will satisfy the requirements. One minor problem is that update_diagnostics() function can be set to const in this case, however I cannot bind a const function by updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); so it remains as a non -const function after all.

[SakodaShintaro]

To improve visibility when viewing with rqt_runtime_monitor, consider making the following two changes:

• Use std::fixed
• Display threshold value

For example, it looks like the following

void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
{
  auto f = [](const double & value) {
    std::stringstream ss;
    ss << std::fixed << value;
    return ss.str();
  };

  stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message);
  stat.add("gyro_bias_x_for_imu_corrector", f(diagnostics_info_.gyro_bias_x_for_imu_corrector));
  stat.add("gyro_bias_y_for_imu_corrector", f(diagnostics_info_.gyro_bias_y_for_imu_corrector));
  stat.add("gyro_bias_z_for_imu_corrector", f(diagnostics_info_.gyro_bias_z_for_imu_corrector));

  stat.add("estimated_gyro_bias_x", f(diagnostics_info_.estimated_gyro_bias_x));
  stat.add("estimated_gyro_bias_y", f(diagnostics_info_.estimated_gyro_bias_y));
  stat.add("estimated_gyro_bias_z", f(diagnostics_info_.estimated_gyro_bias_z));

  stat.add("gyro_bias_threshold", f(gyro_bias_threshold_));
}

When I tried to keep it within the character limit, I ended up with a very short function name :(

If you have any opposing opinions, please let me know.

[TaikiYamada4]

Thank you for the comment!
I followed your recommendation and add the following commit. 👍

[SakodaShintaro]

Looks Good To Me