[Owen-Liuyuxuan]

Summary

• Port temporal MPC/PID/follower reference-mode from X2 (trajectory_reference_mode: resample by time, not arc length).
• Include the X2 follow-ups that temporal mode depends on (not optional extras after #2957):
  • PID velocity look-ahead (#3067)
  • Taubin curvature for short sequences (#3041)
  • MPC stop-hold / confidence steer slew + LPF sync (#3134, #3172)
  • Trajectory-steer feedforward via MPCTrajectory.steer (#3268)
• C++ / package YAML default remains spatial for standalone tests; E2E opts in via launcher #2190.

Why these land together

Temporal following without the later X2 patches snaps steer on spatially collapsed DP trajs, mis-estimates curvature on short horizons, and treats hesitation as a stop. Modifier/optimizer plugin flags that rewrite time_from_start are paired on the launcher PR, not isolated from this change.

Test plan

• colcon build --packages-select autoware_mpc_lateral_controller autoware_pid_longitudinal_controller autoware_trajectory_follower_node
• colcon test --packages-select autoware_mpc_lateral_controller autoware_pid_longitudinal_controller
• CI on this PR
• Pair with launcher temporal YAML — https://github.com/tier4/autoware_launch.xx1/pull/2190
• Vehicle/runtime — not in this pass

Notes for reviewers

• PID ff_scale_min/max (#2948) was already on XX1.
• Elapsed-time origin (#3138) is in the first temporal commit; do not re-apply #3050/#3072 as originally written.
• Not ported: steering_direct_passthrough / MPPI skip-QP (X2 default false).
• Source chain on feat/v0.64/e2e: #2957, #3041, #3067, #3134, #3138, #3172, #3268.

Stack

1. Checker — feat(traffic_light_compliance_checker): copy package from X2 e2e #3368
2. TrafficLightStop — feat(trajectory_modifier): adapt TrafficLightStop from X2 processor #3369
3. boundary-departure — feat(planning_validator): add uncrossable-boundary departure plugin #3370
4. this PR — temporal controller + follow-ups

Depends on #3370. Merge from the bottom.

[mergify]

Tick the box to add this pull request to the merge queue (same as @mergifyio queue).

• Queue this pull request

[Owen-Liuyuxuan]

Two changes are needed to keep this temporal-controller port scoped and regression-resistant.

[P2] Keep the Universe package default spatial. control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml:5 changes the package-level default to temporal. The exact X2 Universe source keeps this value spatial; launch PR #2190 already opts the XX1 E2E configuration into temporal mode. Changing both places silently changes standalone and non-E2E users of the package config, and temporal validation can reject legacy trajectories without strictly increasing timestamps. Please leave the Universe YAML at spatial and make #2190 the scoped E2E override, consistent with the PR summary.

Add the temporal unit coverage from X2. The port adds substantial interpolation/resampling behavior but does not copy the relevant source tests: MPC temporal convertToMPCTrajectory coverage (including short segments), and PID lerpTrajectoryPointByTime stop-hold/restart behavior. Those tests exercise the central semantics and boundary cases of this PR and should migrate with the implementation.

The explicitly omitted Taubin smoothing and trajectory-steer feedforward are not core temporal correctness blockers: Taubin mainly improves path/noise robustness, while steer feedforward requires extending XX1's MPCTrajectory representation. They should be tracked as quality/performance follow-ups rather than silently treated as migrated.

[N-Iemura]

Test result

I tested this changes with psim.

export VEHICLE_ID=5
ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/odaiba_stable vehicle_model:=jpntaxi sensor_model:=aip_xx1_gen2 planning_setting:=diffusion_planner

✅ Working correctly

• Planning Simulator started successfully with Diffusion Planner and Japantaxi 5.
• The vehicle could enter AUTO mode and drive on straight sections.

❌ Problem found

• The vehicle often failed to follow curves as shown in the attached video.
  • The controller repeatedly switched between DRIVE and STOPPING near curves.

Investigation

The suspected cause was the coincident trajectory points preserved by remove_close_points=false.

Diffusion Planner generates stop points at the same position with different time_from_start values. These points create a zero-length segment in calcStopDistance(). The calculation then returns NaN, which is converted to a stop distance of 0.0, causing the controller to incorrectly enter the STOPPING state.

To verify this, I changed only remove_close_points to true and drove the same route again.

remove_close_points=false	remove_close_points=true
The vehicle often failed to follow curves.	The vehicle followed the same curves successfully.
before.webm	after.webm

This problem is expected to be resolved by updating planning pipeline to XX1 maybe

[Owen-Liuyuxuan]

@N-Iemura I think in X2 planner, we are using remove_close_points: false, is there any mechanism in Controller setting that make controller failed?

[sonarqubecloud]

Quality Gate passed

Issues
17 New issues
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