feat(trajectory_follower): improve yaw control on curve

[kosuke55]

Signed-off-by: kosuke55 kosuke.tnp@gmail.com

Description

Improve yaw control on a curve

• calculate curvature from trajectory yaw
  • previously, only position is used.
• keep raw terminal yaw(not use previous point yaw)
• remove smooth_k

Related links

tier4 internal link

Tests performed

psim, real vehicle

before

mpc_yaw-2022-11-21_17.53.50.mp4

after

mpc_yaw-2022-11-21_17.50.13.mp4

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[codecov]

Codecov Report

Base: 11.10% // Head: 11.40% // Increases project coverage by +0.30% 🎉

Coverage data is based on head (715cbd7) compared to base (fd7add5).
Patch coverage: 61.29% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2338      +/-   ##
==========================================
+ Coverage   11.10%   11.40%   +0.30%     
==========================================
  Files        1208     1208              
  Lines       86573    87180     +607     
  Branches    20827    21283     +456     
==========================================
+ Hits         9612     9945     +333     
- Misses      66941    67097     +156     
- Partials    10020    10138     +118     

Flag	Coverage Δ		*Carryforward flag
differential	40.31% <35.00%> (?)
total	11.07% <60.00%> (ø)		Carriedforward from fd7add5

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

Impacted Files	Coverage Δ
...ctory_follower/include/trajectory_follower/mpc.hpp	55.55% <ø> (+8.88%)	⬆️
...wer/include/trajectory_follower/mpc_trajectory.hpp	60.00% <ø> (+10.00%)	⬆️
control/trajectory_follower/src/mpc_trajectory.cpp	68.18% <0.00%> (-8.29%)	⬇️
control/trajectory_follower/src/mpc.cpp	55.30% <30.00%> (+6.90%)	⬆️
control/trajectory_follower/src/mpc_utils.cpp	63.83% <69.23%> (+5.78%)	⬆️
...trajectory_follower/src/mpc_lateral_controller.cpp	44.50% <100.00%> (+9.30%)	⬆️
control/trajectory_follower/test/test_mpc.cpp	61.64% <100.00%> (+4.82%)	⬆️
...c/pure_pursuit/pure_pursuit_lateral_controller.cpp	0.00% <0.00%> (ø)
... and 5 more

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[TakaHoribe]

We still need curvature_smoothing_num_traj param for the smoothing of the feedforward term.

[kosuke55]

deal with #2447