feat(mpc_lateral_controller): visualize/correct the longitudinal and lateral deviations for pid and mpc

[HansOersted]

Description

The PID and MPC controllers are to stabilize the longitudinal velocity and lateral position, respectively.
Specifically, they intend to decrease the deviations (state - reference) or state error (reference -state).

This PR publishes the following deviations (state - reference) for evaluating the quality of the controllers:

1. /control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal
   The deviation of the longitudinal position is published.
2. /control/trajectory_follower/lane_departure_checker_node/debug/deviation/longitudinal_velocity
   The deviation of the longitudinal velocity is published.
3. /control/trajectory_follower/lateral/diagnostic
   The deviation of the lateral position (with respect to the actual reference feeding the optimization module) is corrected.
   The deviation of the lateral position (with respect to the original raw planned date feeding MPC.cpp) is added.

These topics can be further visualized by third-party app, e.g., plotjuggler.

Related links

Tests performed

Screencast.from.02-01-2024.11.25.03.AM.webm

Longitudidal position deviation:

Longitudidal velocity deviation:

Lateral position deviation:
( red: w.r.t. the raw reference feeding mpc.cpp. green: w.r.t. the actual reference feeding the optimization module.)

Notes for reviewers

Interface changes

Effects on system behavior

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.

[zulfaqar-azmi-t4]

Confirmed that the parameter is added.

Screencast.from.2024.02.01.16.23.37.webm

[zulfaqar-azmi-t4]

@HansOersted
Thank you very much for the PR.

Can you please update the documentation as well in the same PR so that if user read the documents, they will know what are the available topics they can use to debug, and what are the significance from observing the topic?

[HansOersted]

@HansOersted Thank you very much for the PR.

Can you please update the documentation as well in the same PR so that if user read the documents, they will know what are the available topics they can use to debug, and what are the significance from observing the topic?

description added a47aae1

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-6268/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-6268/control/mpc_lateral_controller/

[zulfaqar-azmi-t4]

You probably have to fix this part
If you view it in the generated documents, it is like this.
https://autowarefoundation.github.io/autoware.universe/pr-6268/control/mpc_lateral_controller/#:~:text=/control/trajectory_follower/lateral,mpc.cpp%20%7C%20%5B22%5D%20%7C

[HansOersted]

fixed in 6ba472a

[codecov]

Codecov Report

Attention: Patch coverage is 22.72727% with 17 lines in your changes missing coverage. Please review.

Project coverage is 14.79%. Comparing base (d5c7e61) to head (c7cef56).
Report is 2819 commits behind head on main.

Files with missing lines	Patch %	Lines
..._departure_checker_node/lane_departure_checker.cpp	0.00%	9 Missing ⚠️
...rture_checker_node/lane_departure_checker_node.cpp	0.00%	4 Missing ⚠️
control/mpc_lateral_controller/src/mpc.cpp	55.55%	0 Missing and 4 partials ⚠️

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #6268   +/-   ##
=======================================
  Coverage   14.79%   14.79%           
=======================================
  Files        1917     1917           
  Lines      131985   132003   +18     
  Branches    39228    39234    +6     
=======================================
+ Hits        19522    19529    +7     
- Misses      90673    90684   +11     
  Partials    21790    21790           

Flag	Coverage Δ		*Carryforward flag
differential	14.46% <22.72%> (?)
total	14.79% <ø> (+<0.01%)	⬆️	Carriedforward from c36e2ef

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

[takayuki5168]

@HansOersted
Let's run the internal scenario test before merging. I will teach how to do that next week.

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.

[xmfcx]

@HansOersted @takayuki5168 could you finish this task?

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.