mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-01 03:22:07 +08:00
longitudinal maneuvers: add report for longitudinal mpc tuning (#37030)
This commit is contained in:
felsager
@@ -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() + '<br/>'
|
||||
|
||||
|
||||
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)
|
||||
Reference in New Issue
Block a user