fix(obstacle_avoidance_planner): add empty check #1285

sfukuta · 2024-05-02T06:59:06Z

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の経路計画等は実施さるれため、リグレッションはないと判断する
該当項目が継続して空データの場合に、データのタイムアウトなどが発生する可能性がある。
ただし、問題となったデータを利用して、走行を継続できたため、実績ベースでは問題はおきていない。

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.

yn-mrse · 2024-05-07T03:10:36Z

@sfukuta
機能評価では問題があったrosbagに対する試験を実施されていますが、これは妥当な方法でしょうか？
この試験結果はチャンピオンデータになってはいませんか？

yn-mrse · 2024-05-07T03:11:44Z

ノード単体としての後段処理への影響、およびノード外（モジュール・システム）への影響を明記いただきたいです。

yn-mrse · 2024-05-07T05:25:37Z

https://github.com/tier4/autoware.universe/blob/beta/v0.3.18.1/common/interpolation/include/interpolation/interpolation_utils.hpp#L54-L77

L.349〜L.351のslerp関数自体が他の例外についてもthrowしているので、大前提としてslerpへのtry catchは必要かと思います。

その上で特定条件で異なるふるまいを期待する場合にはtry catchより前にif文を追加する対応で違和感はありませんが、
slerp関数に対するすべての例外に対して等しく同じふるまいを期待するなら、try catchのみで十分かと思います。

sfukuta · 2024-05-08T01:08:51Z

@sfukuta 機能評価では問題があったrosbagに対する試験を実施されていますが、これは妥当な方法でしょうか？この試験結果はチャンピオンデータになってはいませんか？

@yn-mrse 該当データしか再現データがないため、before/afterに試行回数と問題発生の件数を記載しました。

sfukuta · 2024-05-08T01:54:47Z

empty時に、空データをreturnする事で、最後のデータを利用するため、ノード単体・システムとしても、
後続処理にも基本的には影響がない。

@yn-mrse 記載を追記しました。

抜粋
- empty時の処理としては、後続は、最後のデータを利用するため、ノード単体・システムとしても、
  後続処理にも基本的には影響がない。

yn-mrse · 2024-05-08T02:11:49Z

@sfukuta

該当データしか再現データがないため、before/afterに試行回数と問題発生の件数を記載しました。

before(3/3)
after(3/3)

問題再現のrosbagが3個あってその全てに対して各1試行でパスできているのか、それとも1つしかないrosbagに対して3試行したのかがわかるように記載をお願いします。
関数レベルのチェックは実施せず、あえてrosbagのみのチェックを実施している理由についても言及をお願いします。

yn-mrse · 2024-05-08T02:14:17Z

抜粋
empty時の処理としては、後続は、最後のデータを利用するため、ノード単体・システムとしても、
後続処理にも基本的には影響がない。

「基本的には」という表現が必要なのであれば影響がある特定シナリオを明記ください。
最後のデータが極めて古かった場合、後続処理に影響があるのではないでしょうか？影響がない条件があるのであればそれも明記してください。（あるいは最後のデータが極めて古い状態で維持される可能性が極めて低いのであればそのような考察も合わせて記載をお願いします）

planning/obstacle_avoidance_planner/src/utils.cpp

sfukuta · 2024-05-09T04:15:05Z

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

影響について、以下のように修正しました。

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

yn-mrse

下記によりApproveします。

Descriptionの記載内容と変更内容が合致している
テスト方法が妥当であり、その結果も問題がない

Descriptionの記載内容については、発生する可能性のある問題挙動に対するリスクを追記いただきたいです。

Approveはしますが、記載内容追記を追って対応いただけますと幸いです。

* fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

kimurariku · 2024-09-12T09:51:10Z

@Mergifyio backport beta/v0.3.19

mergify · 2024-09-12T09:51:47Z

backport beta/v0.3.19

✅ Backports have been created

#1539 fix(obstacle_avoidance_planner): add empty check (backport #1285) has been created for branch beta/v0.3.19

* fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> (cherry picked from commit 39fe090)

* fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix(system_monitor): extend command line to display (backport autowarefoundation#4553) (#768) fix(system_monitor): extend command line to display (autowarefoundation#4553) Signed-off-by: ito-san <[email protected]> Co-authored-by: ito-san <[email protected]> * feat(behavior_path_planner): resample output path (backport #1604) (#782) feat(behavior_path_planner): resample output path (#1604) * feat(behavior_path_planner): resample output path * update param Signed-off-by: Takayuki Murooka <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> * ci: add dispatch-push-event workflow (#803) * ci: add dispatch-push-event workflow Signed-off-by: Keisuke Shima <[email protected]> * fix: change APP KEY Signed-off-by: Keisuke Shima <[email protected]> * chore: use strategy Signed-off-by: Keisuke Shima <[email protected]> * chore: change trigger Signed-off-by: Keisuke Shima <[email protected]> * pre-commit fixes Signed-off-by: Keisuke Shima <[email protected]> * Update .github/workflows/dispatch-push-event.yaml * Update .github/workflows/dispatch-push-event.yaml * style(pre-commit): autofix * Update .github/workflows/dispatch-push-event.yaml --------- Signed-off-by: Keisuke Shima <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909) * fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing * fix implementation * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): fix base points vanishing and inconsistent lane_ids on the spline interpolated path (#929) * add base points to resampled path in behavior_path * Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)" This reverts commit c80c986. * ci(pre-commit): autofix * fix insert * fix: not interpolate behavior velocity path * Revert "Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)"" This reverts commit e6dd540. --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): fix program command line reading (backport autowarefoundation#5191, autowarefoundation#5430) (#995) * perf(system_monitor): fix program command line reading (autowarefoundation#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan <[email protected]> * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan <[email protected]> --------- Signed-off-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): output command line (autowarefoundation#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Owen-Liuyuxuan <[email protected]> Signed-off-by: takeshi.iwanari <[email protected]> Co-authored-by: Yuxuan Liu <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: takeshi-iwanari <[email protected]> Co-authored-by: Akihisa Nagata <[email protected]> * feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta <[email protected]> * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta <[email protected]> * fix parameter name Signed-off-by: Shigekazu Fukuta <[email protected]> * Adapted from PR autowarefoundation#2647 Signed-off-by: Shigekazu Fukuta <[email protected]> * ci(pre-commit): autofix * fix build error * ci(pre-commit): autofix * remove comment line * remove logic * Delete parameters other than those added this time * ci(pre-commit): autofix * add stop vehicle check * ci(pre-commit): autofix * fix stop velocity threshold * fix engage obstacle clear and stop threshold * ci(pre-commit): autofix --------- Signed-off-by: Shigekazu Fukuta <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(obstacle_avoidance_planner): add empty check (#1285) * fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(virtual traffic light): suppress lauch (#1290) * suppress launch Signed-off-by: Yuki Takagi <[email protected]> * add existence check Signed-off-by: Yuki Takagi <[email protected]> --------- Signed-off-by: Yuki Takagi <[email protected]> Co-authored-by: Shigekazu Fukuta <[email protected]> * revert: #929,#909,#782 * revert: #1068 - d56c191. --------- Signed-off-by: ito-san <[email protected]> Signed-off-by: Takayuki Murooka <[email protected]> Signed-off-by: Keisuke Shima <[email protected]> Signed-off-by: Owen-Liuyuxuan <[email protected]> Signed-off-by: takeshi.iwanari <[email protected]> Signed-off-by: Shigekazu Fukuta <[email protected]> Signed-off-by: Yuki Takagi <[email protected]> Co-authored-by: Hiroki OTA <[email protected]> Co-authored-by: ito-san <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> Co-authored-by: Keisuke Shima <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yuxuan Liu <[email protected]> Co-authored-by: Owen-Liuyuxuan <[email protected]> Co-authored-by: takeshi-iwanari <[email protected]> Co-authored-by: Shigekazu Fukuta <[email protected]> Co-authored-by: Yuki TAKAGI <[email protected]>

Shigekazu Fukuta and others added 2 commits May 2, 2024 15:51

fix(obstacle_avoidance_planner): add empty check

2e37262

ci(pre-commit): autofix

d5907ca

sfukuta requested review from takayuki5168, yn-mrse, asa-naki and YoheiMishina May 2, 2024 07:07

Shigekazu Fukuta added 2 commits May 8, 2024 13:06

add invalid_argument

84559a4

delete empty check

153a115

yn-mrse reviewed May 9, 2024

View reviewed changes

planning/obstacle_avoidance_planner/src/utils.cpp Show resolved Hide resolved

yn-mrse reviewed May 9, 2024

View reviewed changes

planning/obstacle_avoidance_planner/src/utils.cpp Outdated Show resolved Hide resolved

Shigekazu Fukuta added 3 commits May 9, 2024 14:16

return code moved to end

f0041d3

add warning log

bacba67

update rclcpp_debug

52e02e9

takayuki5168 approved these changes May 13, 2024

View reviewed changes

Shigekazu Fukuta and others added 3 commits May 13, 2024 15:42

delete debug log

57fe51e

Delete unnecessary blank lines

a811487

Merge branch 'beta/v0.3.18.1' into hotfix/v0.3.18/add_empty_check

e870302

asa-naki approved these changes May 31, 2024

View reviewed changes

yn-mrse approved these changes Jun 5, 2024

View reviewed changes

sfukuta merged commit 39fe090 into beta/v0.3.18.1 Jun 5, 2024
14 of 15 checks passed

sfukuta deleted the hotfix/v0.3.18/add_empty_check branch June 5, 2024 07:28

tier4 deleted a comment from mergify bot Sep 12, 2024

mergify bot mentioned this pull request Sep 12, 2024

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

Closed

7 tasks

asa-naki mentioned this pull request Sep 18, 2024

feat: v0.3.19 rebase #1540

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(obstacle_avoidance_planner): add empty check #1285

fix(obstacle_avoidance_planner): add empty check #1285

sfukuta commented May 2, 2024 •

edited

Loading

yn-mrse commented May 7, 2024

yn-mrse commented May 7, 2024

yn-mrse commented May 7, 2024

sfukuta commented May 8, 2024

sfukuta commented May 8, 2024 •

edited

Loading

yn-mrse commented May 8, 2024 •

edited

Loading

yn-mrse commented May 8, 2024 •

edited

Loading

sfukuta commented May 9, 2024

yn-mrse left a comment

kimurariku commented Sep 12, 2024

mergify bot commented Sep 12, 2024 •

edited

Loading

fix(obstacle_avoidance_planner): add empty check #1285

fix(obstacle_avoidance_planner): add empty check #1285

Conversation

sfukuta commented May 2, 2024 • edited Loading

Description

原因

対策

想定される影響

Related links

Tests performed

機能評価

リグレッション評価

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

yn-mrse commented May 7, 2024

yn-mrse commented May 7, 2024

yn-mrse commented May 7, 2024

sfukuta commented May 8, 2024

sfukuta commented May 8, 2024 • edited Loading

yn-mrse commented May 8, 2024 • edited Loading

yn-mrse commented May 8, 2024 • edited Loading

sfukuta commented May 9, 2024

yn-mrse left a comment

Choose a reason for hiding this comment

kimurariku commented Sep 12, 2024

mergify bot commented Sep 12, 2024 • edited Loading

✅ Backports have been created

sfukuta commented May 2, 2024 •

edited

Loading

sfukuta commented May 8, 2024 •

edited

Loading

yn-mrse commented May 8, 2024 •

edited

Loading

yn-mrse commented May 8, 2024 •

edited

Loading

mergify bot commented Sep 12, 2024 •

edited

Loading