feat(start_planner): prevent hindering rear vehicles

[danielsanchezaran]

Description

Current Autoware:
Currently, obstacle collision check with vehicles coming from the rear is performed when the ego vehicle is trying to merge to a lane from the shoulder lane. As follows:

The collision check is done until the ego vehicle eventually overlaps with the centerline. However, it is possible that the check is performed when the ego vehicle already impedes the pass of vehicles as shown on the next image:

In the case above, the ego vehicle has not quite overlapped the centerline yet (closest point in purple) but the gap between the ego and the lane boundary, W2, is smaller than the width W1 of the NPC vehicle (black). In this cases, with collision check active, the ego vehicle might stop moving, even though the rear vehicle cannot really pass. The final outcome is that the ego vehicle ends up impeding the previous vehicle from passing and also remains stopped.

This PR:
With this PR, the ego will not check overlaps with the center line; instead, it will check whether the remaining gap between the ego and the lane is big enough for the rear obstacle to pass. If it is big enough, the ego stops/yields to the coming vehicle, but, if it is smaller, it means the rear vehicle cannot stop and the ego should proceed. There is also an added parameter "extra_width_margin_for_rear_obstacle" which can be used to artificially enlarge or reduce the rear vehicle's perceived width.

Example:

1. The gap is big enough for the rear vehicle to pass, so the ego stops (start planner's red virtual wall)

2. The gap is small, the rear vehicle cannot pass and the ego has "claimed" the lane: The ego vehicle proceeds, no stop point is added (note the ego is not overlapping the centerline yet):

3. Same ego pose as 2) but the rear bicycle is small, so it can still fit in the gap left by ego: A stop point is inserted and we let the bicycle pass.

Related links

Related ticket: TIER IV INTERNAL LINK

Tests performed

PSIM

Degradation tests -> NO degradation TIER IV INTERNAL LINK

Notes for reviewers

A function to get the backward lanes was moved from lane change module to common so it can be used by the start planner too.

Interface changes

Effects on system behavior

The ego will not impede upcoming traffic by not committing to a lane entrance it should do.

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.

[zulfaqar-azmi-t4]

LGTM

[kyoichi-sugahara]

Suggested change

	// Get the ego's overhang point closest to the centerline path and the gap between said point an
	// Get the ego's overhang point closest to the centerline path and the gap between said point and

[kyoichi-sugahara]

@danielsanchezaran
If my understanding is correct, the assumption is merging from left lane, correct?
If ego merge from right side, the gap is wrongly calculated?

[kyoichi-sugahara]

@danielsanchezaran
It feels like it would be better to determine from which side the merging is happening. Therefore, using getOriginalStartPose() and the target lanes to judge whether the starting point is on the right or left side of the target lane might be a good idea.
What do you think?

[kyoichi-sugahara]

Can not request a change...
How about changing following description from

    if (Can stop within constraints \n && \n no crossing centerline?) then (yes)

to

    if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes)

[danielsanchezaran]

@danielsanchezaran If my understanding is correct, the assumption is merging from left lane, correct? If ego merge from right side, the gap is wrongly calculated?

The current calculates both distances to the left AND right edges of the road and uses the largest one, so, in the case you describe, the gap would be the distance between the left bound of the lane and the ego's overhang point. So it does not matter from which side the merging is happening.

[danielsanchezaran]

The code has been updated so the closest ego overhang point to the lane border is used to calculate the gap. The border to calculate the distance is chosen depending on which side the ego is merging the lane from. @kyoichi-sugahara

[kyoichi-sugahara]

Thank you so much for your quick response!
LGTM!

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 98 lines in your changes are missing coverage. Please review.

Project coverage is 14.79%. Comparing base (7081a61) to head (ec5179b).

Files	Patch %	Lines
..._start_planner_module/src/start_planner_module.cpp	0.00%	78 Missing ⚠️
...g/behavior_path_planner_common/src/utils/utils.cpp	0.00%	14 Missing ⚠️
...behavior_path_start_planner_module/src/manager.cpp	0.00%	5 Missing ⚠️
...rc/utils/path_safety_checker/objects_filtering.cpp	0.00%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6545      +/-   ##
==========================================
- Coverage   14.80%   14.79%   -0.01%     
==========================================
  Files        1917     1917              
  Lines      132086   132146      +60     
  Branches    39262    39262              
==========================================
  Hits        19553    19553              
- Misses      90715    90775      +60     
  Partials    21818    21818              

Flag	Coverage Δ		*Carryforward flag
differential	10.76% <0.00%> (?)
total	14.80% <ø> (+<0.01%)	⬆️	Carriedforward from 7081a61

*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.