fix(behavior_path_planner): fix lane extension in getCurrentLanesFromPath

[kosuke55]

Description

fix infinity extension in loop lanes like

this bug is caused by #5392
If there is no first point of path point behind current_lanes, it will loop forever.
This PR only extends if they exist in the same sequence. Furthermore, the extension is restricted to only within the root_lanes.

Related links

#5392

Tests performed

psim

the failed scenario in local

evaluator_description: fix/getCurrentLanesFromPath
2023/10/26 https://evaluation.tier4.jp/evaluation/reports/bb494913-907e-5aa9-a638-d11a4c087f32/?project_id=prd_jt

https://evaluation.tier4.jp/evaluation/reports/61f7793a-366d-5589-80e4-c817ed75c233/?project_id=prd_jt

Notes for reviewers

Interface changes

Effects on system behavior

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

Attention: 12 lines in your changes are missing coverage. Please review.

Comparison is base (82a6c0c) 14.75% compared to head (c93a2d0) 14.75%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #5415   +/-   ##
=======================================
  Coverage   14.75%   14.75%           
=======================================
  Files        1661     1661           
  Lines      115525   115529    +4     
  Branches    35670    35676    +6     
=======================================
+ Hits        17050    17051    +1     
+ Misses      79231    79222    -9     
- Partials    19244    19256   +12     

Flag	Coverage Δ		*Carryforward flag
differential	12.34% <0.00%> (?)
total	14.75% <ø> (-0.01%)	⬇️	Carriedforward from 5bde001

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

Files	Coverage Δ
...nner/include/behavior_path_planner/utils/utils.hpp	45.45% <ø> (ø)
planning/behavior_path_planner/src/utils/utils.cpp	13.93% <0.00%> (+0.19%)	⬆️

... and 5 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.