fix(behavior_path_planner): fix lane change logic on the edge case

[zulfaqar-azmi-t4]

While experimenting in Odaiba, due to the safety reason, it is difficult to test the lane change function when there is object parked at the end of the road. To verify this, the confirmation is executed via PSIM. r

Once tested in PSIM, when longitudinal check is performed, it is found out that the lane change is not successful, due to the collision check in prepare phase. To allow user to be able to relax the collision check when the enable_collision_check_at_prepare_phase is true, the parameter that ignore target which speed falls under some threshold should be introduced.

Description

The PR solves several issue.

1. When setting enable_collision_check_at_prepare_phase = true, for both stationary object and moving object, the collision check will be conducted for the duration of prepare phase + lane changing phase. But it is nice if we can ignore prepare phase check for stationary object only. Therefore new parameter is added prepare_phase_ignore_target_speed_thresh to ignore target with certain speed.
2. When performing the collision check, ego vehicle's trajectory is converted into predicted path, and for each pose in the predicted path, ego's polygon will be generated. However, currently ego's polygon is generated only at current pose and not the predicted path. Therefore to improve this, ego polygon will be generated at the estimated pose.
3. In addition, the debug message is improve to allow more expressive debug message.
4. Added default value to LaneChangeParameter's member variable to ensure no undefined behavior related to the parameter declaration.

Before change

After change

Related links

Tests performed

Test lane change scenario with stationary and moving target.

Notes for reviewers

None

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.62% // Head: 10.52% // Decreases project coverage by -0.10% ⚠️

Coverage data is based on head (18ec2fb) compared to base (660c6ac).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2287      +/-   ##
==========================================
- Coverage   10.62%   10.52%   -0.11%     
==========================================
  Files        1239     1240       +1     
  Lines       82824    84020    +1196     
  Branches    20399    21013     +614     
==========================================
+ Hits         8803     8842      +39     
- Misses      64329    65246     +917     
- Partials     9692     9932     +240     

Flag	Coverage Δ		*Carryforward flag
differential	3.20% <0.00%> (?)
total	10.62% <0.00%> (ø)		Carriedforward from 660c6ac

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

Impacted Files	Coverage Δ
...lanner/include/behavior_path_planner/utilities.hpp	26.31% <ø> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.15% <0.00%> (+0.15%)	⬆️
...ner/src/scene_module/avoidance/avoidance_utils.cpp	2.13% <ø> (+0.10%)	⬆️
...path_planner/src/scene_module/lane_change/util.cpp	0.00% <0.00%> (ø)
planning/behavior_path_planner/src/utilities.cpp	4.14% <0.00%> (+0.32%)	⬆️
planning/behavior_path_planner/test/input.cpp	60.00% <0.00%> (-7.86%)	⬇️
...ng/behavior_path_planner/test/test_turn_signal.cpp	40.41% <0.00%> (-6.86%)	⬇️
.../behavior_path_planner/src/turn_signal_decider.cpp	35.93% <0.00%> (-0.58%)	⬇️
planning/static_centerline_optimizer/src/main.cpp	0.00% <0.00%> (ø)
planning/static_centerline_optimizer/src/utils.cpp	0.00% <0.00%> (ø)
... and 34 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]

However, since this is parked vehicle, we don't really need to include prepare phase in the collision check.

I don't get this idea well. Why the "collision check in prepare phase" is not necessary for a parked vehicle?

In my understanding, even if it is a parked vehicle, the collision check for lane change must be performed in all phases.
However, it is reasonable that the collision check for parked vehicles does not have to be as strict as for moving vehicles. We could set the collision margin very small in the collision check for parked vehicles.

[TakaHoribe]

LGTM

[TakaHoribe]

I don't get this idea well. Why the "collision check in prepare phase" is not necessary for a parked vehicle?
In my understanding, even if it is a parked vehicle, the collision check for lane change must be performed in all phases.
However, it is reasonable that the collision check for parked vehicles does not have to be as strict as for moving vehicles. We could set the collision margin very small in the collision check for parked vehicles.

Discussion with @zulfaqar-azmi-t4 : the collision check in the prepare phase is to restrict the lane change plans overtaking a vehicle, not critical to the safety.