feat(control_performance_analysis): add new functionalities to evaluate the control modules

[brkay54]

Description

Please see (#567).

To close this issue, I want to add new functionalities to control_performance_analysis package.

New Design:

The outputs which are marked with * will be added.

With this design, we can monitor the motion variables of vehicle (like longitudinal acceleration, longitudinal jerk, lateral jerk etc.). For BUS ODD, we need to stay public transportation limits.

Transportation Limits

Referance

Also, with trajectory_generator_node, we will able to test our control modules with custom paths without any map data.

Outputs will be added

longitudinal_velocity_error: m/s

tracking_curvature_discontinuity_ability: It will be added to see how the curvature change affect the lateral error. It will be calculated by using:
delta(curvature) * (1 / (1 + delta(lateral_error)))
(If you have any suggestion please share with me.)

longitudinal_acceleration: m/s^2

longitudinal_jerk: m/s^3

lateral_acceleration: m/s^2

lateral_jerk: m/s^3

controller_processing_time: ms

Related links

Tests performed

Notes for reviewers

• Run the planning simulator
• Install plotjuggler to visualize the outputs.
• Run the control_performance_analysis tool: $ ros2 launch control_performance_analysis controller_performance_analysis.launch.xml
• Run plotjuggler and import the layout in config/controller_monitor.xml

Now you should see the all outputs. In Cost Outputs tab, you will see the total errors. After test, you can export the statistic from plotjuggler by using CVS exporter, please select Export Statistics. In CVS file, you will see the average, maximum and minimum values of errors and driving monitor outputs. You can compare the controller modules by using these values. Also you can determine the time range by using XY Position tab.

I did not add the trajectory generator module because it may not be necessary to compare control modules. But it can be added in future to use this tool with custom generated path without any map data.

If you have any suggestion please feel free to share with me.

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

Merging #659 (89b9874) into main (2bbb23a) will increase coverage by 0.08%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main    #659      +/-   ##
========================================
+ Coverage   9.53%   9.61%   +0.08%     
========================================
  Files        970     960      -10     
  Lines      65278   64645     -633     
  Branches   11581   11183     -398     
========================================
- Hits        6221    6217       -4     
+ Misses     53893   53265     -628     
+ Partials    5164    5163       -1     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	9.63% <0.00%> (+0.12%)	⬆️	Carriedforward from e5579ea

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

Impacted Files	Coverage Δ
...ce_analysis/control_performance_analysis_utils.hpp	0.00% <0.00%> (ø)
...analysis/src/control_performance_analysis_core.cpp	0.00% <0.00%> (ø)
...analysis/src/control_performance_analysis_node.cpp	0.00% <0.00%> (ø)
...nalysis/src/control_performance_analysis_utils.cpp	0.00% <ø> (ø)
common/grid_map_utils/src/polygon_iterator.cpp	58.87% <0.00%> (-4.68%)	⬇️
...est_node/include/fake_test_node/fake_test_node.hpp	39.13% <0.00%> (ø)
...ner/src/scene_module/occlusion_spot/grid_utils.cpp	0.00% <0.00%> (ø)
... and 12 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 9e846c6...89b9874. Read the comment docs.

[TakaHoribe]

@brkay54 Thank you for the PR. The basic design looks good to me.

I have some questions:

• what does the tracking_curvature_discontinuity_ability mean?
• controller_processing_time is in the output of the control_performance_analysis, but it needs to be calculated in the control module itself.

FYI, some values are also calculated in the planning_evaluator node.

[brkay54]

* what does the `tracking_curvature_discontinuity_ability` mean?

I didn't explain it sorry for that. Actually, we want to measure the capability to deal with curvature discontinuity. It will be scored with: delta(curvature) * (1 / (1 + delta(lateral_error))) (but I'm not sure with this equation, I need to think about it more. If you have any suggestion please share with me.)

* `controller_processing_time` is in the output of the `control_performance_analysis`, but it needs to be calculated in the control module itself.

Yes you are right. I will change this output name. It will measure the time between the control messages.

FYI, some values are also calculated in the planning_evaluator node.

Thank you, I will check there.

[brkay54]

cc @xmfcx @maxime-clem @mitsudome-r @MertClk

In current BUS ODD architecture, we have nav_msgs::Odometry output from localization stack. See here.

There is no measured acceleration output there. However in localization discussion, people decided to (I guess) add measured acceleration output to localization stack. Please see here.

Issue #567 was labeled with BUS ODD. It is little confusing. Should I assume there will be measured acceleration output from localization stack? Otherwise, I will calculate the estimated acceleration and I will evaluate the control stack with that acceleration.

[xmfcx]

cc @xmfcx @maxime-clem @mitsudome-r @MertClk

In current BUS ODD architecture, we have nav_msgs::Odometry output from localization stack. See here.

There is no measured acceleration output there. However in localization discussion, people decided to (I guess) add measured acceleration output to localization stack. Please see here.

Issue #567 was labeled with BUS ODD. It is little confusing. Should I assume there will be measured acceleration output from localization stack? Otherwise, I will calculate the estimated acceleration and I will evaluate the control stack with that acceleration.

We are working on a new message for the localization output, will post the PR here tomorrow.

[brkay54]

@TakaHoribe @boyali it is ready for review.

[boyali]

I think we need to recheck error accelerations, there are some errors in formulations maybe coming from the original.

core - line 259

error_vars.error.lateral_error_acceleration =
Vx * tan(current_steering_val) / wheelbase_ - curvature_est * Vx;
this expression is epsi_dot. I will post my notes from discord channel.

[brkay54]

@maxime-clem @boyali It is ready for review, instructions are written in readme.md. Can you try the evaluation tool? If you see any problem please share with me.

[maxime-clem]

I could test the updated control_performance_analysis following the instructions and it works well.
I only had issues selecting the necessary topics in plotjuggler. Otherwise the plots look good and the statistics can be exported to CSV.

[brkay54]

@xmfcx merge please?