fix(behavior_velocity_planner): expend the ego_lane_with_next_lane area

[beyzanurkaya]

Description

fixes: #4153

ego_lane_with_next_lane only keeps 2 lanes and in some cases, the total length of these combined lanes may not be in the range of start_arc_length and end_arc_length calculated beforehand. now I add lanelets to ego_lane_with_next_lanes until the total length of added lanelets is between arc values.

Tests performed

vokoscreenNG-2023-07-05_19-19-09.mov

Effects on system behavior

Not applicable.

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

Patch coverage has no change and project coverage change: -0.90 ⚠️

Comparison is base (92b78db) 15.17% compared to head (cf1506d) 14.27%.

❗ Current head cf1506d differs from pull request most recent head 9435e6f. Consider uploading reports for the commit 9435e6f to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4177      +/-   ##
==========================================
- Coverage   15.17%   14.27%   -0.90%     
==========================================
  Files        1496     1571      +75     
  Lines      102981   108795    +5814     
  Branches    31621    31705      +84     
==========================================
- Hits        15623    15527      -96     
- Misses      70365    76369    +6004     
+ Partials    16993    16899      -94     

Flag	Coverage Δ		*Carryforward flag
differential	7.56% <0.00%> (?)
total	14.30% <ø> (-0.87%)	⬇️	Carriedforward from 01bf2c6

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

Impacted Files	Coverage Δ
...ity_intersection_module/src/scene_intersection.cpp	0.00% <0.00%> (ø)

... and 264 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

[beyzanurkaya]

@soblin friendly ping

[soblin]

@beyzanurkaya The size of the ego_polygon is critical for collision checking with NPCs, so can you add

const auto trimmed_ego_polygon = getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length);

? This should ensure that the trimmed polygon is not empty. The rest part looks good to me.

[beyzanurkaya]

@soblin The approach I add didn't use trimmed_ego_polygon. After adding the line you marked;

const auto trimmed_ego_polygon = getPolygonFromArcLength(ego_lane_with_next_lanes, start_arc_length, end_arc_length);

I don't quite understand where you want to use it. Can you give me more details?

[soblin]

My suggestion is to resurrect trimmed_ego_polygon variable with this one which is calculated from ego_lane_with_next_lanes.

[beyzanurkaya]

@soblin I made the changes you wanted and also I cleaned the code. i think everything is okay now.

[soblin]

LGTM from my side, thanks!