feat(behavior_velocity_planner::intersection): add occlusion detection feature #3458

soblin · 2023-04-19T01:59:45Z

Description

This PR adds the functionality to consider occluded area in attention area to care the possible collisions with upcoming vehicles from the occluded area.

workflow

If occlusion is detected on the attention area, the vehicle will not judge "SAFE" because it may be missing vehicles running from the occluded attention area. Until the occlusion becomes clear, the ego vehicle will behave as follows.

two phase stop

Firstly the vehicle will stop at the default stop line for occlusion/before_creep_stop_time.

creep toward the limit occlusion stop line

After that the vehicle will creep toward the limit occlusion stop line at the velocity of occlusion/occlusion_creep_velocity.

wait until the occlusion get cleared
If the occlusion get cleared because ego approached enough to the attention area, or the blocking vehicle disappeared, etc., then it restarts collision checking and pass the intersection accordingly.

algorithm

Internally this module generates a costmap /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/occlusion_grid where each cell represents the distance to ego's path along the lane.

Currently this costmap is visualized as a grid_map whose cost is colorized with heatmap.

Tests performed

By default this feature is disabled and has no influence on the current system as of now.

There was no degradation on the CI.
https://evaluation.tier4.jp/evaluation/reports/d78641b5-3ab8-5bcc-8e56-a9d6058bb997?project_id=prd_jt
https://evaluation.tier4.jp/evaluation/reports/16dd774e-d506-5332-9d95-e241d83f9bc9?project_id=prd_jt

Notes for reviewers

This feature is under development and so is set to off by default. After the feature gets stable and reliable I will update the document and enable the flag.

Interface changes

A topic /planning/cooperate_status/intersection_occlusion is newly added

Effects on system behavior

If enabled, the vehicle will stop in the intersection if occlusion is detected.

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.

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…passed 1st stop line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…ARED Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

…to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

taikitanaka3 · 2023-04-24T02:10:20Z

@soblin
~~please add PR link to tier4_autoware_msgs RTC message~~ I found it

