fix(interpolation): add check size of query_keys

[sfukuta]

Description

走行中に、motion_planning_containerが落ちることによって、EMが発生する問題の対策PRとなる。

原因

coredumpの結果より、スプライン補間時に利用する点の情報が不足しているため、
存在しないオブジェクト情報に対するアクセスが原因と分かった。

#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

対策

query_keysはサイズチェックを実施し、オブジェクト情報が不足している時はアクセスしないようにする。

Related links

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

Tests performed

機能評価

• 問題のあったrosbagを再生した時に、追加したエラーメッセージがコンソールに出力される事：
• 問題のあったrosbagを再生した時に、EMが発生しない事：

リグレッション評価

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

Notes for reviewers

Interface changes

Effects on system behavior

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.