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

IQ.Pilot Prebuilt Release @ 6dc11f9

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-12 11:06:53 -05:00
commit a772f89326
2275 changed files with 618288 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
iqpilot/tools/maneuvers/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
/reports/

54
iqpilot/tools/maneuvers/README-lateral.md Normal file
View File

@@ -0,0 +1,54 @@
# Lateral Maneuvers Testing Tool
> [!WARNING]
> Use caution when using this tool.
Test your vehicle's lateral control tuning with this tool. The tool will test the vehicle's ability to follow a few lateral maneuvers and includes a tool to generate a report from the route.
## Instructions
1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches.
2. The full maneuver suite runs at 20 and 30 mph.
3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually:
```sh
echo -n 1 > /data/params/d/LateralManeuverMode
```
4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages.
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.
7. Locate the route(s) — they will stand out with lots of orange intervals in their timeline. Ensure "All logs" show as "uploaded."
8. Gather the route ID and then run the report generator. The file will be exported to the same directory:
```sh
$ python iqpilot/tools/maneuvers/lateral_report.py 98395b7c5b27882e/000001cc--5a73bde686
processing report for KIA_EV6
plotting maneuver: step right 20mph, runs: 3
plotting maneuver: step left 20mph, runs: 3
plotting maneuver: sine 0.5Hz 20mph, runs: 3
plotting maneuver: step right 30mph, runs: 3
Opening report: iqpilot/tools/maneuvers/reports/lateral/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
```
The IQ.Pilot `lateral_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports
auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the
top-N highest-peak sweeps by speed/peak filters. See `lateral_report.py --help`.
## Testing the tooling without a car
`simulate_lateral.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
rlog that `lateral_report.py` reads. Use it to verify the daemon and the report generator after changing either:
```sh
$ python iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat/rlog.zst
$ python iqpilot/tools/maneuvers/lateral_report.py /tmp/lat/rlog.zst
```
The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.

60
iqpilot/tools/maneuvers/README-longitudinal.md Normal file
View File

