Longitudinal MPC: dont creep

[FreyJo]

I saw the creeping behavior in the first maneuver in test_longitudinal.py.
I addressed this in this PR by modifying get_safe_obstacle_distance for small speeds.
Below a certain speed, I chose v_min = .5 the follow distance should not decrease further.
Otherwise openpilot brakes and then wants to get closer since the desired distance gets smaller.

Plot:

Output of test_longitudinal.py
Before this PR:

13.00 sec   129.45 m    0.15 m/s   -0.19 m/s2   lead_rel:   6.30 m   -0.15 m/s
13.20 sec   129.48 m    0.12 m/s   -0.16 m/s2   lead_rel:   6.27 m   -0.12 m/s
13.40 sec   129.50 m    0.09 m/s   -0.13 m/s2   lead_rel:   6.25 m   -0.09 m/s
13.60 sec   129.51 m    0.06 m/s   -0.11 m/s2   lead_rel:   6.24 m   -0.06 m/s
13.80 sec   129.52 m    0.04 m/s   -0.08 m/s2   lead_rel:   6.23 m   -0.04 m/s
14.00 sec   129.53 m    0.03 m/s   -0.06 m/s2   lead_rel:   6.22 m   -0.03 m/s
14.20 sec   129.53 m    0.02 m/s   -0.05 m/s2   lead_rel:   6.22 m   -0.02 m/s
14.40 sec   129.54 m    0.01 m/s   -0.03 m/s2   lead_rel:   6.21 m   -0.01 m/s
14.60 sec   129.54 m    0.01 m/s   -0.02 m/s2   lead_rel:   6.21 m   -0.01 m/s
14.80 sec   129.54 m    0.00 m/s   -0.01 m/s2   lead_rel:   6.21 m   -0.00 m/s
15.00 sec   129.54 m    0.00 m/s   -0.01 m/s2   lead_rel:   6.21 m   -0.00 m/s
15.20 sec   129.54 m    0.00 m/s    0.00 m/s2   lead_rel:   6.21 m    0.00 m/s
15.40 sec   129.54 m    0.00 m/s    0.00 m/s2   lead_rel:   6.21 m   -0.00 m/s
15.60 sec   129.54 m    0.00 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.00 m/s
15.80 sec   129.54 m    0.00 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.00 m/s
16.00 sec   129.54 m    0.00 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.00 m/s
16.20 sec   129.54 m    0.01 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.01 m/s
16.40 sec   129.54 m    0.01 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.01 m/s
16.60 sec   129.54 m    0.01 m/s    0.01 m/s2   lead_rel:   6.21 m   -0.01 m/s
16.80 sec   129.55 m    0.01 m/s    0.01 m/s2   lead_rel:   6.20 m   -0.01 m/s
17.00 sec   129.55 m    0.02 m/s    0.01 m/s2   lead_rel:   6.20 m   -0.02 m/s
17.20 sec   129.55 m    0.02 m/s    0.01 m/s2   lead_rel:   6.20 m   -0.02 m/s
17.40 sec   129.56 m    0.02 m/s    0.01 m/s2   lead_rel:   6.19 m   -0.02 m/s
17.60 sec   129.56 m    0.02 m/s    0.01 m/s2   lead_rel:   6.19 m   -0.02 m/s
17.80 sec   129.57 m    0.02 m/s    0.01 m/s2   lead_rel:   6.18 m   -0.02 m/s
18.00 sec   129.57 m    0.03 m/s    0.01 m/s2   lead_rel:   6.18 m   -0.03 m/s
18.20 sec   129.58 m    0.03 m/s    0.01 m/s2   lead_rel:   6.17 m   -0.03 m/s
18.40 sec   129.58 m    0.03 m/s    0.01 m/s2   lead_rel:   6.17 m   -0.03 m/s
18.60 sec   129.59 m    0.03 m/s    0.01 m/s2   lead_rel:   6.16 m   -0.03 m/s
18.80 sec   129.59 m    0.03 m/s    0.01 m/s2   lead_rel:   6.16 m   -0.03 m/s
19.00 sec   129.60 m    0.03 m/s    0.00 m/s2   lead_rel:   6.15 m   -0.03 m/s
19.20 sec   129.61 m    0.03 m/s    0.00 m/s2   lead_rel:   6.14 m   -0.03 m/s
19.40 sec   129.61 m    0.04 m/s    0.00 m/s2   lead_rel:   6.14 m   -0.04 m/s
19.60 sec   129.62 m    0.04 m/s    0.00 m/s2   lead_rel:   6.13 m   -0.04 m/s
19.80 sec   129.63 m    0.04 m/s    0.00 m/s2   lead_rel:   6.12 m   -0.04 m/s

After this PR:

13.00 sec   129.06 m    0.11 m/s   -0.17 m/s2   lead_rel:   6.69 m   -0.11 m/s
13.20 sec   129.08 m    0.08 m/s   -0.14 m/s2   lead_rel:   6.67 m   -0.08 m/s
13.40 sec   129.09 m    0.06 m/s   -0.11 m/s2   lead_rel:   6.66 m   -0.06 m/s
13.60 sec   129.10 m    0.04 m/s   -0.08 m/s2   lead_rel:   6.65 m   -0.04 m/s
13.80 sec   129.11 m    0.02 m/s   -0.06 m/s2   lead_rel:   6.64 m   -0.02 m/s
14.00 sec   129.11 m    0.01 m/s   -0.04 m/s2   lead_rel:   6.64 m   -0.01 m/s
14.20 sec   129.11 m    0.01 m/s   -0.03 m/s2   lead_rel:   6.64 m   -0.01 m/s
14.40 sec   129.11 m    0.00 m/s   -0.02 m/s2   lead_rel:   6.64 m   -0.00 m/s
14.60 sec   129.11 m    0.00 m/s   -0.01 m/s2   lead_rel:   6.64 m   -0.00 m/s
14.80 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
15.00 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
15.20 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
15.40 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
15.60 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
15.80 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
16.00 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
16.20 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
16.40 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
16.60 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
16.80 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
17.00 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
17.20 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
17.40 sec   129.11 m    0.00 m/s   -0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
17.60 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
17.80 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
18.00 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
18.20 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
18.40 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
18.60 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
18.80 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
19.00 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
19.20 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
19.40 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
19.60 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s
19.80 sec   129.11 m    0.00 m/s    0.00 m/s2   lead_rel:   6.64 m   -0.00 m/s

[haraschax]

I don't think it's a good idea to change the follow distance function unless it has well tested clear improvements for long control in the field.