fix(autoware_path_optimizer): incorrect application of input velocity due to badly mapping output trajectory to input trajectory

[PanConChicharron]

Description

Core changes for autowarefoundation/autoware_universe#10403

Adds a yaw-constrained search for the findFirstNearestIndexWithSoftConstraints function that prevents mapping poses to trajectory points with large yaw deviations.

Related links

Parent Issue:

• Link

How was this PR tested?

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[PanConChicharron]

build-and-test-differential fails, but the test runs successfully on my local PC:

colcon test --ctest-args --rerun-failed --output-on-failure --packages-above autoware_motion_utils

Starting >>> autoware_motion_utils
Finished <<< autoware_motion_utils [0.89s]

Summary: 1 package finished [1.75s]

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 35.89%. Comparing base (4cb18f5) to head (70a15fd).
Report is 158 commits behind head on main.

Additional details and impacted files

@@             Coverage Diff             @@
##             main     #355       +/-   ##
===========================================
- Coverage   78.75%   35.89%   -42.87%     
===========================================
  Files          11      179      +168     
  Lines         193    11224    +11031     
  Branches       73     3953     +3880     
===========================================
+ Hits          152     4029     +3877     
- Misses         11     6783     +6772     
- Partials       30      412      +382     

Flag	Coverage Δ
differential	35.89% <100.00%> (?)
total	?

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

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[youtalk]

LGTM

[kosuke55]

( the PR title should have been like feat(motion_utils): add yaw constraint ... . I thought path_optimizer is in autoware_core )