fix(accel_brake_map_calibrator): fix accel_brake_map_calibrator not to output invalid maps

[tkimura4]

Description

I have fixed accel_brake_map_calibrator not to output invalid maps.
(
In particular, in previous implementations, there were cases where an invalid map would be outputted when the values of adjacent elements in the map were very close. I have fixed this issue. )

Related links

(TIERIV Internal Link) https://tier4.atlassian.net/browse/RT0-31507

Tests performed

Before:

An accel_brake_map_calibrator output invalid accel_map.csv

Output:

default,0,0.1,0.572158,1.04432,1.51647,1.98863,2.46079,2.93295,3.40511,3.87726,4.34942,4.82158,5.29374,5.76589,6.23805,6.71021,7.18237,7.65453,8.12668,8.59884,9.071
0,-0.13,-0.129,-0.163,-0.163,-0.165,-0.201,-0.227,-0.283,-0.333,-0.411,-0.465,-0.498,-0.499,-0.5,-0.553,-0.58,-0.581,-0.582,-0.583,-0.584,-0.585
0.001,-0.129,-0.128,-0.162,-0.162,-0.164,-0.2,-0.226,-0.282,-0.332,-0.41,-0.464,-0.497,-0.498,-0.499,-0.552,-0.579,-0.58,-0.581,-0.582,-0.583,-0.584
0.032,0.231,0.232,0.137,-0.026,-0.027,-0.051,-0.052,-0.053,-0.054,-0.055,-0.056,-0.057,-0.058,-0.059,-0.06,-0.061,-0.062,-0.063,-0.064,-0.065,-0.067
0.063,0.311,0.312,0.153,0.04,0.039,0.017,-0.035,-0.052,-0.053,-0.054,-0.055,-0.056,-0.057,-0.058,-0.059,-0.06,-0.061,-0.062,-0.063,-0.064,-0.066
0.093,0.312,0.314,0.167,0.095,0.094,0.083,0.063,0.04,0.039,0.038,0.037,0.036,0.035,0.034,0.033,0.032,0.031,0.03,0.029,0.028,0.027
0.124,0.313,0.314,0.222,0.096,0.095,0.086,0.064,0.041,0.04,0.039,0.038,0.037,0.036,0.035,0.034,0.033,0.032,0.031,0.03,0.029,0.028
0.155,0.4,0.401,0.337,0.097,0.096,0.087,0.075,0.074,0.073,0.072,0.071,0.07,0.069,0.068,0.067,0.066,0.065,0.064,0.063,0.062,0.061

After:

Even if an invalid accel_map is provided as input, the output of accel_brake_map_calibrator will be a valid file.

Input:

0,0.3,0.3,0.3,0.3001,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
0.1,0.3,0.3,0.3002,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
0.2,0.3,0.3,0.3,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61

Output:

default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
0,0.303,0.302,0.301,0.300,-0.400,-0.410,-0.420,-0.440,-0.460,-0.480,-0.500
0.100,0.305,0.303,0.302,0.301,0.120,0.050,-0.080,-0.160,-0.200,-0.240,-0.280

Notes for reviewers

Interface changes

none

ROS Topic Changes

none

ROS Parameter Changes

none

Effects on system behavior

accel_brake_map_calibrator becomes not to output invalid maps.

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.

[tkimura4]

Another Case

Input:

default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
0,0.3,0.29995,0.299993,0.29992,0.29,0.2895,0.28,0.27999,0.27998,0.27995,0.2799
0.1,0.3,0.29999,0.2994,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
0.2,0.3,0.3,0.3,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61

Output: (Before: invalid)

default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
0,0.300,0.299,0.292,0.291,0.290,0.289,0.280,0.280,0.280,0.280,0.280
0.100,0.301,0.300,0.299,0.298,0.291,0.290,0.281,0.281,0.281,0.281,-0.280
0.200,0.302,0.301,0.300,0.300,0.297,0.296,0.282,0.282,0.282,0.282,-0.030
0.300,1.750,1.600,1.420,1.300,1.140,1.000,0.900,0.800,0.720,0.640,0.580
0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100
0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610

Output: (After: valid)

default,0,1.39,2.78,4.17,5.56,6.94,8.33,9.72,11.11,12.5,13.89
0,0.303,0.302,0.301,0.300,0.290,0.289,0.284,0.283,0.282,0.281,0.280
0.100,0.304,0.303,0.302,0.301,0.291,0.290,0.285,0.284,0.283,0.282,0.281
0.200,0.603,0.602,0.601,0.600,0.480,0.340,0.286,0.285,0.284,0.283,0.282
0.300,1.750,1.600,1.420,1.300,1.140,1.000,0.900,0.800,0.720,0.640,0.580
0.400,2.650,2.480,2.300,2.130,1.950,1.750,1.580,1.450,1.320,1.200,1.100
0.500,3.300,3.250,3.120,2.920,2.680,2.350,2.170,1.980,1.880,1.730,1.610

[1222-takeshi]

LGTM