fix(pose_instability_detector): fix a crash bug in pose_instability_detector

[SakodaShintaro]

Description

Cherry-pick of the following PR

• fix(pose_instability_detector): fix a rare error autowarefoundation/autoware.universe#7681

This seems to occur when performing initial position estimation twice, as the EKF is lost during the second iteration.

Error

1719899212.4359140 [ERROR] [pose_instability_detector_node-77]: process has died [pid 97654, exit code -6, cmd '/home/autoware/pilot-auto.xx1/install/pose_instability_detector/lib/pose_instability_detector/pose_instability_detector_node --ros-args -r __node:=pose_instability_detector -r __ns:=/localization/pose_twist_fusion_filter -p use_sim_time:=False -p wheel_radius:=0.31 -p wheel_width:=0.18 -p wheel_base:=2.75 -p wheel_tread:=1.485 -p front_overhang:=0.8 -p rear_overhang:=0.85 -p left_overhang:=0.105 -p right_overhang:=0.105 -p vehicle_height:=2.5 -p max_steer_angle:=0.55 --params-file /home/autoware/pilot-auto.xx1/install/pose_instability_detector/share/pose_instability_detector/config/pose_instability_detector.param.yaml -r ~/input/odometry:=/localization/kinematic_state -r ~/input/twist:=/localization/twist_estimator/twist_with_covariance'].
1719899211.9946663 [pose_instability_detector_node-77] *** Aborted at 1719899211 (unix time) try "date -d @1719899211" if you are using GNU date ***
1719899211.9958658 [pose_instability_detector_node-77] PC: @ 0x0 (unknown)
1719899211.9960396 [pose_instability_detector_node-77] @ 0x7f944d9b74d6 google::(anonymous namespace)::FailureSignalHandler()
1719899211.9972198 [pose_instability_detector_node-77] @ 0x7f944d24d520 (unknown)
1719899211.9973106 [pose_instability_detector_node-77] @ 0x7f944d2a19fc pthread_kill
1719899211.9982655 [pose_instability_detector_node-77] @ 0x7f944d24d476 raise
1719899211.9983547 [pose_instability_detector_node-77] @ 0x7f944d2337f3 abort
1719899211.9993689 [pose_instability_detector_node-77] @ 0x7f944d4f6b9e (unknown)
1719899211.9994493 [pose_instability_detector_node-77] @ 0x7f944d50220c (unknown)
1719899212.0003834 [pose_instability_detector_node-77] @ 0x7f944d502277 std::terminate()
1719899212.0016036 [pose_instability_detector_node-77] @ 0x7f944d5024d8 __cxa_throw
1719899212.0030823 [pose_instability_detector_node-77] @ 0x7f944d81612a _ZN6rclcpp4TimeC2ERKN18builtin_interfaces3msg5Time_ISaIvEEE16rcl_clock_type_e.cold
1719899212.0042346 [pose_instability_detector_node-77] @ 0x7f944be31d9b PoseInstabilityDetector::clip_out_necessary_twist()
1719899212.0043607 [pose_instability_detector_node-77] @ 0x7f944be3292d PoseInstabilityDetector::dead_reckon()
1719899212.0044293 [pose_instability_detector_node-77] @ 0x7f944be33e51 PoseInstabilityDetector::callback_timer()
1719899212.0045168 [pose_instability_detector_node-77] @ 0x7f944be37565 rclcpp::GenericTimer<>::execute_callback()
1719899212.0056338 [pose_instability_detector_node-77] @ 0x7f944d834ffe rclcpp::Executor::execute_any_executable()
1719899212.0068991 [pose_instability_detector_node-77] @ 0x7f944d83bc90 rclcpp::executors::SingleThreadedExecutor::spin()
1719899212.0070331 [pose_instability_detector_node-77] @ 0x55c4b9c4d486 main
1719899212.0081487 [pose_instability_detector_node-77] @ 0x7f944d234d90 (unknown)
1719899212.0092106 [pose_instability_detector_node-77] @ 0x7f944d234e40 __libc_start_main
1719899212.0093455 [pose_instability_detector_node-77] @ 0x55c4b9c4de35 _start

Tests performed

Not applicable.

Effects on system behavior

Not applicable.

Interface changes

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.

[sonarcloud]

Quality Gate passed

Issues
0 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarCloud