fix(behavior_path_planner): add angle limits to nearest point search

[tkimura4]

Signed-off-by: tomoya.kimura tomoya.kimura@tier4.jp

Description

When searching for closest point to the ego-vehicle in behavior_path_planner, there is no angle limit.
Therefore, the closest point could not be searched correctly for the intersecting paths.

In this PR, an angle limits is added to closest-point search to fix this bug.

With the PR

Without the PR

Related links

Tests performed

Notes for reviewers

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

Merging #595 (f6e2fe8) into main (2f3e335) will decrease coverage by 10.78%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #595       +/-   ##
==========================================
- Coverage   10.78%   0.00%   -10.79%     
==========================================
  Files         710      34      -676     
  Lines       50461    4943    -45518     
  Branches     6579       0     -6579     
==========================================
- Hits         5443       0     -5443     
+ Misses      40611    4943    -35668     
+ Partials     4407       0     -4407     

Impacted Files	Coverage Δ
...r/scene_module/avoidance/avoidance_module_data.hpp	0.00% <ø> (ø)
...or_path_planner/src/behavior_path_planner_node.cpp	0.00% <0.00%> (ø)
...nning/behavior_path_planner/src/path_utilities.cpp	0.00% <0.00%> (ø)
...er/src/scene_module/avoidance/avoidance_module.cpp	0.00% <0.00%> (ø)
.../src/scene_module/side_shift/side_shift_module.cpp	0.00% <0.00%> (ø)
...ontrol/trajectory_follower/test/test_mpc_utils.cpp
...e_auto_geometry/test/include/test_bounding_box.hpp
...erception/shape_estimation/lib/corrector/utils.cpp
...rocessor/passthrough_filter/passthrough_uint16.hpp
... and 672 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 982263a...f6e2fe8. Read the comment docs.

[yukkysaito]

cc @taikitanaka3 @takayuki5168
@taikitanaka3 has created a function to search, and I would like to use it.

#405
I would like to move this one to common utils for motion planner.

[TakaHoribe]

LGTM

oops. I missed the comment above.

I missed one comment

[tkimura4]

@yukkysaito

I would like to move this one to common utils for motion planner.

What function is it for?

[yukkysaito]

What function is it for?

Sorry, it is not enough.
When the path intersects, this function searches the neighborhood within the path before the intersection.

[tkimura4]

@yukkysaito
The behavior_path_planner already uses tier4_autoware_utils::findNearestIndex function.

[yukkysaito]

@tkimura4 findFirstNearSearchRangeIndex should be used. I will describe it in detail later.

[yukkysaito]

After discussion, a formal fix will be made in a separate PR.