feat(localization): add pose_instability_detector

[SakodaShintaro]

Description

Added pose_instability_detector node.

See README.md for details.

A known problem is that the amount of data published to /diagnostics in the current autoware is too large and can cause it to drop some data.

Tests performed

The following tests have been run and verified

• run autoware on sample rosbag
• run autoware on AWSIM's rosbag and the error between ekf and GT is the same as before.
• colcon test

# if use lcov, need following libraries
# sudo apt-get install -y lcov python3-colcon-lcov-result

colcon build --symlink-install \
  --cmake-args \
    -DCMAKE_BUILD_TYPE=Release \
    -DBUILD_TESTING=ON \
    -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage' \
    -DCMAKE_C_FLAGS='-fprofile-arcs -ftest-coverage' \
  --packages-select pose_instability_detector
colcon test --return-code-on-test-failure --packages-select pose_instability_detector
colcon lcov-result --packages-select pose_instability_detector
colcon test-result --all --verbose | grep pose_instability_detector

A directory named lcov will be created under ~/autoware, where coverage can be checked.

Result

The following results are obtained when run in AWSIM's rosbag.

Effects on system behavior

An output to check the stability of the EKF output values will be added to the diagnostics.

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.

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.

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

[YamatoAndo]

@SakodaShintaro Please update the rqt_runtime_monitor.png to its latest state just before merging.

[Motsu-san]

[SakodaShintaro]

@KYabuuchi @YamatoAndo

Thank you for comments.
Have all the problems been resolved?

It seems that there is a change to launch, so it cannot be merged without the approve of the person who has the authority to do so.
The algorithm of the code has already been reviewed by Sakamoto-san.