feat(avoidance): implement avoidance path safety check logic

[satoshi-ota]

Description

BackGround

The previous avoidance module did not have a safety check mechanism, and the operator had no choice but to OR (over ride) and interrupt the avoidance maneuver as manualy if a vehicle in the adjacent or opposite lane approached after the module had switched output paths for avoidance. This problems has a significant impact on miles per disengagement, so we would like to implement safety check mechanism and yield behavior in the module.

As a first step, I implement safetye check mechanism in this PR. The latter (yield maneuver) will be implement in another PR.

Logic

Add following new parameters for safety check mechanism.

enable_safety_check: false

# For safety check
safety_check_backward_distance: 50.0  # [m]
safety_check_time_horizon: 10.0 # [s]
safety_check_idling_time: 1.5 # [s]
safety_check_accel_for_rss: 2.5  # [m/ss]

Step1

safety_check_backward_distance is the parameter related to the safety check area. The module checks a collision risk for all vehicle that is within shift side lane and between object _check_forward_distance ahead and safety_check_backward_distance behind.

NOTE: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target.

Step2

Judge the risk of collision based on ego future position and object prediction path. The module calculates Ego's future position in the time horizon (safety_check_time_horizon), and use object's prediction path as object future position.

After calculating the future position of Ego and object, the module calculates the lateral/longitudinal deviation of Ego and the object. The module also calculates the lateral/longitudinal margin necessary to determine that it is safe to execute avoidance maneuver, and if both the lateral and longitudinal distances are less than the margins, it determines that there is a risk of a collision at that time.

The value of the longitudinal margin is calculated based on Responsibility-Sensitive Safety theory (RSS). The safety_check_idling_time represents $T_{idle}$, and safety_check_accel_for_rss represents $a_{max}$.

$$D_{lon} = V_{ego}T_{idle} + \frac{1}{2}a_{max}T_{idle}^2 + \frac{(V_{ego} + a_{max}T_{idle})^2}{2a_{max}} - \frac{V_{obj}^2}{2a_{max}}$$

The lateral margin is changeable based on ego longitudinal velocity. If the vehicle is driving at a high speed, the lateral margin should be larger, and if the vehicle is driving at a low speed, the value of the lateral margin should be set to a smaller value. Thus, the lateral margin for each vehicle speed is set as a parameter, and the module determines the lateral margin from the current vehicle speed as shown in the following figure.

target_velocity_matrix:
  col_size: 4
  matrix:
    [2.78, 5.56, 11.1, 13.9,   # target velocity [m/s]
     0.20, 0.90, 1.10, 1.20]   # margin [m]

Assumption

Related links

• chore: sync awf-latest tier4/autoware_launch#637

Tests performed

Evaluator Result

[TIER IV INTERNAL] This PR#1 (817/825)
[TIER IV INTERNAL] This PR#2 (818/825)
[TIER IV INTERNAL] Baseline (812/825)

Test in local

Befor the tests, please use this autoware_launch branch and set following params in config to true.

enable_safety_check: true
publish_debug_marker: true # optional

Since this PR only implements a function that returns the safety of the avoidance path as a boolean, it does not change the behavior of the vehicle (Even if the module judges the avoidance path is NOT safe, do nothing. The vehicle will go on avoidance maneuver.). On the other hand, we can see the status via debug marker and check whether the module has determined that the avoidance path is safe or not.

• debug marker topic: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance
• The vehicles that has collision risk are shown as BLUE CUBE in rviz (namespace unsafe_objects).
• The safety judge is shown as WHITE TEXT in rviz (namespace ego_status)

Notes for reviewers

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.

[codecov]

Codecov Report

Base: 10.53% // Head: 10.34% // Decreases project coverage by -0.19% ⚠️

Coverage data is based on head (4fc4f88) compared to base (d969e52).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2534      +/-   ##
==========================================
- Coverage   10.53%   10.34%   -0.20%     
==========================================
  Files        1269     1269              
  Lines       87732    89708    +1976     
  Branches    21012    22397    +1385     
==========================================
+ Hits         9242     9277      +35     
- Misses      68627    70452    +1825     
- Partials     9863     9979     +116     

Flag	Coverage Δ		*Carryforward flag
differential	3.01% <0.00%> (?)
total	10.53% <0.00%> (+<0.01%)	⬆️	Carriedforward from 4c1fa74

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

Impacted Files	Coverage Δ
...lanner/scene_module/avoidance/avoidance_module.hpp	0.00% <0.00%> (ø)
...r/scene_module/avoidance/avoidance_module_data.hpp	0.00% <0.00%> (ø)
...lanner/include/behavior_path_planner/utilities.hpp	34.48% <ø> (+5.07%)	⬆️
...or_path_planner/src/behavior_path_planner_node.cpp	0.28% <0.00%> (+0.10%)	⬆️
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
...ner/src/scene_module/avoidance/avoidance_utils.cpp	1.39% <0.00%> (-0.01%)	⬇️
..._path_planner/src/scene_module/avoidance/debug.cpp	0.00% <0.00%> (ø)
...path_planner/src/scene_module/lane_change/util.cpp	0.00% <0.00%> (ø)
...r/src/scene_module/pull_out/geometric_pull_out.cpp	0.00% <0.00%> (ø)
...anner/src/scene_module/pull_out/shift_pull_out.cpp	0.00% <0.00%> (ø)
... and 15 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[TakaHoribe]

NOTE: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target.

Would you let me know the reason of this approach.

[satoshi-ota]

NOTE: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target.

Would you let me know the reason of this approach.

@TakaHoribe Thanks for your comment.
The reason why we use CoG of object instead of polygon is that we do NOT want to extract vehicles in the ego's lane as safety check target as much as possible. With the current object recognition accuracy, even if a vehicle traveling behind ego, its polygon sometimes have overlap with the adjacent lane, so if the target is filtered by the former method, the safety check mechanism may make wrong dicision as unsafe for vehicle that is in ego's lane. On the other hand, the module can estimate the object driving lane relatively accurately by using CoG.

[satoshi-ota]

Add evaluator result in description.

[TIER IV INTERNAL] This PR (817/825)
[TIER IV INTERNAL] Baseline (812/825)

[satoshi-ota]

@TakaHoribe Thanks for all your reviews. I have written release note. I'll merge this.

[yukkysaito]

@satoshi-ota @TakaHoribe After this PR is merged, maybe Psim cannot work 🙏

[satoshi-ota]

@satoshi-ota @TakaHoribe After this PR is merged, maybe Psim cannot work pray

@yukkysaito Thanks for your report. I'm sorry about that. I will be careful. 🙇