feat(autoware_mpc_lateral_controller): add predicted trajectory acconts for input delay

[kyoichi-sugahara]

Description

merging this PR first.

Current Implementation Issue

The MPC optimization calculation uses an initial state that accounts for input delay. However, in the current implementation, when calculating the predicted trajectory from the obtained solution, it uses the currently observed state instead of the state that considers this delay. This creates an inconsistency between the optimization calculation and the trajectory prediction.

New Visualization Option

To verify the accuracy of following the reference trajectory, I have added an option to visualize the resampled reference trajectory.

Initial Findings

• Initial verification results suggest that the predicted trajectory considering delay follows the target path more accurately.
• As the steering angle increases, the difference between the predicted trajectories with and without delay consideration tends to become larger.

Note: Further verification is needed to confirm these initial findings.

Line Color	Description	Coordinate	Topic Name
🔵 Blue (dotted)	Reference trajectory	World	/control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory
⚪ White	Predicted trajectory	World	/control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_with_delay
🔴 Red	Predicted trajectory (with delay consideration)	World	/control/trajectory_follower/lateral/predicted_trajectory

How was this PR tested?

by visualizing predicted trajectory w/wo delay and reference trajectory

2024-08-10_22.09.13.mp4

• PASS TIERVI INTERNAL SCENARIOS

Notes for reviewers

Ideally, we should interpolate the trajectory from the current state to the state considering input delay, and then generate the predicted trajectory from this delay-considered state. Please note that the trajectory visualized in this PR generates the predicted trajectory directly from the delay-considered state without performing this interpolation.

ROS Parameter Changes

Additions and removals

Change type	Parameter Name	Type	Default Value	Description
Added	debug_publish_resampled_reference_trajectory	bool	false	Publish debug resampled reference trajectory

[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.

[takayuki5168]

Is there any reason not to apply the input delay to the official predicted trajectory?

[kyoichi-sugahara]

@takayuki5168
Based on my understanding, it's better to apply the input delay for the official predicted path. In my additional tests, I've found that when reducing steer_tau, the original predicted path tends to oscillate, whereas the predicted path that considers delay follows the reference path more closely.

However, as an answer to the question, there are no reasons other than conservative ones. The main concern is that the change could affect the validation functionality in the latter stages. Therefore, I am proposing a configuration that allows for visualization for comparison purposes.

Do you think we should adopt a path that considers input_delay as the official predicted path?

[takayuki5168]

Currently, the control_validator and lane_departure_checker which depend on the predicted path are not used so much, so I think we do not have to be sensitive to that. This is not my strong opinion, but I feel that changing sooner is better.

[kyoichi-sugahara]

@takayuki5168
How about adopting the predicted path that considers input delay as the official version, while keeping an option to revert to the original predicted path just in case? I've done some light verification on my end by changing vehicle parameters and trying several rosbags, and it seems fine, but it's better to be prepared.

[takayuki5168]

I agree.

[kyoichi-sugahara]

@takayuki5168
addressed in c10f9fd

[takayuki5168]

Sounds like considering the input delay will have more accuracy. Any reason not to consider the input delay here?

[codecov]

Codecov Report

Attention: Patch coverage is 90.47619% with 2 lines in your changes missing coverage. Please review.

Project coverage is 24.95%. Comparing base (301e36b) to head (17a4b8a).
Report is 11 commits behind head on main.

Files with missing lines	Patch %	Lines
...ontrol/autoware_mpc_lateral_controller/src/mpc.cpp	89.47%	1 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8436      +/-   ##
==========================================
- Coverage   25.02%   24.95%   -0.08%     
==========================================
  Files        1322     1325       +3     
  Lines       98047    98438     +391     
  Branches    37795    37896     +101     
==========================================
+ Hits        24536    24562      +26     
- Misses      71020    71382     +362     
- Partials     2491     2494       +3     

Flag	Coverage Δ		*Carryforward flag
differential	33.87% <90.47%> (?)
total	25.02% <ø> (+<0.01%)	⬆️	Carriedforward from 301e36b

*This pull request uses carry forward flags. Click here to find out more.

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