fix(pose_instability_detector): fix a rare error

[SakodaShintaro]

Description

In pose_instability_detector, a bug has been fixed that in rare abnormal cases, it may access to outside the queue size and crash, or it may show abnormal values.

I have added test cases that cause this, and I have confirmed that it fails probabilistically if it is not fixed.
Since accessing outside the range is undefined behavior, it does not necessarily always fail.

By making a modification of pose_instability_detector.cpp, the test will no longer fail even after 100 consecutive attempts.

Related links

None.

How was this PR tested?

The following script was used.

./test_loop.sh pose_instability_detector

#!/bin/bash
set -eux

TEST_TARGET_PKG=$1
LOOP_NUM=${2:-100}

colcon build --symlink-install \
  --cmake-args \
    -DCMAKE_BUILD_TYPE=Release \
    -DBUILD_TESTING=ON \
  --packages-select $TEST_TARGET_PKG

for ((i=1; i<=LOOP_NUM; i++))
do
  colcon test --packages-select ${TEST_TARGET_PKG} --event-handlers console_cohesion+ --return-code-on-test-failure
  echo "Finish test $i"
done

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[codecov]

Codecov Report

Attention: Patch coverage is 88.46154% with 3 lines in your changes missing coverage. Please review.

Project coverage is 12.82%. Comparing base (507e3f4) to head (d8e27de).
Report is 171 commits behind head on main.

Files	Patch %	Lines
...ability_detector/src/pose_instability_detector.cpp	25.00%	2 Missing and 1 partial ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #7681       +/-   ##
===========================================
- Coverage   14.84%   12.82%    -2.03%     
===========================================
  Files        1999       34     -1965     
  Lines      139163     2043   -137120     
  Branches    43716      201    -43515     
===========================================
- Hits        20661      262    -20399     
+ Misses      95731     1768    -93963     
+ Partials    22771       13    -22758     

Flag	Coverage Δ
differential	12.82% <88.46%> (?)
total	?

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

[xmfcx]

@SakodaShintaro could you rebase to the latest main?

I want to see if the fix on comment-on-pr workflow issue is fixed.

• ci(comment-on-pr): make it work on forks #7680

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[TaikiYamada4]

After all, I think the dead reckon process should be skipped when the time of latest and prev is same, and publish a WARN diagnostics. (And then I believe the size of result_deque will be always be > 2.)

But I will mark that as a TODO for me, and I can approve the cpp part.
Can you take a look to my comment in the test part? 🙏

[TaikiYamada4]

LGTM! 👍