From 97329e46ae11b92cf7f82fed485e5e141be6dfe2 Mon Sep 17 00:00:00 2001 From: felsager <76905857+felsager@users.noreply.github.com> Date: Mon, 26 Jan 2026 16:07:13 -0800 Subject: [PATCH] longitudinal maneuvers: add report for longitudinal mpc tuning (#37030) --- .../mpc_longitudinal_tuning_report.py | 276 ++++++++++++++++++ 1 file changed, 276 insertions(+) create mode 100644 tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py diff --git a/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py new file mode 100644 index 000000000..583c6240e --- /dev/null +++ b/tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py @@ -0,0 +1,276 @@ +import io +import sys +import markdown +import numpy as np +import matplotlib.pyplot as plt +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver +from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance + +TIME = 0 +EGO_V = 3 +EGO_A = 5 +LEAD_DISTANCE= 2 + +axis_labels = ['Time (s)', + 'Ego position (m)', + 'Lead distance (m)', + 'Ego Velocity (m/s)', + 'Lead Velocity (m/s)', + 'Ego acceleration (m/s^2)', + ] + + +def get_html_from_results(results, labels, AXIS): + fig, ax = plt.subplots(figsize=(16, 8)) + for idx, speed in enumerate(list(results.keys())): + ax.plot(results[speed][:, TIME], results[speed][:, AXIS], label=labels[idx]) + + ax.set_xlabel('Time (s)') + ax.set_ylabel(axis_labels[AXIS]) + ax.legend(bbox_to_anchor=(1.02, 1), loc='upper left', borderaxespad=0) + ax.grid(True, linestyle='--', alpha=0.7) + ax.text(-0.075, 0.5, '.', transform=ax.transAxes, color='none') + + fig_buffer = io.StringIO() + fig.savefig(fig_buffer, format='svg', bbox_inches='tight') + plt.close(fig) + return fig_buffer.getvalue() + '
' + + +htmls = [] + +results = {} +name = 'Resuming behind lead' +labels = [] +for lead_accel in np.linspace(1.0, 4.0, 4): + man = Maneuver( + '', + duration=11, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 10 * lead_accel], + cruise_values=[100, 100], + prob_lead_values=[1.0, 1.0], + breakpoints=[1., 11], + ) + valid, results[lead_accel] = man.evaluate() + labels.append(f'{lead_accel} m/s^2 lead acceleration') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, EGO_A)) + + +results = {} +name = 'Approaching stopped car from 140m' +labels = [] +for speed in np.arange(0,45,5): + man = Maneuver( + name, + duration=30., + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=140., + speed_lead_values=[0.0, 0.], + breakpoints=[0., 30.], + ) + valid, results[speed] = man.evaluate() + results[speed][:,2] = results[speed][:,2] - results[speed][:,1] + labels.append(f'{speed} m/s approach speed') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_A)) +htmls.append(get_html_from_results(results, labels, LEAD_DISTANCE)) + + +results = {} +name = 'Following 5s oscillating lead' +labels = [] +speed = np.int64(10) +for oscil in np.arange(0, 10, 1): + man = Maneuver( + '', + duration=30., + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed), + speed_lead_values=[speed, speed, speed - oscil, speed + oscil, speed - oscil, speed + oscil, speed - oscil], + breakpoints=[0.,2., 5, 8, 15, 18, 25.], + ) + valid, results[oscil] = man.evaluate() + labels.append(f'{oscil} m/s oscilliation size') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, EGO_A)) + + + +results = {} +name = 'Speed profile when converging to steady state lead at 30m/s' +labels = [] +for distance in np.arange(20, 140, 10): + man = Maneuver( + '', + duration=50, + initial_speed=30.0, + lead_relevancy=True, + initial_distance_lead=distance, + speed_lead_values=[30.0], + breakpoints=[0.], + ) + valid, results[distance] = man.evaluate() + results[distance][:,2] = results[distance][:,2] - results[distance][:,1] + labels.append(f'{distance} m initial distance') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, LEAD_DISTANCE)) + + +results = {} +name = 'Speed profile when converging to steady state lead at 20m/s' +labels = [] +for distance in np.arange(20, 140, 10): + man = Maneuver( + '', + duration=50, + initial_speed=20.0, + lead_relevancy=True, + initial_distance_lead=distance, + speed_lead_values=[20.0], + breakpoints=[0.], + ) + valid, results[distance] = man.evaluate() + results[distance][:,2] = results[distance][:,2] - results[distance][:,1] + labels.append(f'{distance} m initial distance') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, LEAD_DISTANCE)) + + +results = {} +name = 'Following car at 30m/s that comes to a stop' +labels = [] +for stop_time in np.arange(4, 14, 1): + man = Maneuver( + '', + duration=50, + initial_speed=30.0, + lead_relevancy=True, + initial_distance_lead=60.0, + speed_lead_values=[30.0, 30.0, 0.0, 0.0], + breakpoints=[0., 20., 20 + stop_time, 30 + stop_time], + ) + valid, results[stop_time] = man.evaluate() + results[stop_time][:,2] = results[stop_time][:,2] - results[stop_time][:,1] + labels.append(f'{stop_time} seconds stop time') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_A)) +htmls.append(get_html_from_results(results, labels, LEAD_DISTANCE)) + + +results = {} +name = 'Response to cut-in at half follow distance' +labels = [] +for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=10, + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(speed, speed)/2, + speed_lead_values=[speed, speed, speed], + cruise_values=[speed, speed, speed], + prob_lead_values=[0.0, 0.0, 1.0], + breakpoints=[0., 5.0, 5.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_A)) +htmls.append(get_html_from_results(results, labels, LEAD_DISTANCE)) + + +results = {} +name = 'Follow a lead that accelerates at 2m/s^2 until steady state speed' +labels = [] +for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=50, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0, speed], + prob_lead_values=[1.0, 1.0, 1.0], + breakpoints=[0., 1.0, speed/2], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, EGO_A)) + + +results = {} +name = 'From stop to cruise' +labels = [] +for speed in np.arange(0, 40, 5): + man = Maneuver( + '', + duration=50, + initial_speed=0.0, + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0], + cruise_values=[0.0, speed], + prob_lead_values=[0.0, 0.0], + breakpoints=[1., 1.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, EGO_A)) + + +results = {} +name = 'From cruise to min' +labels = [] +for speed in np.arange(10, 40, 5): + man = Maneuver( + '', + duration=50, + initial_speed=float(speed), + lead_relevancy=True, + initial_distance_lead=desired_follow_distance(0.0, 0.0), + speed_lead_values=[0.0, 0.0], + cruise_values=[speed, 10.0], + prob_lead_values=[0.0, 0.0], + breakpoints=[1., 1.01], + ) + valid, results[speed] = man.evaluate() + labels.append(f'{speed} m/s speed') + +htmls.append(markdown.markdown('# ' + name)) +htmls.append(get_html_from_results(results, labels, EGO_V)) +htmls.append(get_html_from_results(results, labels, EGO_A)) + +if len(sys.argv) < 2: + file_name = 'long_mpc_tune_report.html' +else: + file_name = sys.argv[1] + +with open(file_name, 'w') as f: + f.write(markdown.markdown('# MPC longitudinal tuning report')) + +with open(file_name, 'a') as f: + for html in htmls: + f.write(html)