fix(behavior_path_planner): revival of skipped lane_ids by smooth goal connection

[takayuki5168]

Description

Smooth goal connection, described as follows, removes lane_ids within a certain-ranged circle whose center is the goal, which makes beahvior_velocity_planner not work.
https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/#smooth-goal-connection

Fixed the issue by filling removed lane_ids to pre_goal lane points.

This PR includes

• small refactoring (I thought this refactoring is related to the fix, but it's actually not, sorry about that).
• test for the issue

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.

[takayuki5168]

TODO

• check if all the lane ids are contained in the path, where some lane ids are in a certain-ranged circle whose center is goal.

[codecov]

Codecov Report

Merging #1626 (0e7b5ac) into main (9f451b4) will decrease coverage by 0.11%.
The diff coverage is 36.50%.

@@            Coverage Diff             @@
##             main    #1626      +/-   ##
==========================================
- Coverage   10.77%   10.65%   -0.12%     
==========================================
  Files        1111     1111              
  Lines       78530    79862    +1332     
  Branches    18554    19583    +1029     
==========================================
+ Hits         8464     8513      +49     
- Misses      61206    62372    +1166     
- Partials     8860     8977     +117     

Flag	Coverage Δ		*Carryforward flag
differential	1.39% <45.09%> (?)
total	10.76% <0.00%> (ø)		Carriedforward from 4556a7a

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

Impacted Files	Coverage Δ
...ning/behavior_path_planner/test/test_utilities.cpp	27.77% <22.22%> (-5.56%)	⬇️
planning/behavior_path_planner/src/utilities.cpp	3.94% <42.22%> (+2.89%)	⬆️
...nning/behavior_path_planner/src/path_utilities.cpp	0.00% <0.00%> (ø)
...r_path_planner/src/scene_module/pull_over/util.cpp	0.00% <0.00%> (ø)
...th_planner/src/scene_module/utils/path_shifter.cpp	0.00% <0.00%> (ø)
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
...er/src/scene_module/pull_over/pull_over_module.cpp	0.00% <0.00%> (ø)
...lanner/scene_module/avoidance/avoidance_module.hpp	0.00% <0.00%> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.34% <0.00%> (+0.11%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

[TakaHoribe]

LGTM. Nice work!