[PanConChicharron]

Description

When applying the input velocity to the output of MPT, the initial pose of the output of MPT is used to trim the input trajectory. In certain cases when there is a sharp U-turn, depending on the width of the adjacent, opposing lane into which Ego is turning into, the initial pose of the output trajectory may be mapped to a point in the opposing lane - leading to incorrect input velocity application.

This PR adds a yaw-thresholded closest-distance point search step to ensure that this doesn't occur and also trims the output of MPT to start from the current ego pose so that the initial pose of the MPT output will always map to a point within the current lane.

Related links

Parent Issue:
https://tier4.atlassian.net/browse/RT0-35764

How was this PR tested?

Results in Ticket.

Evaluator Result:

Evaluator results borrowed from:
autowarefoundation#10403

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[rej55]

@PanConChicharron cc @s-azumi
According to following link, it seems the test for autoware_motion_utils fails.
Could you check it?
https://github.com/tier4/autoware_universe/actions/runs/14400775438/job/40385880775?pr=1954

2025-04-11T11:40:38.8472802Z 1: [ RUN      ] trajectory.findFirstNearestIndexWithSoftConstraints
2025-04-11T11:40:38.8473519Z 1: /__w/autoware_universe/autoware_universe/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp:4774: Failure
2025-04-11T11:40:38.8474148Z 1: Expected equality of these values:
2025-04-11T11:40:38.8474635Z 1:   findFirstNearestIndexWithSoftConstraints( poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4)
2025-04-11T11:40:38.8475159Z 1:     Which is: 5
2025-04-11T11:40:38.8475348Z 1:   1U
2025-04-11T11:40:38.8475516Z 1:     Which is: 1
2025-04-11T11:40:38.8475993Z 1: /__w/autoware_universe/autoware_universe/common/autoware_motion_utils/test/src/trajectory/test_trajectory.cpp:4778: Failure
2025-04-11T11:40:38.8476546Z 1: Expected equality of these values:
2025-04-11T11:40:38.8477049Z 1:   findFirstNearestSegmentIndexWithSoftConstraints( poses, createPose(-2.1, 0.1, 0.0, 0.0, 0.0, pi), 0.0, 0.4)
2025-04-11T11:40:38.8477541Z 1:     Which is: 5
2025-04-11T11:40:38.8477719Z 1:   0U
2025-04-11T11:40:38.8477875Z 1:     Which is: 0
2025-04-11T11:40:38.8478186Z 1: [  FAILED  ] trajectory.findFirstNearestIndexWithSoftConstraints (0 ms)

[rej55]

@PanConChicharron cc @takayuki5168
Thank you for fix.
Let me confirm. You changed the test code but is it okay?
Is the design of findFirstNearestSegmentIndexWithSoftConstraints() changed?

[takayuki5168]

@rej55

Is the design of findFirstNearestSegmentIndexWithSoftConstraints() changed?

Yes. All the Planning/Control team members agreed with this change.
However, we are not 100% sure that this change will not have a bad impact unexpectedly, so we'll continue seeing how this change works after the PR is merged.

[rej55]

@takayuki5168 It's okay if you agreed this change.
I will merge this PR.

[takayuki5168]

@PanConChicharron @s-azumi
As a hotfix, we want to apply this change to the S1 branch which had an issue.
Is the target branch of this PR (beta/v0.39) correct?
https://tier4.atlassian.net/browse/RT0-35764

[PanConChicharron]

@PanConChicharron @s-azumi As a hotfix, we want to apply this change to the S1 branch which had an issue. Is the target branch of this PR (beta/v0.39) correct? https://tier4.atlassian.net/browse/RT0-35764

I believe so, since the branch at fault mentioned in the ticket is beta/v0.39

[s-azumi]

@PanConChicharron cc: @takayuki5168
At the time we were using beta/v0.39, but now we have a branch for S1.
If you need this change to only apply to S1, could you please change the merge target branch to beta/s1/v0.39.0?

[PanConChicharron]

@PanConChicharron cc: @takayuki5168 At the time we were using beta/v0.39, but now we have a branch for S1. If you need this change to only apply to S1, could you please change the merge target branch to beta/s1/v0.39.0?

I went ahead and changed the base branch.

[sonarqubecloud]

Quality Gate passed

Issues
1 New issue
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud

[takayuki5168]

@s-azumi
Please merge the PR when you think it's ready.