@@ -0,0 +1,60 @@
# 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-mici` on your device. The toggle is hidden on release branches.
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 IQ.Pilot between maneuvers and turn around if there is not enough space.
3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually:
```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 iqpilot/tools/maneuvers/longitudinal_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 iqpilot/tools/maneuvers/reports/longitudinal/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
```
`longitudinal_report.py` also takes a path to a local `rlog.zst` or a directory of them.
## Testing the tooling without a car
`simulate_longitudinal.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog
that `longitudinal_report.py` reads. Use it to verify the daemon and the report generator after changing either:
```sh
$ python iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long/rlog.zst
$ python iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long/rlog.zst
```
The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.

221
iqpilot/tools/maneuvers/lateral_maneuversd.py Normal file
View File

@@ -0,0 +1,221 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from iqpilot.cereal import messaging, car
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL, Ratekeeper
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
from iqpilot.tools.maneuvers.longitudinal_maneuversd import Action, Maneuver as _Maneuver
# thresholds for starting maneuvers
MAX_SPEED_DEV = 0.7 # deviation in m/s
MAX_CURV = 0.004 # 250 m radius
MAX_ROLL = 0.12 # 6.8°
TIMER = 2.0 # sec stable conditions before starting maneuver
# The curvature step yanks the rim and spikes driver torque for a frame or two on cars with a
# tight override threshold (VW MQB trips at 0.8 Nm), which single-frame aborts read as a driver
# grab. Measured on VW_GOLF_MK7: 9/9 maneuvers died 0.15s in with the driver near hands-off.
STEER_PRESSED_FRAMES = 3
@dataclass
class Maneuver(_Maneuver):
_baseline_curvature: float = 0.0
def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
self._run_completed = False
# only start maneuver on straight, flat roads
ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)
if self._ready_cnt > (TIMER / DT_MDL):
if not self._active:
self._baseline_curvature = curvature
self._active = True
if not self._active:
return 0.0
return self._step()
def reset(self):
super().reset()
self._ready_cnt = 0
def _sine_action(amplitude, period, duration):
t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
a = amplitude * np.sin(2 * np.pi * t / period)
return Action(a.tolist(), t.tolist())
MANEUVERS = [
Maneuver(
"step right 20mph",
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"step left 20mph",
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"sine 0.5Hz 20mph",
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"jitter 20mph",
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
repeat=2,
initial_speed=20. * CV.MPH_TO_MS,
),
Maneuver(
"step right 30mph",
[Action([0.5], [1.0]), Action([-0.5], [1.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"step left 30mph",
[Action([-0.5], [1.0]), Action([0.5], [1.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"sine 0.5Hz 30mph",
[_sine_action(1.0, 2.0, 2.0), Action([0.0], [0.5])],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
Maneuver(
"jitter 30mph",
[Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)],
repeat=2,
initial_speed=30. * CV.MPH_TO_MS,
),
]
def main():
params = Params()
cloudlog.info("lateral_maneuversd is waiting for CarParams")
messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
# iqpilot: subscribe only to the services we actually read and drive timing with a
# Ratekeeper instead of polling modelV2. msgq caps each topic at NUM_READERS (15) and
# evicts ALL subscribers when exceeded; iqpilot runs many daemons, and unlike longitudinal
# maneuver mode (which disables plannerd), lateral mode keeps plannerd running. Subscribing
# to selfdriveState/modelV2 here (selfdriveState is unused; modelV2 was only a poll source)
# tips those topics past 15 → eviction storm → UI/speed render drops to a few fps.
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'])
pm = messaging.PubMaster(['lateralManeuverPlan', 'alertDebug'])
rk = Ratekeeper(int(1. / DT_MDL), print_delay_threshold=None) # 20 Hz, matches DT_MDL maneuver timing
maneuvers = iter(MANEUVERS)
maneuver = None
complete_cnt = 0
aborted_cnt = 0
steer_pressed_cnt = 0
abort_reason = ''
display_holdoff = 0
prev_text = ''
while True:
sm.update(0)
if maneuver is None:
maneuver = next(maneuvers, None)
alert_msg = messaging.new_message('alertDebug')
alert_msg.valid = True
plan_send = messaging.new_message('lateralManeuverPlan')
accel = 0
v_ego = max(sm['carState'].vEgo, 0)
curvature = sm['controlsState'].desiredCurvature
if complete_cnt > 0:
complete_cnt -= 1
alert_msg.alertDebug.alertText1 = 'Completed'
alert_msg.alertDebug.alertText2 = maneuver.description
elif maneuver is not None:
# any driver input aborts the maneuver, but only a sustained hold counts as steering override
CS = sm['carState']
steer_pressed_cnt = (steer_pressed_cnt + 1) if CS.steeringPressed else 0
steer_override = steer_pressed_cnt >= STEER_PRESSED_FRAMES
if steer_override or CS.gasPressed:
aborted_cnt = int(1.0 / DT_MDL)
abort_reason = ('steering pressed' if steer_override else 'gas pressed').ljust(20)
aborted = aborted_cnt > 0
speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV
if aborted or speed_out_of_range:
maneuver.reset()
roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0
accel = maneuver.get_accel(v_ego, sm['carControl'].latActive, curvature, roll)
if maneuver._run_completed:
complete_cnt = int(1.0 / DT_MDL)
alert_msg.alertDebug.alertText1 = 'Complete'
alert_msg.alertDebug.alertText2 = maneuver.description
elif maneuver.active:
action_remaining = maneuver.actions[maneuver._action_index].time_bp[-1] - maneuver._action_frames * DT_MDL
if maneuver.description.startswith('sine'):
freq = maneuver.description.split()[1]
alert_msg.alertDebug.alertText1 = f'Active sine {freq} {max(action_remaining, 0):.1f}s'
else:
alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s'
alert_msg.alertDebug.alertText2 = maneuver.description
elif aborted_cnt > 0:
aborted_cnt -= 1
alert_msg.alertDebug.alertText1 = abort_reason
elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive):
alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph'
elif maneuver._ready_cnt > 0:
ready_time = max(TIMER - maneuver._ready_cnt * DT_MDL, 0)
alert_msg.alertDebug.alertText1 = f'Starting: {int(ready_time) + 1}'
alert_msg.alertDebug.alertText2 = maneuver.description
else:
curv_ok = abs(curvature) < MAX_CURV
reason = 'road not straight' if not curv_ok else 'road not flat'
alert_msg.alertDebug.alertText1 = f'Waiting: {reason}'
alert_msg.alertDebug.alertText2 = maneuver.description
else:
alert_msg.alertDebug.alertText1 = 'Maneuvers Finished'
# prevent flickering text
setup = ('Set speed', 'Starting', 'Waiting')
text = alert_msg.alertDebug.alertText1
same = text == prev_text or (text.startswith('Starting') and prev_text.startswith('Starting'))
if not same and text.startswith(setup) and prev_text.startswith(setup) and display_holdoff > 0:
alert_msg.alertDebug.alertText1 = prev_text
display_holdoff -= 1
else:
prev_text = text
display_holdoff = int(0.5 / DT_MDL) if text.startswith(setup) else 0
pm.send('alertDebug', alert_msg)
plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
if plan_send.valid:
plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2
pm.send('lateralManeuverPlan', plan_send)
if maneuver is not None and maneuver.finished and complete_cnt == 0:
maneuver = None
rk.keep_time()
if __name__ == "__main__":
main()

280
iqpilot/tools/maneuvers/lateral_report.py Normal file
View File

@@ -0,0 +1,280 @@
#!/usr/bin/env python3
import argparse
import base64
import io
import math
import numpy as np
import os
import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
from iqpilot.common.utils import tabulate
from iqpilot.cereal import car
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.system.hardware.hw import Paths
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL
from iqpilot.tools.maneuvers.lateral_maneuversd import STEER_PRESSED_FRAMES
from iqpilot.tools.maneuvers.longitudinal_report import format_car_params
ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
STEER_OVERRIDE_S = STEER_PRESSED_FRAMES * DT_MDL
def lat_accel(curvature, v):
return curvature * max(v, 1.0) ** 2
def steering_overridden(t_carState, carState):
# mirrors lateral_maneuversd: the curvature step spikes driver torque for a frame or two on
# cars with a tight override threshold, so only a sustained hold invalidates the run
start = None
for t, cs in zip(t_carState, carState, strict=True):
if not cs.steeringPressed:
start = None
elif start is None:
start = t
elif t - start >= STEER_OVERRIDE_S:
return True
return False
def report(platform, route, _description, CP, ID, maneuvers):
output_path = Path(__file__).resolve().parent / "reports" / "lateral"
output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
output_path.mkdir(parents=True, exist_ok=True)
target_cross_times = defaultdict(list)
builder = [
"<style>summary { cursor: pointer; }\n td, th { padding: 8px; } </style>\n",
"<h1>Lateral 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:
# filter incomplete runs
completed_runs = [msgs for msgs in runs
if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')]
print(f'plotting maneuver: {description}, runs: {len(completed_runs)}')
if not completed_runs:
continue
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(completed_runs):
last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
msgs = [m for m in msgs if m.logMonoTime <= last_active]
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True)
t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True)
# make time relative seconds
t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl]
t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState]
t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState]
t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan]
t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput]
# maneuver validity
latActive = [m.latActive for m in carControl]
maneuver_valid = all(latActive) and not steering_overridden(t_carState, carState)
_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")
baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo)
v_ego = [m.vEgo for m in carState]
cross_markers = []
if description.startswith(('sine', 'jitter')):
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel)
for lp, v in zip(lateralPlan, v_ego, strict=False))
threshold = amplitude * 0.5
builder.append('<h3 style="font-weight: normal">50% peak')
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
actual = lat_accel(cs.curvature, v) - baseline_accel
if abs(actual) > threshold:
builder.append(f', <strong>crossed in {t:.3f}s</strong>')
cross_markers.append((t, actual + baseline_accel))
if maneuver_valid:
target_cross_times[description].append(t)
break
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
if maneuver_valid:
target_cross_times.setdefault(description, [])
else:
action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
for i in range(1, min(len(lateralPlan), len(v_ego))):
if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001:
desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel
action_targets.append((i, desired))
for j, (start_i, act_target) in enumerate(action_targets):
start_time = t_lateralPlan[start_i]
end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1]
builder.append(f'<h3 style="font-weight: normal">aTarget: {round(act_target, 1)} m/s^2')
prev_crossed = False
for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False):
if not (start_time <= t <= end_time):
continue
actual_accel = lat_accel(cs.curvature, v) - baseline_accel
crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel)
if crossed and prev_crossed:
cross_time = t - start_time
builder.append(f', <strong>crossed in {cross_time:.3f}s</strong>')
cross_markers.append((t, act_target + baseline_accel))
if maneuver_valid:
target_cross_times[description].append(cross_time)
break
prev_crossed = crossed
else:
builder.append(', <strong>not crossed</strong>')
builder.append('</h3>')
if maneuver_valid:
target_cross_times.setdefault(description, [])
plt.rcParams['font.size'] = 40
fig = plt.figure(figsize=(30, 40))
ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]})
ax[0].grid(linewidth=4)
desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2'
desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)]
if description.startswith(('sine', 'jitter')):
ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6)
else:
t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)]
desired_lat_accel = [baseline_accel] + desired_lat_accel
ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post')
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)]
ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6)
ax[0].set_ylabel('Lateral Accel (m/s^2)')
for ct, cv in cross_markers:
ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None')
ax[0].legend(prop={'size': 30})
ax[1].grid(linewidth=4)
if CP.steerControlType in ANGLE_CONTROL:
steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
else:
steer_field, steer_ylabel = 'torque', 'Steer torque'
ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6)
ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6)
ax[1].set_ylabel(steer_ylabel)
ax[1].legend(prop={'size': 30})
ax[2].grid(linewidth=4)
ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6)
ax[2].set_ylabel('Velocity (mph)')
ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f'))
ax[2].legend()
t_accel = np.array(t_controlsState[:len(actual_lat_accel)])
raw_jerk = np.gradient(actual_lat_accel, t_accel)
dt_avg = np.mean(np.diff(t_accel))
jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg)
filtered_jerk = [jerk_filter.update(j) for j in raw_jerk]
ax[3].grid(linewidth=4)
ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6)
ax[3].set_ylabel('Jerk (m/s^3)')
ax[3].legend()
ax[4].grid(linewidth=4)
ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl],
label='carControl.orientationNED[0]', linewidth=6)
ax[4].set_ylabel('Roll (deg)')
ax[4].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', 'mean', 'min', 'max']
table = []
for description, times in target_cross_times.items():
l = [description, len(times)]
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))
def open_route(route: str) -> LogReader:
if os.path.isdir(route):
rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst"))
if not rlogs:
raise SystemExit(f"no *rlog.zst files in {route}")
print(f"loading {len(rlogs)} rlogs from {route}")
return LogReader(rlogs, only_union_types=True)
if route.endswith(('.zst', '.bz2', '.log')) and not os.path.exists(route):
raise SystemExit(f"no such file: {route}")
if os.path.exists(route) or '/' in route or '|' in route:
return LogReader(route, only_union_types=True)
segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg]
return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route')
parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs')
parser.add_argument('description', type=str, nargs='?')
args = parser.parse_args()
lr = open_route(args.route)
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 = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete'
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)

200
iqpilot/tools/maneuvers/longitudinal_maneuversd.py Executable file
View File

@@ -0,0 +1,200 @@
#!/usr/bin/env python3
import numpy as np
from dataclasses import dataclass
from iqpilot.cereal import messaging
from iqpilot.common.constants import CV
from iqpilot.common.realtime import DT_MDL
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.controls.lib.drive_helpers import should_stop
@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("maneuversd is waiting for CarParams")
params.get("CarParams", block=True)
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 = should_stop(v_ego, accel)
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

187
iqpilot/tools/maneuvers/longitudinal_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 iqpilot.common.utils import tabulate
from iqpilot.tools.lib.logreader import LogReader
from iqpilot.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 / "reports" / "longitudinal"
output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html"
output_path.mkdir(parents=True, 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_deviceMotion, deviceMotion = zip(*[(m.logMonoTime, m.deviceMotion) for m in msgs if m.which() == 'deviceMotion'], 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_deviceMotion = [(t - t_deviceMotion[0]) / 1e9 for t in t_deviceMotion]
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_deviceMotion, deviceMotion, 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_deviceMotion, [m.accelerationDevice.x for m in deviceMotion], label='deviceMotion.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
iqpilot/tools/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)'}

293
iqpilot/tools/maneuvers/mpc_longitudinal_tuning_report.py Normal file
View File

@@ -0,0 +1,293 @@
import io
import sys
import numpy as np
import matplotlib.pyplot as plt
from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.controls.tests.test_following_distance import desired_follow_distance
from iqpilot.tools.maneuvers.maneuver_helpers import Axis, axis_labels
from iqpilot.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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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(f'<h1>{name}</h1>')
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('<h1>MPC longitudinal tuning report</h1>')
for html in htmls:
f.write(html)

244
iqpilot/tools/maneuvers/sim_harness.py Normal file
View File

@@ -0,0 +1,244 @@
#!/usr/bin/env python3
"""Closed-loop offline harness for the maneuver daemons.
Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a
synthetic vehicle, and records every message to an rlog that the report generators can read.
Used to validate the maneuver tooling without a car.
"""
import math
import os
import signal
import subprocess
import sys
import time
from pathlib import Path
from typing import NamedTuple
import numpy as np
import zstandard as zstd
from iqpilot.cereal import car, messaging
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_CTRL, Ratekeeper
from iqpilot.common.basedir import BASEDIR
PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
PUB_20HZ = ('modelV2', 'deviceMotion', 'vehicleParameters')
SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan')
STEER_RATIO = 15.0
WHEELBASE = 2.78
class LongPlan(NamedTuple):
aTarget: float
shouldStop: bool
class LatPlan(NamedTuple):
desiredCurvature: float
class Plant:
"""Vehicle model. Subclasses consume the daemon's plan and fill the published messages."""
sim = None
PLAN = 'longitudinalPlan'
def __init__(self, v_ego: float = 0.0):
self.v_ego = v_ego
self.a_ego = 0.0
self.curvature = 0.0 # commanded, controlsState.desiredCurvature
self.achieved_curvature = 0.0 # measured, controlsState.curvature
self.lat_accel = 0.0
self.long_active = True
self.lat_active = True
self.steering_pressed = False
self.gas_pressed = False
def step(self, dt: float, plan) -> None:
raise NotImplementedError
def _angle(self, curvature: float) -> float:
return math.degrees(curvature * WHEELBASE * STEER_RATIO)
def _torque(self, curvature: float) -> float:
return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0))
def fill_car_state(self, cs) -> None:
cs.vEgo = float(self.v_ego)
cs.vEgoRaw = float(self.v_ego)
cs.vEgoCluster = float(self.v_ego)
cs.aEgo = float(self.a_ego)
cs.standstill = self.v_ego < 0.01
cs.steeringPressed = self.steering_pressed
cs.gasPressed = self.gas_pressed
cs.steeringAngleDeg = self._angle(self.achieved_curvature)
cs.cruiseState.enabled = True
cs.cruiseState.available = True
cs.cruiseState.speed = float(max(self.v_ego, 1.0))
def fill_car_control(self, cc) -> None:
cc.enabled = True
cc.latActive = self.lat_active
cc.longActive = self.long_active
cc.orientationNED = [0.0, 0.0, 0.0]
cc.actuators.curvature = float(self.curvature)
cc.actuators.accel = float(self.a_ego)
cc.actuators.steeringAngleDeg = self._angle(self.curvature)
cc.actuators.torque = self._torque(self.curvature)
class ManeuverSim:
def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA",
max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True):
self.module = module
self.plant = plant
plant.sim = self
self.fingerprint = fingerprint
self.max_maneuvers = max_maneuvers
self.timeout = timeout
self.verbose = verbose
self.events: list[bytes] = []
self.alert1 = ''
self.alert2 = ''
self.seen_maneuvers: list[str] = []
self.finished = False
def _write_car_params(self):
CP = car.CarParams.new_message()
CP.carFingerprint = self.fingerprint
CP.brand = "toyota"
CP.openpilotLongitudinalControl = True
CP.autoResumeSng = True
CP.steerRatio = STEER_RATIO
CP.wheelbase = WHEELBASE
Params().put("CarParams", CP.to_bytes())
return CP
def _head_events(self, CP):
init = messaging.new_message('initData')
init.valid = True
init.initData.gitCommit = "simulated"
init.initData.gitBranch = "sim"
init.initData.gitRemote = "iqpilot-sim"
self.events.append(init.to_bytes())
cpm = messaging.new_message('carParams')
cpm.valid = True
cpm.carParams = CP
self.events.append(cpm.to_bytes())
def _launch(self):
env = dict(os.environ)
env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "")
return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"],
cwd=str(BASEDIR), env=env, start_new_session=True)
def _on_alert(self, ad):
text1, text2 = ad.alertText1, ad.alertText2
if (text1, text2) != (self.alert1, self.alert2):
if self.verbose:
print(f" [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}")
if text2 and text2 not in self.seen_maneuvers:
self.seen_maneuvers.append(text2)
if text1 == 'Maneuvers Finished':
self.finished = True
self.alert1, self.alert2 = text1, text2
def run(self, out: Path) -> Path:
self._head_events(self._write_car_params())
pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ))
socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB}
proc = self._launch()
self.t_start = time.monotonic()
rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None)
plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None}
frame = 0
try:
while True:
for s, sock in socks.items():
while True:
raw = sock.receive(non_blocking=True)
if raw is None:
break
self.events.append(raw)
evt = messaging.log_from_bytes(raw)
if s == 'alertDebug':
self._on_alert(evt.alertDebug)
elif s == 'longitudinalPlan':
plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop)
elif s == 'lateralManeuverPlan':
plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None
self.plant.step(DT_CTRL, plans[self.plant.PLAN])
for s in PUB_100HZ:
raw = self._build(s).to_bytes()
self.events.append(raw)
pm.send(s, raw)
if frame % 5 == 0:
for s in PUB_20HZ:
raw = self._build(s).to_bytes()
self.events.append(raw)
pm.send(s, raw)
frame += 1
if self.finished:
break
if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers:
break
if time.monotonic() - self.t_start > self.timeout:
print(" timed out")
break
rk.keep_time()
finally:
if proc.poll() is None:
os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
proc.wait(timeout=5)
for sock in socks.values():
del sock
out.parent.mkdir(parents=True, exist_ok=True)
out.write_bytes(zstd.compress(b"".join(self.events), 10))
return out
def _build(self, s: str):
msg = messaging.new_message(s)
msg.valid = True
if s == 'carState':
self.plant.fill_car_state(msg.carState)
elif s == 'carControl':
self.plant.fill_car_control(msg.carControl)
elif s == 'carOutput':
msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego)
msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature)
msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature)
msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature)
elif s == 'controlsState':
msg.controlsState.curvature = float(self.plant.achieved_curvature)
msg.controlsState.desiredCurvature = float(self.plant.curvature)
elif s == 'selfdriveState':
msg.selfdriveState.enabled = True
msg.selfdriveState.active = True
msg.selfdriveState.state = 'enabled'
elif s == 'modelV2':
msg.modelV2.frameId = 0
msg.modelV2.action.desiredCurvature = 0.0
elif s == 'deviceMotion':
msg.deviceMotion.accelerationDevice.x = float(self.plant.a_ego)
msg.deviceMotion.accelerationDevice.y = float(self.plant.lat_accel)
msg.deviceMotion.velocityDevice.x = float(self.plant.v_ego)
msg.deviceMotion.inputsOK = True
msg.deviceMotion.posenetOK = True
msg.deviceMotion.sensorsOK = True
elif s == 'vehicleParameters':
msg.vehicleParameters.valid = True
msg.vehicleParameters.roll = 0.0
msg.vehicleParameters.steerRatio = STEER_RATIO
return msg

87
iqpilot/tools/maneuvers/simulate_lateral.py Executable file
View File

@@ -0,0 +1,87 @@
#!/usr/bin/env python3
"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
./iqpilot/tools/maneuvers/simulate_lateral.py --out /tmp/lat_rlog.zst
./iqpilot/tools/maneuvers/lateral_report.py /tmp/lat_rlog.zst
"""
import argparse
import re
from pathlib import Path
from iqpilot.common.constants import CV
from iqpilot.tools.maneuvers.lateral_maneuversd import MANEUVERS
from iqpilot.tools.maneuvers.sim_harness import ManeuverSim, Plant
CURV_TAU = 0.05 # controlsd curvature command tracking
RACK_WN = 8.0 # steering rack + tire natural frequency (rad/s)
RACK_ZETA = 0.7 # underdamped, so achieved curvature overshoots like a real rack
CRUISE_ACCEL = 1.2
SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph")
class LateralPlant(Plant):
PLAN = 'lateralManeuverPlan'
def __init__(self, steer_input: float = 0.0):
super().__init__(v_ego=MANEUVERS[0].initial_speed)
self.sim = None
self._rack_rate = 0.0
self.target_speed = MANEUVERS[0].initial_speed
self._by_description = {m.description: m.initial_speed for m in MANEUVERS}
# seconds of steeringPressed to assert at each maneuver start, mimicking the driver-torque
# spike a curvature step produces on a car with a tight override threshold
self.steer_input = steer_input
self._steer_hold = 0.0
self._was_active = False
def _update_target(self):
if self.sim is None:
return
speed = self._by_description.get(self.sim.alert2)
if speed is None:
match = SET_SPEED_RE.search(self.sim.alert1)
speed = float(match.group(1)) * CV.MPH_TO_MS if match else None
if speed is not None:
self.target_speed = speed
def step(self, dt, plan):
self._update_target()
active = plan is not None
if self.steer_input > 0 and active and not self._was_active:
self._steer_hold = self.steer_input
self._was_active = active
self.steering_pressed = self._steer_hold > 0
self._steer_hold = max(self._steer_hold - dt, 0.0)
err = self.target_speed - self.v_ego
self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL)
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0
self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature)
self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate)
self.achieved_curvature += dt * self._rack_rate
self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_maneuvers_sim/rlog.zst"))
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
parser.add_argument("--timeout", type=float, default=900.0)
parser.add_argument("--steer-input", type=float, default=0.0,
help="seconds of steeringPressed to assert at each maneuver start (0 = hands off)")
args = parser.parse_args()
sim = ManeuverSim("iqpilot.tools.maneuvers.lateral_maneuversd", LateralPlant(args.steer_input),
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
out = sim.run(args.out)
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
if __name__ == "__main__":
main()

51
iqpilot/tools/maneuvers/simulate_longitudinal.py Executable file
View File

@@ -0,0 +1,51 @@
#!/usr/bin/env python3
"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
./iqpilot/tools/maneuvers/simulate_longitudinal.py --out /tmp/long_rlog.zst
./iqpilot/tools/maneuvers/longitudinal_report.py /tmp/long_rlog.zst
"""
import argparse
from pathlib import Path
from iqpilot.tools.maneuvers.sim_harness import ManeuverSim, Plant
WN = 6.0 # powertrain natural frequency (rad/s)
ZETA = 0.6 # underdamped, so actual accel overshoots the target like a real car
class LongitudinalPlant(Plant):
def __init__(self):
super().__init__()
self.jerk = 0.0
def step(self, dt, plan):
a_target = float(plan.aTarget) if plan is not None else 0.0
if plan is not None and plan.shouldStop:
a_target = min(a_target, -0.5)
self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk)
self.a_ego += dt * self.jerk
self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0)
if self.v_ego <= 0.0:
self.a_ego = min(self.a_ego, 0.0)
self.jerk = min(self.jerk, 0.0)
self.lat_accel = 0.0
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_maneuvers_sim/rlog.zst"))
parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)")
parser.add_argument("--timeout", type=float, default=900.0)
args = parser.parse_args()
sim = ManeuverSim("iqpilot.tools.maneuvers.longitudinal_maneuversd", LongitudinalPlant(),
max_maneuvers=args.max_maneuvers, timeout=args.timeout)
out = sim.run(args.out)
print(f"\nmaneuvers seen: {sim.seen_maneuvers}")
print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)")
if __name__ == "__main__":
main()