feat(accel_brake_map_calibrator): add new Recursive Least Square method

[td12321]

Description

For accel_map_calibrator, implemented a new method to execute Recursive Least Squares on 4 points around new data.

Related links

Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845

Tests performed

run accel_brake_map_calibrator and play a rosbag data.

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.

[TakaHoribe]

I rebased to autowarefoundation/main. The original code was saved in my fork just for in case.
https://github.com/TakaHoribe/autoware.universe/tree/feature/improve_map_calibrator-ORIGINAL

[codecov]

Codecov Report

Base: 10.82% // Head: 10.36% // Decreases project coverage by -0.46% ⚠️

Coverage data is based on head (f08ea59) compared to base (e009b13).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1255      +/-   ##
==========================================
- Coverage   10.82%   10.36%   -0.47%     
==========================================
  Files        1178     1170       -8     
  Lines       84624    84005     -619     
  Branches    19947    19581     -366     
==========================================
- Hits         9159     8704     -455     
- Misses      65756    65795      +39     
+ Partials     9709     9506     -203     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	10.39% <0.00%> (-0.41%)	⬇️	Carriedforward from af09aa7

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

Impacted Files	Coverage Δ
...map_calibrator/accel_brake_map_calibrator_node.hpp	0.00% <ø> (ø)
...calibrator/src/accel_brake_map_calibrator_node.cpp	0.00% <0.00%> (ø)
...lude/freespace_planning_algorithms/reeds_shepp.hpp	50.00% <0.00%> (-50.00%)	⬇️
.../freespace_planning_algorithms/src/reeds_shepp.cpp	55.51% <0.00%> (-38.97%)	⬇️
...ehicle/raw_vehicle_cmd_converter/src/brake_map.cpp	17.85% <0.00%> (-27.31%)	⬇️
...eespace_planning_algorithms/abstract_algorithm.hpp	44.44% <0.00%> (-25.56%)	⬇️
...lude/behavior_path_planner/turn_signal_decider.hpp	25.00% <0.00%> (-25.00%)	⬇️
...ehicle/raw_vehicle_cmd_converter/src/accel_map.cpp	19.23% <0.00%> (-20.06%)	⬇️
...hicle/raw_vehicle_cmd_converter/src/csv_loader.cpp	53.84% <0.00%> (-11.07%)	⬇️
...ace_planning_algorithms/src/abstract_algorithm.cpp	78.16% <0.00%> (-5.54%)	⬇️
... and 173 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]

There are so many if-condition for accel_mode in this function.
Please define variables for the accel_mode at the top of the function, and remove the if-condition from the algorithm code, like below:

before

  const double zll = accel_mode ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index)
                                : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index);
  const double zhl = accel_mode
                       ? update_accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index)
                       : update_brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index);
  const double zlh = accel_mode
                       ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1)
                       : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1);
  const double zhh = accel_mode
                       ? update_accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index + 1)
                       : update_brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index + 1);

  ...

  if (accel_mode) {
    update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) =
      accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + map_offset(0);
    update_accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index) =
      accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index) + map_offset(1);
    update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1) =
      accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1) + map_offset(2);
    update_accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index + 1) =
      accel_map_value_.at(accel_pedal_index + 1).at(accel_vel_index + 1) + map_offset(3);
  } else {
    update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index) =
      brake_map_value_.at(brake_pedal_index).at(brake_vel_index) + map_offset(0);
    update_brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index) =
      brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index) + map_offset(1);
    update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1) =
      brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1) + map_offset(2);
    update_brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index + 1) =
      brake_map_value_.at(brake_pedal_index + 1).at(brake_vel_index + 1) + map_offset(3);
  }

after

  auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_;
  const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_;
  const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index;
  const auto & vel_index = accel_mode ? accel_vel_index : brake_vel_index;

  const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0);
  const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0);
  const double zlh = update_map_value.at(pedal_index + 0).at(vel_index + 1);
  const double zhh = update_map_value.at(pedal_index + 1).at(vel_index + 1);

  ...

  update_map_value.at(pedal_index + 0).at(vel_index + 0) = map_value.at(pedal_index + 0).at(vel_index + 0) + map_offset(0);
  update_map_value.at(pedal_index + 1).at(vel_index + 0) = map_value.at(pedal_index + 1).at(vel_index + 0) + map_offset(1);
  update_map_value.at(pedal_index + 0).at(vel_index + 1) = map_value.at(pedal_index + 0).at(vel_index + 1) + map_offset(2);
  update_map_value.at(pedal_index + 1).at(vel_index + 1) = map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3);

[stale]

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

[shmpwk]

spell miss
https://github.com/autowarefoundation/autoware.universe/actions/runs/3239609749/jobs/5309193223

I fixed it by
a39884b

[shmpwk]

LGTM!

accel_map.csv
brake_map.csv

(For TIER IV internal infomation)
I calibrated them using the 10min rosbags (2022.10.12) from dd0a1334-760f-4ed1-84dc-9d12cf23baf5_2022-10-12-16-24-14_0.db3 to dd0a1334-760f-4ed1-84dc-9d12cf23baf5_2022-10-12-16-24-14_10.db3

[tkimura4]

LGTM