feat(goal_planner): calculate stop pose from closest goal candidate

[kosuke55]

Description

currently the stop pose when path is not found is caculated from goal search start pose.
this include invalid goal pose like in no_parking_lane and stop too far.
In this PR, calculate stop pose from actual goal candidates which do not include no_parking_lane goal.

before

after

Related links

this may solve degradation of #5371 (comment)

Tests performed

psim

evaluator_description: feat/goal_planner_stop_pose
2023/10/25 https://evaluation.tier4.jp/evaluation/reports/8bfda284-da50-5c03-ac67-a2f834d6c731/?project_id=prd_jt

evaluator_description: feat/goal_planner_stop_pose
2023/10/26 https://evaluation.tier4.jp/evaluation/reports/04de2c84-3b7f-5299-b0b1-9eb0d98f6721/?project_id=prd_jt

Notes for reviewers

Interface changes

none

Effects on system behavior

none

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

Comparison is base (98fe02b) 14.75% compared to head (0e3681f) 15.09%.
Report is 4 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5410      +/-   ##
==========================================
+ Coverage   14.75%   15.09%   +0.33%     
==========================================
  Files        1661     1661              
  Lines      115526   115549      +23     
  Branches    35669    35685      +16     
==========================================
+ Hits        17049    17443     +394     
+ Misses      79232    78589     -643     
- Partials    19245    19517     +272     

Flag	Coverage Δ		*Carryforward flag
differential	14.95% <17.30%> (?)
total	14.75% <ø> (-0.01%)	⬇️	Carriedforward from 98fe02b

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

Files	Coverage Δ
...king/include/autonomous_emergency_braking/node.hpp	0.00% <ø> (ø)
control/autonomous_emergency_braking/src/node.cpp	0.00% <ø> (ø)
...ased_localizer/src/ar_tag_based_localizer_core.cpp	0.00% <ø> (ø)
..._planner/utils/goal_planner/goal_searcher_base.hpp	20.00% <ø> (ø)
...nner/include/behavior_path_planner/utils/utils.hpp	45.45% <ø> (ø)
.../scene_module/goal_planner/goal_planner_module.hpp	27.77% <0.00%> (-0.53%)	⬇️
planning/behavior_path_planner/src/utils/utils.cpp	17.64% <16.66%> (+3.89%)	⬆️
...h_planner/src/utils/goal_planner/goal_searcher.cpp	1.32% <0.00%> (-0.09%)	⬇️
.../scene_module/goal_planner/goal_planner_module.cpp	9.37% <26.92%> (+4.10%)	⬆️

... and 10 files with indirect coverage changes

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

[kyoichi-sugahara]

LGTM!