fix(static_drivable_area_expansion): fix bug in drivable bound edge processing logic

[satoshi-ota]

Description

Fix #5825.
I found a bug in drivable bound edge(start/goal point) processing logic.

This function got unexpected point index since there is no constraints. As a result, left bound was not built properly.

size_t findNearestSegmentIndexFromLateralDistance(
  const std::vector<T> & points, const geometry_msgs::msg::Point & target_point)

In this PR, I added yaw threshold so that it can get proper point index.

Tests performed

Psim

simplescreenrecorder-2023-12-21_11.51.44.mp4

Effects on system behavior

Nothing.

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.

[brkay54]

Thank you for the work @satoshi-ota -san, I tried the PR on the master, the avoidance module runs as we want. LGTM!

[satoshi-ota]

@brkay54 I'm happy to hear that. This PR will be reviewed by TIER IV internal member. cc @danielsanchezaran @soblin

[shmpwk]

LGTM

[danielsanchezaran]

LGTM

[codecov]

Codecov Report

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

Comparison is base (1c5f38a) 15.28% compared to head (1cdc572) 15.28%.
Report is 4 commits behind head on main.

Files	Patch %	Lines
...s/drivable_area_expansion/static_drivable_area.cpp	11.76%	2 Missing and 13 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5928      +/-   ##
==========================================
- Coverage   15.28%   15.28%   -0.01%     
==========================================
  Files        1750     1750              
  Lines      120392   120577     +185     
  Branches    36729    36869     +140     
==========================================
+ Hits        18404    18427      +23     
- Misses      81335    81481     +146     
- Partials    20653    20669      +16     

Flag	Coverage Δ		*Carryforward flag
differential	11.69% <11.76%> (?)
total	15.28% <ø> (-0.01%)	⬇️	Carriedforward from 1c5f38a

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