feat(obstacle_avoidance_planner): better boundary search for MPT

[takayuki5168]

Related Issue(required)

Description(required)

When there are multiple candidates of bounds (e.g. there is a vehicle to be avoided in the middle of the lane, and we're not sure which way to avoid (right or left)), sometimes discontinuity of boundary along the trajectory occurs.

Therefore, we pick up the boundary with the following steps.

• Step 1. check if bounds is continuous to the previous one along the trajectory
  • Since there is distortion due to the trajectory's curvature, we just check if current and previous one are overlapped.
• Step 2. check if bounds is longer than vehicle width.
• Step 3. pick nearest bounds to the trajectory from valid ones

This is the usecase which failes without this PR.

• Before this PR
  

• After this PR
  

Review Procedure(required)

Related PR(optional)

Pre-Review Checklist for the PR Author

PR Author should check the checkboxes below when creating the PR.

• Read commit-guidelines
• Assign PR to reviewer

If you are adding new package following items are required:

• Documentation with description of the package is available
• A sample launch file and parameter file are available if the package contains executable nodes

Checklist for the PR Reviewer

Reviewers should check the checkboxes below before approval.

• Commits are properly organized and messages are according to the guideline
• PR title describes the changes

Post-Review Checklist for the PR Author

PR Author should check the checkboxes below before merging.

• All open points are addressed and tracked via issues or tickets

CI Checks

• Build and test for PR / build-and-test-pr: Required to pass before the merge.
• Build and test for PR / clang-tidy-pr: NOT required to pass before the merge. It is up to the reviewer(s). Found false positives? See the [guidelines][clang-tidy-guidelines].
• Check spelling: NOT required to pass before the merge. It is up to the reviewer(s). See here if you want to add some words to the spell check dictionary.

[codecov]

Codecov Report

Merging #538 (ee9dfff) into main (3108a01) will decrease coverage by 10.98%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main    #538       +/-   ##
==========================================
- Coverage   10.98%   0.00%   -10.99%     
==========================================
  Files         708      16      -692     
  Lines       49726    2805    -46921     
  Branches     7128       0     -7128     
==========================================
- Hits         5462       0     -5462     
+ Misses      39878    2805    -37073     
+ Partials     4386       0     -4386     

Impacted Files	Coverage Δ
...g/obstacle_avoidance_planner/src/mpt_optimizer.cpp	0.00% <0.00%> (ø)
...lidar_apollo_instance_segmentation/feature_map.hpp
common/fake_test_node/src/fake_test_node.cpp
...toware_auto_common/test/test_float_comparisons.cpp
...er/scene_module/scene_module_bt_node_interface.hpp
...terpolation/src/spline_interpolation_points_2d.cpp
...nner/include/utilization/boost_geometry_helper.hpp
...ower_nodes/test/trajectory_follower_test_utils.hpp
...ehavior_path_planner/src/behavior_tree_manager.cpp
... and 684 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 3108a01...ee9dfff. Read the comment docs.

[tkimura4]

I confirned the obstacle_avoidance_planner works well.

[tkimura4]

LGTM