fix(behavior_path_planner): fix abnormal emergency stop when enable_avoidance_over_opposite_direction is true

[shulanbushangshu]

Description

When the parameter "enable_avoidance_over_opposite_direction"is true(The default is true), the vehicle uses the reverse lane for obstacle avoidance; When the vehicle avoids the obstacles and returns to the main lane, the vehicle will stop abnormally and then run again (because the calculated drivable area changes suddenly).
By this PR, it can fix the abnormal emergency stop in obstacle avoidance.

Before this PR:
https://user-images.githubusercontent.com/102840938/183551281-062b1912-62a9-4287-971f-4e6be5b0c5ea.mp4
After this PR：
https://user-images.githubusercontent.com/102840938/183551431-8e3e4335-e2d3-42de-8e33-5603f9ed3ef1.mp4

Related links

Tests performed

Current simulation：

no-fix-drivearea.mp4

Modified simulation：

fix-drivearea.mp4

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.

[takayuki5168]

@zulfaqar-azmi-t4 @TakaHoribe Could you review this.

[codecov]

Codecov Report

Merging #1544 (310b447) into main (319d15e) will increase coverage by 0.20%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #1544      +/-   ##
==========================================
+ Coverage   10.73%   10.93%   +0.20%     
==========================================
  Files        1115     1039      -76     
  Lines       78829    70209    -8620     
  Branches    18561    18250     -311     
==========================================
- Hits         8460     7679     -781     
+ Misses      61647    53925    -7722     
+ Partials     8722     8605     -117     

Flag	Coverage Δ		*Carryforward flag
differential	0.78% <0.00%> (?)
total	11.02% <0.00%> (+0.30%)	⬆️	Carriedforward from 7566a66

*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%> (ø)
...include/obstacle_cruise_planner/common_structs.hpp	0.00% <ø> (ø)
...ation_based_planner/optimization_based_planner.hpp	0.00% <ø> (ø)
.../optimization_based_planner/velocity_optimizer.hpp	0.00% <ø> (ø)
...se_planner/pid_based_planner/pid_based_planner.hpp	0.00% <ø> (ø)
...lude/obstacle_cruise_planner/planner_interface.hpp	0.00% <0.00%> (ø)
...ation_based_planner/optimization_based_planner.cpp	0.00% <0.00%> (ø)
.../optimization_based_planner/velocity_optimizer.cpp	0.00% <ø> (ø)
...lanner/src/pid_based_planner/pid_based_planner.cpp	0.00% <0.00%> (ø)
.../obstacle_cruise_planner/src/planner_interface.cpp	0.00% <ø> (ø)
... and 764 more

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

[shulanbushangshu]

@zulfaqar-azmi-t4 ,could you check this simulated phenomenon?

[zulfaqar-azmi-t4]

@shulanbushangshu
Hi, sorry for the late response.
Just to update, currently I am testing your solution.
I'll let your know tomorrow after I analyze your code a bit.

[zulfaqar-azmi-t4]

@shulanbushangshu
I don't have a map to test, but technically, there is a possibility that the drivable area extension might will result with the same issue if the number of opposite lane > 1.

Another thing of my concern in that multiple similar lanelet will be pushed back (i think the same issue already existed), and it might affect the performance a bit.

I have another solution to this, but I will need to discuss internally about this part, and let you know about it.

[shulanbushangshu]

@zulfaqar-azmi-t4 .Thank you for your examination.I use "extended_lanelets.push_back(search_lanelets.at(1)) " to ensure that the current adjacent lane is used instead of multiple lanes.I have tested the two opposite lanes and the result is well.

LINK:
https://user-images.githubusercontent.com/102840938/184335777-0429403f-4411-4dc5-b807-f8e61b601d02.mp4

Screencast.2022-08-12.18.21.41.mp4

[zulfaqar-azmi-t4]

Hi @shulanbushangshu
We have discussed internally for the workaround of this.

From the look of it, the drivable area extension is too conservative, which add complexity to the current method.
One simpler solution is to just extend the drivable area to both left and right of avoidance_data_.current_lanelets. This way,

1. we can reduce the complexity by not having conditional statement.
2. we can remove this

We don't really have to worry about the portion of the drivable area that is not used.

The portion of the code that should be modify is here.

Basically is to have

        search_lanelets = route_handler->getAllSharedLineStringLanelets(
          lane);
      }
      extended_lanelets.insert(
        extended_lanelets.end(), search_lanelets.begin(), search_lanelets.end());

Let me know what do you think about this.

[shulanbushangshu]

Hi,@zulfaqar-azmi-t4
Yes, the original idea of considering the driveable area which would be used leds to the complexity of my previous code.
As you think, we can simply add the left and right lanes of the current lane to the extension Lanes.
I change the code as you say and simulate:

case1:
https://user-images.githubusercontent.com/102840938/184581248-431ed969-50b4-492b-aeb8-95984435a4c0.mp4
case2:
https://user-images.githubusercontent.com/102840938/184582823-ceb69f91-4d42-414d-9028-ab083bcd86ed.mp4

The result is well.The drivable area will remain the maximum range during the operation of the obstacle avoidance module.
By the way, whether or not the maximum driveable area increases the amount of calculation in the module"obstacle_avoidance_planner"

[zulfaqar-azmi-t4]

Hi, @shulanbushangshu. Thank you very much for the confirmation.

Great. Considering enable_avoidance_over_opposite_direction, a little more stuff might need to be added.

About obstacle_avoidance_planner, I have checked with the corresponding developer about the impact of the changes and it seems to be fine.

[zulfaqar-azmi-t4]

Hi @shulanbushangshu
Just checking in case you need some help regarding this. If yes, then I can make some changes from my branch and make a separate PR.

[zulfaqar-azmi-t4]

Hi @shulanbushangshu

Due to some internal discussion and request, we also requires this part to be fix. Therefore, I deeply apologize for having to create another PR to fix this part. The PR included the details of what we have discussed, with some additional refactoring. Please let me know if there is any issue on your side.

#1688

[shulanbushangshu]

@zulfaqar-azmi-t4
Yes,I have read the new code.It works well.I will close this pr.