IQ.Lvbs/IQ.Pilot
8
7
Fork 3
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ 8627e35

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 10:33:03 -05:00
commit eea1278105
4608 changed files with 1126544 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
tools/longitudinal_maneuvers/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
/longitudinal_reports/

48
tools/longitudinal_maneuvers/README.md Normal file
View File

@@ -0,0 +1,48 @@
# Longitudinal Maneuvers Testing Tool
Test your vehicle's longitudinal control tuning with this tool. The tool will test the vehicle's ability to follow a few longitudinal maneuvers and includes a tool to generate a report from the route.
<details><summary>Sample snapshot of a report.</summary><img width="600px" src="https://github.com/user-attachments/assets/d18d0c7d-2bde-44c1-8e86-1741ed442ad8"></details>
## Instructions
1. Check out a development branch such as `master` on your comma device.
2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage openpilot between maneuvers and turn around if there is not enough space.
3. Turn off the vehicle and set this parameter which will signal to openpilot to start the longitudinal maneuver daemon:
```sh
echo -n 1 > /data/params/d/LongitudinalManeuverMode
```
4. Turn your vehicle back on. You will see the "Longitudinal Maneuver Mode" alert:
![videoframe_6652](https://github.com/user-attachments/assets/e9d4c95a-cd76-4ab7-933e-19937792fa0f)
5. Ensure the road ahead is clear, as openpilot will not brake for any obstructions in this mode. Once you are ready, press "Set" on your steering wheel to start the tests. The tests will run for about 4 minutes. If you need to pause the tests, press "Cancel" on your steering wheel. You can resume the tests by pressing "Resume" on your steering wheel.
**Note:** For GM cars, it is recommended to hold down the resume button for all low-speed tests (starting, stopping and creep) to avoid the car entering standstill.
![cog-clip-00 01 11 250-00 01 22 250](https://github.com/user-attachments/assets/c312c1cc-76e8-46e1-a05e-bb9dfb58994f)
6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle.
![fin2](https://github.com/user-attachments/assets/c06960ae-7cfb-44af-beaa-4dc28848e49d)
7. Visit https://connect.comma.ai and locate the route(s). They will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
![image](https://github.com/user-attachments/assets/cfe4c6d9-752f-4b24-b421-4b90a01933dc)
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
```sh
$ python tools/longitudinal_maneuvers/generate_report.py 57048cfce01d9625/0000010e--5b26bc3be7 'pcm accel compensation'
processing report for LEXUS_ES_TSS2
plotting maneuver: start from stop, runs: 4
plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
```
You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself.

187
tools/longitudinal_maneuvers/generate_report.py Executable file
View File

@@ -0,0 +1,187 @@
#!/usr/bin/env python3
import argparse
import base64
import io
import os
import math
import pprint
import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
from tabulate import tabulate
from openpilot.tools.lib.logreader import LogReader
from openpilot.system.hardware.hw import Paths
def format_car_params(CP):
return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2)
def report(platform, route, _description, CP, ID, maneuvers):
output_path = Path(__file__).resolve().parent / "longitudinal_reports"
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
output_path.mkdir(exist_ok=True)
target_cross_times = defaultdict(list)
builder = [
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
"<h1>Longitudinal maneuver report</h1>\n",
f"<h3>{platform}</h3>\n",
f"<h3>{route}</h3>\n",
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
]
if _description is not None:
builder.append(f"<h3>Description: {_description}</h3>\n")
builder.append(f"<details><summary><h3 style='display: inline-block;'>CarParams</h3></summary><pre>{format_car_params(CP)}</pre></details>\n")
builder.append('{ summary }') # to be replaced below
for description, runs in maneuvers:
print(f'plotting maneuver: {description}, runs: {len(runs)}')
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
builder.append(f"<h2>{description}</h2>\n")
for run, msgs in enumerate(runs):
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_livePose, livePose = zip(*[(m.logMonoTime, m.livePose) for m in msgs if m.which() == 'livePose'], strict=True)
t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True)
# make time relative seconds
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
t_livePose = [(t - t_livePose[0]) / 1e9 for t in t_livePose]
t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan]
# maneuver validity
longActive = [m.longActive for m in carControl]
maneuver_valid = all(longActive) and (not any(cs.cruiseState.standstill for cs in carState) or CP.autoResumeSng)
_open = 'open' if maneuver_valid else ''
title = f'Run #{int(run)+1}' + (' <span style="color: red">(invalid maneuver!)</span>' if not maneuver_valid else '')
builder.append(f"<details {_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
# get first acceleration target and first intersection
aTarget = longitudinalPlan[0].aTarget
target_cross_time = None
builder.append(f'<h3 style="font-weight: normal">Initial aTarget: {round(aTarget, 2)} m/s^2')
# Localizer is noisy, require two consecutive 20Hz frames above threshold
prev_crossed = False
for t, lp in zip(t_livePose, livePose, strict=True):
crossed = (0 < aTarget < lp.accelerationDevice.x) or (0 > aTarget > lp.accelerationDevice.x)
if crossed and prev_crossed:
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
target_cross_time = t
if maneuver_valid:
target_cross_times[description].append(t)
break
prev_crossed = crossed
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
pitches = [math.degrees(m.orientationNED[1]) for m in carControl]
builder.append(f'<h3 style="font-weight: normal">Average pitch: <strong>{sum(pitches) / len(pitches):0.2f} degrees</strong></h3>')
plt.rcParams['font.size'] = 40
fig = plt.figure(figsize=(30, 26))
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]})
ax[0].grid(linewidth=4)
ax[0].plot(t_carControl, [m.actuators.accel for m in carControl], label='carControl.actuators.accel', linewidth=6)
ax[0].plot(t_carOutput, [m.actuatorsOutput.accel for m in carOutput], label='carOutput.actuatorsOutput.accel', linewidth=6)
ax[0].plot(t_longitudinalPlan, [m.aTarget for m in longitudinalPlan], label='longitudinalPlan.aTarget', linewidth=6)
ax[0].plot(t_carState, [m.aEgo for m in carState], label='carState.aEgo', linewidth=6)
ax[0].plot(t_livePose, [m.accelerationDevice.x for m in livePose], label='livePose.accelerationDevice.x', linewidth=6)
# TODO localizer accel
ax[0].set_ylabel('Acceleration (m/s^2)')
#ax[0].set_ylim(-6.5, 6.5)
ax[0].legend(prop={'size': 30})
if target_cross_time is not None:
ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
ax[1].grid(linewidth=4)
ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6)
ax[1].set_ylabel('Velocity (m/s)')
ax[1].legend()
ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6)
ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6)
ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6)
for i in (2, 3):
ax[i].set_yticks([0, 1], minor=False)
ax[i].set_ylim(-1, 2)
ax[i].legend()
ax[-1].set_xlabel("Time (s)")
fig.tight_layout()
buffer = io.BytesIO()
fig.savefig(buffer, format='webp')
plt.close(fig)
buffer.seek(0)
builder.append(f"<img src='data:image/webp;base64,{base64.b64encode(buffer.getvalue()).decode()}' style='width:100%; max-width:800px;'>\n")
builder.append("</details>\n")
summary = ["<h2>Summary</h2>\n"]
cols = ['maneuver', 'crossed', 'runs', 'mean', 'min', 'max']
table = []
for description, runs in maneuvers:
times = target_cross_times[description]
l = [description, len(times), len(runs)]
if len(times):
l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
table.append(l)
summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n')
sum_idx = builder.index('{ summary }')
builder[sum_idx:sum_idx + 1] = summary
with open(output_fn, "w") as f:
f.write(''.join(builder))
print(f"\nOpening report: {output_fn}\n")
webbrowser.open_new_tab(str(output_fn))
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Generate longitudinal maneuver report from route')
parser.add_argument('route', type=str, help='Route name (e.g. 00000000--5f742174be)')
parser.add_argument('description', type=str, nargs='?')
args = parser.parse_args()
if '/' in args.route or '|' in args.route:
lr = LogReader(args.route)
else:
segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg]
lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs])
CP = lr.first('carParams')
ID = lr.first('initData')
platform = CP.carFingerprint
print('processing report for', platform)
maneuvers: list[tuple[str, list[list]]] = []
active_prev = False
description_prev = None
for msg in lr:
if msg.which() == 'alertDebug':
active = 'Maneuver Active' in msg.alertDebug.alertText1
if active and not active_prev:
if msg.alertDebug.alertText2 == description_prev:
maneuvers[-1][1].append([])
else:
maneuvers.append((msg.alertDebug.alertText2, [[]]))
description_prev = maneuvers[-1][0]
active_prev = active
if active_prev:
maneuvers[-1][1][-1].append(msg)
report(platform, args.route, args.description, CP, ID, maneuvers)

