feat(intersection): more precise pass judge handling considering occlusion detection and 1st/2nd attention lane

[soblin]

Description

launcher PR: autowarefoundation/autoware_launch#792

simplify pass judge decision

Before this commit, is_permanent_go_ became TRUE if ego is

• before default stop line OR
• over (1st) pass judge line OR
• middle speed (larger than keep_detection_velocity_threshold)

AND

• over default stop line AND
• over (1st) pass judge line AND
• is_go_out_ == true AND
• previously_safe_ == true

, which is equal to

• over default stop line AND
• over (1st) pass judge line AND
• middle speed AND
• is_go_out_ == true AND
• previously_safe_ == true

After this PR, this condition is simplified to

• over default stop line AND
  • reason: before default stop line, it is preferred to stop.
• over (1st) pass judge line AND
  • reason: if stopped, ego will stop in the middle of the intersection
• previously_safe_ == true
  • reason: sometimes ego passes pass judge line while it is trying to stop. To continue stopping/deceleration, is_permanent_go_ is not set to true unless previously_safe_ == true

With this commit, middle speed condition is removed because it was ambiguous

Also with this commit, is_go_out_ condition is removed because it is equivalent to "previously SAFE" when auto-mode, and when manual-mode still it is equivalent to it for the following reason:

• even if occlusion/collision does not exist but the occlusion is disapproved, this module calculates "ext_occlusion_requested" and this state is judged as occluded. So the previous state is OCCLUDED, which means previously_safe_ == false and it matches is_go_out_ == false (this value is set to false through occlusion_activated_ variable through RTC)

In manual-mode RTC approval is used regardless of how this module judges, so when both "intersection" and "intersection_occlusion" is disapproved, ego will keep stopping at default stop line (if disapproval is not too late and did not pass the sole pass judge line) although the internal state changes from "FirstWaitBeforePeeking" to "PeekingTowardOcclusion" due to the stopping.
Then if "intersection" is approved, still the state is OCCLUDED(because "ext_occlusion_requested") and previously_safe_ == false matches is_go_out_ == false.
If only "intersection_occlusion" is approved and "intersection" is disapproved, then it is expected that ego is NOT after default stop line (maybe this is not the case due to low control precision) and NOT after pass_judge_line(it is highly expected because if ego is stopped at default stop line, pass judge line is almost at the first_attention_area_stopline which is ahead of default stop line), so it will keep stopping.

The position of 2nd pass judge line is set to the same index of 1st pass judge line if second attention lane does not exist.

pure collision detection

[intersection without second attention lane]

If ego is

• over default stop line AND
• over 1st == 2nd pass judge line AND
• previously_safe_ == true

then ego will forcefully pass the intersection. So ego can stop for the following conditions

• collision is detected before default stop line, regardless of its speed
  • maybe ego passes 1st == 2nd pass judge line while it is trying to stop. But in this state previously_safe_ == false, so ego can completely stop until previously_safe_ == true.
• collision is once detected before/after default stop line, and ego stopped after default stop line. Then previously_safe_ became true and ego restarted, although ego is low enough to be before pass judge line. Then if collision is detected again almost at the same time as the restart, ego can stop.

Ego does not stop if it passed pass judge line safely. During the repetitive collision detection, it is possible that ego stops both after default stop line and pass judge line, mainly because it keeps trying to stop during UNSAFE even if it passed the pass judge line while decelerating. So ego stops while it is decelerating and/or stopping for collision even if it passed pass judge line while trying to stop, but once previously_safe_ == true at that position, ego never stops anymore.

[intersection with second attention lane]

For the case without second attention lane, if ego passed 1st pass judge line safely, it never stops.
If second attention lane exist, it can still stop if it is before 2nd pass judge line.

collision detection with occlusion

[intersection without second attention lane]

[occlusion is detected from "before default stop line" to "somewhere between default stop line and peeking position"]

Regardless of ego speed if occlusion is detected before default stop line, ego will stop once and then start peeking. Due to the temporal stop and peeking, 1st pass judge line position will be after default stop line in most cases.

• If occlusion is cleared before 1st pass judge line while peeking, then intersection_occlusion wall disappears, and at , that moment ego is still before 1st pass judge line. Then pure collision detection begins.
• If occlusion is detected after passing 1st pass judge line while peeking, then passed_1st_judge_line_while_peeking_ becomes true and 1st pass judge line changes to occlusion stop line position. So even if occlusion is cleared after this transition, still ego is before 1st pass judge line, thereby it can continue extra occlusion/collision detection there.

[occlusion is detected from "after the default stop line" to "somewhere between default stop line and peeking position"]

If ego already passed 1st pass judge line when ego passed default stop line, ego never stops.
If ego slowly passed default stop line (due to other reasons), then it maybe before 1st pass judge line, so it can start peeking. Since ego is over the default stop line, it will try to stop at intersection_occlusion wall by peeking(deceleration).

• If occlusion is cleared during the peeking before 1st pass judge line, pure collision detection begins.
• If occlusion is detected during the peeking after it passed 1st pass judge line, pass judge line changes to occlusion stop line position. So even if occlusion is cleared after this transition, still ego is before pass judge line, thereby it can continue extra occlusion/collision detection there.

[intersection with second attention lane]

Before this PR, collision/occlusion detection continued while is_permanet_go_ == false. After this PR, ego continues collision/occlusion detection while it is before 2nd pass judge line.

Related links

https://tier4.atlassian.net/browse/RT1-4748

Tests performed

https://evaluation.tier4.jp/evaluation/reports/9bce54e7-644b-5a1d-9d83-6a31ed9fdd91?project_id=prd_jt

pure collision at intersection with single attention lane

At [00:36] ego passed the 1st pass judge line

PR6047-2024-01-15_19.51.42.mp4

In the next PR, if ego is over the 1st pass judge line and the most probable collision is expected to happen on the first attention lane, ego will GO even if it is before the 2nd pass judge line.

PR6047-2024-01-15_20.01.05.mp4

with occlusion

PR6047-2024-01-16_15.30.19.mp4

Notes for reviewers

Interface changes

none

Effects on system behavior

none

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.

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-6047/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-6047/planning/behavior_velocity_intersection_module/

[codecov]

Codecov Report

Attention: 45 lines in your changes are missing coverage. Please review.

Comparison is base (d957135) 14.65% compared to head (8bd98bb) 14.64%.
Report is 36 commits behind head on main.

Files	Patch %	Lines
...ity_intersection_module/src/scene_intersection.cpp	0.00%	32 Missing ⚠️
...ehavior_velocity_intersection_module/src/debug.cpp	0.00%	11 Missing ⚠️
...avior_velocity_intersection_module/src/manager.cpp	0.00%	0 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6047      +/-   ##
==========================================
- Coverage   14.65%   14.64%   -0.01%     
==========================================
  Files        1857     1857              
  Lines      126513   126542      +29     
  Branches    37059    37080      +21     
==========================================
  Hits        18535    18535              
- Misses      87155    87182      +27     
- Partials    20823    20825       +2     

Flag	Coverage Δ		*Carryforward flag
differential	5.14% <0.00%> (?)
total	14.65% <ø> (+<0.01%)	⬆️	Carriedforward from d957135

*This pull request uses carry forward flags. Click here to find out more.

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