…n feature (autowarefoundation#3458) * migrated Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * WIP Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * IntersectionModuleManager own one rtc_interface for intersection_occlusion Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> * divide occlusion safety and activated Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> * fixed to update occlusion cooperate status at construction Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed getOcclusionFirstStopSafety() Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed not to show both intersection and intersection_occlusion after passed 1st stop line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed the intersection_occlusion/inersection stop position afte r CLEARED Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * if occlusion cleared when eog is before 1st stop line, set stop line to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * (misc) fix viz, sync param Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed vehicle footprint offset calculation Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add occcupancy_grid_map method/param var to launcher Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * migrated latest Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use static pass judge line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * removed some params Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * organized param Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add occlusion enable flag Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * revert occupancy grid settings in this PR Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * remove comment Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed pass judge line collision detection to original Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * style(pre-commit): autofix * use vehicle_length for static pass judge line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed virtual wall marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

…n feature (autowarefoundation#3458) * migrated Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * WIP Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * IntersectionModuleManager own one rtc_interface for intersection_occlusion Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> * divide occlusion safety and activated Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> * fixed to update occlusion cooperate status at construction Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed getOcclusionFirstStopSafety() Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed not to show both intersection and intersection_occlusion after passed 1st stop line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed the intersection_occlusion/inersection stop position afte r CLEARED Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * if occlusion cleared when eog is before 1st stop line, set stop line to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * (misc) fix viz, sync param Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed vehicle footprint offset calculation Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add occcupancy_grid_map method/param var to launcher Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * migrated latest Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * use static pass judge line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * removed some params Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * organized param Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * add occlusion enable flag Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * revert occupancy grid settings in this PR Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * remove comment Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed pass judge line collision detection to original Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * style(pre-commit): autofix * use vehicle_length for static pass judge line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> * fixed virtual wall marker Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> --------- Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp> Signed-off-by: Mamoru Sobue <hilo.soblin@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: Mingyu Li <mingyu.li@tier4.jp>

soblin force-pushed the feat/intersection-occlusion-latest branch from f2f21dd to ea74e4e Compare April 19, 2023 04:18

github-actions bot removed the component:launch Launch files, scripts and initialization tools. (auto-assigned) label Apr 19, 2023

soblin changed the title ~~feat (behavior_velocity_planner::intersection): add occlusion detection feature~~ feat(behavior_velocity_planner::intersection): add occlusion detection feature Apr 19, 2023

soblin force-pushed the feat/intersection-occlusion-latest branch 4 times, most recently from 4d85a2a to 1d68d30 Compare April 19, 2023 11:37

soblin marked this pull request as ready for review April 19, 2023 12:30

soblin requested review from rej55, taikitanaka3, a team, kosuke55, kyoichi-sugahara, mkuri, maxime-clem, satoshi-ota, shmpwk, takayuki5168, TomohitoAndo, tkimura4, purewater0901 and yukkysaito as code owners April 19, 2023 12:30

soblin force-pushed the feat/intersection-occlusion-latest branch from 92c08c1 to 47f3466 Compare April 19, 2023 12:51

soblin and others added 18 commits April 20, 2023 13:21

fixed to update occlusion cooperate status at construction

7dd3b24

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fixed getOcclusionFirstStopSafety()

ed9ffdf

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fixed not to show both intersection and intersection_occlusion after …

a7caed2

…passed 1st stop line Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fixed the intersection_occlusion/inersection stop position afte r CLE…

eb9e04a

…ARED Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

if occlusion cleared when eog is before 1st stop line, set stop line …

6eb2ff6

…to 1st stop line and clear prev_occlusion_stop_line_pose_ Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

(misc) fix viz, sync param

caa4644

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fixed vehicle footprint offset calculation

5695dc4

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

add occcupancy_grid_map method/param var to launcher

55ad405

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

migrated latest

b174a05

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

use static pass judge line

501c360

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

removed some params

079dde9

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

organized param

f488aaa

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

add occlusion enable flag

a5e6621

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

revert occupancy grid settings in this PR

d5fe019

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

remove comment

cc9005e

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

fixed pass judge line collision detection to original

c586559

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

style(pre-commit): autofix

c35ab5a

use vehicle_length for static pass judge line

8749f7a

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

soblin force-pushed the feat/intersection-occlusion-latest branch from 94829e4 to 8749f7a Compare April 20, 2023 04:44

soblin added 2 commits April 21, 2023 15:43

Merge branch 'main' into feat/intersection-occlusion-latest

5b8bf39

fixed virtual wall marker

db1becf

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

takayuki5168 approved these changes Apr 22, 2023

View reviewed changes

taikitanaka3 approved these changes Apr 24, 2023

View reviewed changes

takayuki5168 merged commit a3a653d into autowarefoundation:main Apr 24, 2023

soblin deleted the feat/intersection-occlusion-latest branch April 24, 2023 13:13

tier4-autoware-public-bot bot mentioned this pull request Apr 25, 2023

chore: sync upstream tier4/autoware.universe#376

Merged

TakaHoribe mentioned this pull request May 11, 2023

Intersection module development plan #3224

Open

3 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(behavior_velocity_planner::intersection): add occlusion detection feature #3458

feat(behavior_velocity_planner::intersection): add occlusion detection feature #3458

soblin commented Apr 19, 2023 •

edited

Loading

taikitanaka3 commented Apr 24, 2023 •

edited

Loading

feat(behavior_velocity_planner::intersection): add occlusion detection feature #3458

feat(behavior_velocity_planner::intersection): add occlusion detection feature #3458

Conversation

soblin commented Apr 19, 2023 • edited Loading

Description

workflow

algorithm

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

taikitanaka3 commented Apr 24, 2023 • edited Loading

soblin commented Apr 19, 2023 •

edited

Loading

taikitanaka3 commented Apr 24, 2023 •

edited

Loading