18
tools/longitudinal_maneuvers/maneuver_helpers.py Normal file
View File

@@ -0,0 +1,18 @@
from enum import IntEnum
class Axis(IntEnum):
TIME = 0
EGO_POSITION = 1
LEAD_DISTANCE= 2
EGO_V = 3
LEAD_V = 4
EGO_A = 5
D_REL = 6
axis_labels = {Axis.TIME: 'Time (s)',
Axis.EGO_POSITION: 'Ego position (m)',
Axis.LEAD_DISTANCE: 'Lead absolute position (m)',
Axis.EGO_V: 'Ego Velocity (m/s)',
Axis.LEAD_V: 'Lead Velocity (m/s)',
Axis.EGO_A: 'Ego acceleration (m/s^2)',
Axis.D_REL: 'Lead distance (m)'}

199
tools/longitudinal_maneuvers/maneuversd.py Executable file
View File

@@ -0,0 +1,199 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from cereal import messaging, car
from openpilot.common.constants import CV
from openpilot.common.realtime import DT_MDL
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
@dataclass
class Action:
accel_bp: list[float] # m/s^2
time_bp: list[float] # seconds
def __post_init__(self):
assert len(self.accel_bp) == len(self.time_bp)
@dataclass
class Maneuver:
description: str
actions: list[Action]
repeat: int = 0
initial_speed: float = 0. # m/s
_active: bool = False
_finished: bool = False
_run_completed: bool = False
_action_index: int = 0
_action_frames: int = 0
_ready_cnt: int = 0
_repeated: int = 0
def _step(self) -> float:
self._run_completed = False
action = self.actions[self._action_index]
action_accel = np.interp(self._action_frames * DT_MDL, action.time_bp, action.accel_bp)
self._action_frames += 1
# reached duration of action
if self._action_frames > (action.time_bp[-1] / DT_MDL):
# next action
if self._action_index < len(self.actions) - 1:
self._action_index += 1
self._action_frames = 0
# repeat maneuver
elif self._repeated < self.repeat:
self._repeated += 1
self._run_completed = True
self.reset()
# finish maneuver
else:
self._run_completed = True
self._finished = True
return float(action_accel)
def get_accel(self, v_ego: float, long_active: bool, standstill: bool, cruise_standstill: bool) -> float:
ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not cruise_standstill
if self.initial_speed < 0.01:
ready = ready and standstill
self._ready_cnt = (self._ready_cnt + 1) if ready else 0
if self._ready_cnt > (3. / DT_MDL):
self._active = True
if not self._active:
return min(max(self.initial_speed - v_ego, -2.), 2.)
return self._step()
def reset(self):
self._active = False
self._action_frames = 0
self._action_index = 0
@property
def finished(self):
return self._finished
@property
def active(self):
return self._active
MANEUVERS = [
Maneuver(
"come to stop",
[Action([-0.5], [12])],
repeat=2,
initial_speed=5.,
),
Maneuver(
"start from stop",
[Action([1.5], [6])],
repeat=2,
initial_speed=0.,
),
Maneuver(
"creep: alternate between +1m/s^2 and -1m/s^2",
[
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
Action([1], [3]), Action([-1], [3]),
],
repeat=2,
initial_speed=0.,
),
Maneuver(
"brake step response: -1m/s^2 from 20mph",
[Action([-1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"brake step response: -4m/s^2 from 20mph",
[Action([-4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +1m/s^2 from 20mph",
[Action([1], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"gas step response: +4m/s^2 from 20mph",
[Action([4], [3])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
]
def main():
params = Params()
cloudlog.info("joystickd is waiting for CarParams")
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2')
pm = messaging.PubMaster(['longitudinalPlan', 'iqPlan', 'driverAssistance', 'alertDebug'])
maneuvers = iter(MANEUVERS)
maneuver = None
while True:
sm.update()
if maneuver is None:
maneuver = next(maneuvers, None)
alert_msg = messaging.new_message('alertDebug')
alert_msg.valid = True
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_checks()
longitudinalPlan = plan_send.longitudinalPlan
accel = 0
v_ego = max(sm['carState'].vEgo, 0)
if maneuver is not None:
accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].standstill, sm['carState'].cruiseState.standstill)
if maneuver.active:
alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2'
else:
alert_msg.alertDebug.alertText1 = f'Setting up to {maneuver.initial_speed * CV.MS_TO_MPH:0.2f} mph'
alert_msg.alertDebug.alertText2 = f'{maneuver.description}'
else:
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
pm.send('alertDebug', alert_msg)
longitudinalPlan.aTarget = accel
longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = True
longitudinalPlan.hasLead = True
longitudinalPlan.speeds = [0.2] # triggers carControl.cruiseControl.resume in controlsd
pm.send('longitudinalPlan', plan_send)
plan_iq_send = messaging.new_message('iqPlan')
plan_iq_send.valid = True
pm.send('iqPlan', plan_iq_send)
assistance_send = messaging.new_message('driverAssistance')
assistance_send.valid = True
pm.send('driverAssistance', assistance_send)
if maneuver is not None and maneuver.finished:
maneuver = None

294
tools/longitudinal_maneuvers/mpc_longitudinal_tuning_report.py Normal file
View File

@@ -0,0 +1,294 @@
import io
import sys
import markdown
import numpy as np
import matplotlib.pyplot as plt
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
from openpilot.tools.longitudinal_maneuvers.maneuver_helpers import Axis, axis_labels
from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
def get_html_from_results(results, labels, AXIS):
fig, ax = plt.subplots(figsize=(16, 8))
for idx, key in enumerate(results.keys()):
ax.plot(results[key][:, Axis.TIME], results[key][:, AXIS], label=labels[idx])
ax.set_xlabel(axis_labels[Axis.TIME])
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/>'
def generate_mpc_tuning_report():
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, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.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()
labels.append(f'{speed} m/s approach speed')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Following 5s (triangular) 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 oscillation size')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
results = {}
name = 'Following 5s (sinusoidal) oscillating lead'
labels = []
speed = np.int64(10)
duration = float(30)
f_osc = 1. / 5
for oscil in np.arange(0, 10, 1):
bps = DT_MDL * np.arange(int(duration / DT_MDL))
lead_speeds = speed + oscil * np.sin(2 * np.pi * f_osc * bps)
man = Maneuver(
'',
duration=duration,
initial_speed=float(speed),
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(speed, speed),
speed_lead_values=lead_speeds,
breakpoints=bps,
)
valid, results[oscil] = man.evaluate()
labels.append(f'{oscil} m/s oscillation size')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.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()
labels.append(f'{distance} m initial distance')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
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()
labels.append(f'{distance} m initial distance')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
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=30,
initial_speed=30.0,
cruise_values=[30.0, 30.0, 30.0],
lead_relevancy=True,
initial_distance_lead=60.0,
speed_lead_values=[30.0, 30.0, 0.0],
breakpoints=[0., 5., 5 + stop_time],
)
valid, results[stop_time] = man.evaluate()
labels.append(f'{stop_time} seconds stop time')
htmls.append(markdown.markdown('# ' + name))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
results = {}
name = 'Response to cut-in at half follow distance'
labels = []
for speed in np.arange(0, 40, 5):
man = Maneuver(
'',
duration=20,
initial_speed=float(speed),
cruise_values=[speed, speed, speed],
lead_relevancy=True,
initial_distance_lead=desired_follow_distance(speed, speed) / 2,
speed_lead_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, Axis.EGO_A))
htmls.append(get_html_from_results(results, labels, Axis.D_REL))
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=60,
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, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.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, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.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, Axis.EGO_V))
htmls.append(get_html_from_results(results, labels, Axis.EGO_A))
return htmls
if __name__ == '__main__':
htmls = generate_mpc_tuning_report()
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'))
for html in htmls:
f.write(html)