feat(behavior_path_planner): add avoid_margin for over distance case

[beyzanurkaya]

Signed-off-by: beyza bnk@leodrive.ai

Description

Fixes: #2197

Before PR:

After PR:

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.

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.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Base: 10.45% // Head: 10.39% // Decreases project coverage by -0.06% ⚠️

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

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2198      +/-   ##
==========================================
- Coverage   10.45%   10.39%   -0.07%     
==========================================
  Files        1252     1252              
  Lines       91231    91787     +556     
  Branches    20922    21379     +457     
==========================================
  Hits         9537     9537              
- Misses      71557    72061     +504     
- Partials    10137    10189      +52     

Flag	Coverage Δ		*Carryforward flag
differential	3.22% <0.00%> (?)
total	10.42% <0.00%> (ø)		Carriedforward from 11ff8ef

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

Impacted Files	Coverage Δ
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)

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.

[beyzanurkaya]

waiting for review :)

[mitsudome-r]

ping @takayuki5168

[satoshi-ota]

@beyzanurkaya cc @mitsudome-r @takayuki5168 Thanks for your contribute!! I'll review in this week.

[satoshi-ota]

@beyzanurkaya Thanks for your PR 😄

I assume that this PR modification will let the avoidance module generate an unpassable avoidance path due to insufficient lateral margin. Could you please tell me about the benefits of this PR? You want avoidance module to generate an avoidance path even if it is insufficient for the lateral margin defined by the parameter, right?

[xmfcx]

@mehmetdogru will share more details on this issue/PR.

[mehmetdogru]

@satoshi-ota

Sorry for not being clear enough and of course in the suggested way some avoidance path would have insufficient clearance for the ego. So let me explain the issue we are having with the current implementation first then I will shortly present my new proposal.

So in avoidance module there are two margin params (lateral_collision_margin & lateral_collision_safety_buffer) added to vehicle_width/2 to calculate avoid_margin. They are of course the same for all the lanes and roads regardless of how wide or narrow they are. In cases where there are both wide and narrow roads such as in campuses things are getting tricky.

We know that:

avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;

---

In case 1 I tune my parameters for narrow roads:

So I keep lat_collision_margin, lat_collision_safety_buffer and road_shoulder_safety_margin as small as possible. However for wide roads I want ego to shift more since there are plenty of space on the lane but I tuned the params for narrow road so it is not possible:

---

In case 2 I tune my parameters for wide roads:

I set params above as I think is proper for the safety and comfort then for narrow roads ego wouldn't do the avoidance:

---

My proposal is that instead of keeping avoid_margin stable (at_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width), we can let it vary between max_allowable_lateral_distance and min_safety_lateral_distance.

max_allowable_lateral_distance already exists:

max_allowable_lateral_distance = o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width

And lets define min_safety_lateral_distance as:

min_safety_lateral_distance = lat_collision_safety_buffer + 0.5 * vehicle_width

So I think in the current implementation lat_collision_margin and lat_collision_safety_buffer does the same job since just summation of these two is used. With the approach I am proposing we can set min & max margin using lat_collision_margin and lat_collision_safety_buffer then let avoid_margin vary between them. In this case for the both scenarios above, ego would do the avoidance as user wanted.

@satoshi-ota to please review the proposed changes in the PR.

[satoshi-ota]

@mehmetdogru @beyzanurkaya Thank you. They are clear to me.

So I think in the current implementation lat_collision_margin and lat_collision_safety_buffer does the same job since just summation of these two is used. With the approach I am proposing we can set min & max margin using lat_collision_margin and lat_collision_safety_buffer then let avoid_margin vary between them. In this case for the both scenarios above, ego would do the avoidance as user wanted.

I was concerned that even if it was a shift point that could not be passed, an avoidance path would be generated and the vehicle would follow it, but now, I understand that what you want to do by your explanation and this commit 👍

I think your method looks good to me.

[mehmetdogru]

@satoshi-ota

Shall we go ahead and merge these changes? Are you ok with them?

[satoshi-ota]

@beyzanurkaya @mehmetdogru LGTM

[satoshi-ota]

@beyzanurkaya @mehmetdogru Could you update avoidance documentation in another PR?