feat(behavior_path_planner): potentially speed up cut overlapped lanes

[danielsanchezaran]

Description

Potentially reduce processing time on CutOverlappedLanes by breaking early when finding the first path point within drivable lanes, also simplified a little the code by using a lambda function.

Related links

TIER IV Internal link: Evaluator tests for first commit (Currently running a new test, but in principle there is no degradation related to this change)

Tests performed

Ran evaluator (see above) and also tested on PSim if the overlapped lanes are cut correctly, the behavior is correct.

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: 13 lines in your changes are missing coverage. Please review.

Comparison is base (d4d632a) 15.34% compared to head (11f05db) 15.21%.
Report is 34 commits behind head on main.

Files	Patch %	Lines
...s/drivable_area_expansion/static_drivable_area.cpp	0.00%	13 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5906      +/-   ##
==========================================
- Coverage   15.34%   15.21%   -0.13%     
==========================================
  Files        1743     1762      +19     
  Lines      119952   122521    +2569     
  Branches    36481    37911    +1430     
==========================================
+ Hits        18402    18642     +240     
- Misses      80963    82915    +1952     
- Partials    20587    20964     +377     

Flag	Coverage Δ		*Carryforward flag
differential	11.73% <0.00%> (?)
total	15.34% <ø> (+<0.01%)	⬆️	Carriedforward from d4d632a

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

[satoshi-ota]

LGTM 👍 Thanks!