feat(pose_instability_detector): change validation algorithm

[TaikiYamada4]

Description

Change of instability detection

Currently, pose_instability_detector compares the EKF pose and dead reckoning (DR) pose with a constant threshold defined in the pose_instability_detector.param.yaml file.

However, since the instability definition should change by the timer period and the vehicle twist, I made a framework for the pose_instability_detector to calculate the threshold of instability by itself, using the characteristics of the localization component. Note that this PR only puts timer period on consideration.

Hence, the pose_instability_detector.param.yaml drastically changed so that the parameters for calculation has been required but not the threshold itself.

Read the README.md for more information.

Change of DR algorithm

Since there was a lack of time span for twist integration, I revised the algorithm so that the twist integration is fully done from the previous EKF pose to the current EKF pose.

Related links

The following link shows the test results using actual driving data (For TIER IV people only)
TIER IV INTERNAL LINK

Tests performed

I have tested this module with several driving data in TIER IV, and check that the /diagnostics seems to output proper values. (For TIER IV people, see the link above.)
I also check that the rqt_runtime_monitor works well.

Currently, pose_instability_detector have output WARNs because of "large twist variance" and "angular twist error due to wrong gyro bias settings" even in normal environments but they appear very rare and only when the angular twist has a large perturbation.

Notes for reviewers

Interface changes

ROS Topic Changes

None

ROS Parameter Changes

Discarded all parameters, and added the following instead.

Name	Default Value	Description
timer_period	0.5	The period of timer_callback (sec).
heading_velocity_maximum	16.667	The maximum of heading velocity (m/s).
heading_velocity_scale_factor_tolerance	3.0	The tolerance of heading velocity scale factor (%).
angular_velocity_maximum	0.523	The maximum of angular velocity (rad/s).
angular_velocity_standard_deviation	0.00175	The standard deviation of angular velocity (rad/s).
angular_velocity_scale_factor_tolerance	0.2	The tolerance of angular velocity scale factor (%).
angular_velocity_bias_tolerance	0.00698	The tolerance of angular velocity bias (rad/s).
pose_estimator_longitudinal_tolerance	0.11	The tolerance of longitudinal position of pose estimator (m).
pose_estimator_lateral_tolerance	0.11	The tolerance of lateral position of pose estimator (m).
pose_estimator_vertical_tolerance	0.11	The tolerance of vertical position of pose estimator (m).
pose_estimator_angular_tolerance	0.00631	The tolerance of roll angle of pose estimator (rad).

Effects on system behavior

This PR should not change the system behavior. However, since the current pose_instability_detector has optimistic threshold values and this PR makes it low, WARN diagnostics from this node may appear respectively more frequently (but yet with low possibility).

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.

[KYabuuchi]

I have not yet reviewed the detailed implementation, but I left some comments on what I noticed quickly.

[TaikiYamada4]

@KYabuuchi
Thank you so much for your reviewing.
I believe I've fixed the dead reckoning algorithm when updating the orientation.

Anyways, I will revert this PR to draft since I have to evaluate this PR by myself since many changes have happened. Plus, I am to remove the consideration of the process noise in dead reckoning and I have to check that out too.

[KYabuuchi]

At first, please fix the failing test. Currently, diagnostics are not published unless both odometry and twist are subscribed to, so it seems the test cannot exit the while loop.

But, I think the current implementation is basically fine. 🙆‍♂️
I will approve once the issues I pointed out are resolved.

[TaikiYamada4]

I've changed calculate_threshold method and dead_reckon method to public so that it can be used by other modules.
With this change, I also changed calculate_threshold to output the threshold values explicitly w/o changing the private variables.

[KYabuuchi]

Looks Good To Me 🙆‍♂️
Thank you for patiently responding to my many requests.