fix(obstacle_avoidance_planner): add empty check (backport #1285)

[mergify]

Description

走行中に、motion_planning_containerが落ちることによって、EMが発生する問題の対策PRとなる。
今回の対策によって、稼働を継続できる条件において、EMが発生せずに、稼働を継続する。

原因

空データに対してスプライン補間をした場合に例外が発生する事が分かった。

#0  __pthread_kill_implementation (no_tid=0, signo=6, threadid=140599944683328)
    at ./nptl/pthread_kill.c:44
#1  __pthread_kill_internal (signo=6, threadid=140599944683328)
    at ./nptl/pthread_kill.c:78
#2  __GI___pthread_kill (threadid=140599944683328, signo=signo@entry=6)
    at ./nptl/pthread_kill.c:89
#3  0x00007fdff9f21476 in __GI_raise (sig=sig@entry=6)
    at ../sysdeps/posix/raise.c:26
#4  0x00007fdff9f077f3 in __GI_abort () at ./stdlib/abort.c:79
#5  0x00007fdffa1ccb9e in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007fdffa1d820c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007fdffa1d8277 in std::terminate() ()
   from /lib/x86_64-linux-gnu/libstdc++.so.6
#8  0x00007fdffa1d84d8 in __cxa_throw ()
   from /lib/x86_64-linux-gnu/libstdc++.so.6
#9  0x00007fdfee95d366 in interpolation_utils::validateKeys ()
    at /home/autoware/autoware.proj/src/autoware/universe/common/interpolation/include/interpolation/interpolation_utils.hpp:75
#10 0x00007fdfee95dc36 in SplineInterpolation::getSplineInterpolatedValues ()
    at /home/autoware/autoware.proj/src/autoware/universe/common/interpolation/src/spline_interpolation.cpp:152
#11 0x00007fdfee95f1bb in interpolation::slerp ()
    at /home/autoware/autoware.proj/src/autoware/universe/common/interpolation/src/spline_interpolation.cpp:94
#12 0x00007fdfeea9a3a6 in interpolation_utils::interpolate2DTrajectoryPoints () at /home/autoware/autoware.proj/src/autoware/universe/planning/obstacle_avoidance_planner/src/utils.cpp:346

上記ログからinterpolation_utils::validateKeys関数で例外が発生していることがわかる。
実装を確認したところ、同関数のthrowに対するtry-catchが存在していないことが判明した。

対策

例外発生部でtry-catchを追加し、例外発生時には経路の補間を実施せず、補間がない経路出力を後段に与える。

想定される影響

• 後続処理としては，PathPointに対してスプライン補間の経路予測結果と比較し、差分についてyaw方向への補整を実施する。この経路予測が、空データとなる事で、yaw方向への補正が実施されなくなる。
  スプライン補間による補整が実施されないだけで、planningの経路計画等は実施さるれため、リグレッションはないと判断する

• 該当項目が継続して空データの場合に、データのタイムアウトなどが発生する可能性がある。
  ただし、問題となったデータを利用して、走行を継続できたため、実績ベースでは問題はおきていない。

Related links

https://tier4.atlassian.net/browse/AEAP-1142

Tests performed

機能評価

• interpolate2DTrajectoryPoints関数について、複雑な構成となっており、単一関数として確認は困難になるため、
  　関数単体でのテストではなく、例外が発生するrosbagを利用した機能評価により、問題の改善を確認する。

• 問題が発生した1件のrosbagを再生した時に、"process died"のメッセージが表示されない事を確認済み

before (N=3, process has died = 3)

2024-04-22T11:27:21.756406+09:00 autoware-ecu run.sh[166368]: [component_container_mt-24] terminate called after throwing an instance of 'std::invalid_argument'
2024-04-22T11:27:21.756576+09:00 autoware-ecu run.sh[166368]: [component_container_mt-24]   what():  Points is empty.

2024-04-22T11:27:23.697840+09:00 autoware-ecu run.sh[166368]: [ERROR] [component_container_mt-24]: process has died [pid 166751, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args -r __node:=motion_planning_container -r __ns:=/planning/scenario_planning/lane_driving/motion_planning -p use_sim_time:=False -p wheel_radius:=0.258 -p wheel_width:=0.145 -p wheel_base:=1.335 -p wheel_tread:=0.955 -p front_overhang:=0.53 -p rear_overhang:=0.375 -p left_overhang:=0.0725 -p right_overhang:=0.0725 -p vehicle_height:=1.87 -p max_steer_angle:=0.64'].

after (N=3, process has died = 0)

[component_container_mt-11] [WARN] [1715233036.458925767] [util]: Points is empty.

リグレッション評価

• 問題のあった経路を走行した時に、EMが発生しない事を確認済み

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.

---

This is an automatic backport of pull request #1285 done by [Mergify](https://mergify.com).

[kimurariku]

git diff beta/v0.3.18.1 mergify/bp/fix/add_empty_check/pr-1285 planning/obstacle_avoidance_planner/src/utils.cpp で差分がない確認は出来ました。
ここ他に確認しておくことはありますか

[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