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

IQ.Pilot Release Commit @ 2b39aa6

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-29 00:19:10 -05:00
parent 8f052b6f93
commit c7908ad2e0
226 changed files with 11978 additions and 11349 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
tools/lateral_maneuvers/README.md
View File

@@ -7,7 +7,7 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th
## Instructions
1. Check out a development branch such as `master-mici` on your comma device.
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:
@@ -17,7 +17,7 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th
4. Turn your vehicle back on. You will see "Lateral Maneuver Mode".
5. Ensure the area ahead is clear, as iqpilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let iqpilot stabilize lateral. After 2 seconds of steady straight driving, the maneuver will begin automatically. iqpilot 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, steering is touched or iqpilot disengages.
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.
@@ -37,4 +37,18 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th
Opening report: tools/lateral_maneuvers/lateral_reports/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
```
The iqpilot `generate_report.py` also 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 `generate_report.py --help`.
The IQ.Pilot `generate_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 `generate_report.py --help`.
## Testing the tooling without a car
`sim_maneuvers.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an
rlog that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either:
```sh
$ python tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat/rlog.zst
$ python tools/lateral_maneuvers/generate_report.py /tmp/lat/rlog.zst
```
The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early.

539
tools/lateral_maneuvers/generate_report.py
View File

@@ -1,376 +1,261 @@
#!/usr/bin/env python3
"""Lateral maneuver compliance report — analog of tools/longitudinal_maneuvers/generate_report.py.
Produces the "sine 0.5 Hz 30 mph" / "50% peak crossed in X.XXXs" style HTML report comma posts
on social media for steering-rack compliance comparisons. Reads any iqpilot/openpilot rlog
route, slices it into lateral maneuver windows (either by `alertDebug` markers from a scripted
maneuversd run, or by auto-detection of contiguous lat-active sweeps), and emits a 4-panel
plot per run:
1. Lateral accel (desired + actual, m/s²) on left axis and steering-wheel angle (deg) on
right axis. Black circle marks the time the actual lat-accel first crosses 50 % of the
desired peak in the same direction.
2. Vehicle speed (mph)
3. Lateral jerk (m/s³), numerically differentiated from actual lat-accel
4. Roll (deg) from liveParameters
Usage:
python tools/lateral_maneuvers/generate_report.py <route> [description]
Examples:
python tools/lateral_maneuvers/generate_report.py 1ce1b50dd82993a1\\|0000003b--a389fbdf35
python tools/lateral_maneuvers/generate_report.py /path/to/local/rlog.zst "sine 0.5Hz 30mph"
"""
import argparse
import base64
import io
import math
import numpy as np
import os
import pprint
import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
import numpy as np
from tabulate import tabulate
from openpilot.common.utils import tabulate
from cereal import car
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
from openpilot.tools.lib.logreader import LogReader
from openpilot.system.hardware.hw import Paths
from openpilot.common.constants import CV
from openpilot.tools.longitudinal_maneuvers.generate_report import format_car_params
ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED)
MPS_TO_MPH = 2.23693629
AUTO_DESIRED_LAT_ACCEL_THRESHOLD = 0.5
AUTO_MIN_V_EGO = 5.0
AUTO_MIN_DURATION_S = 1.5
AUTO_GAP_S = 0.5
def lat_accel(curvature, v):
return curvature * max(v, 1.0) ** 2
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 _series(msgs, which):
rows = [(m.logMonoTime, getattr(m, which)) for m in msgs if m.which() == which]
if not rows:
return [], []
t, v = zip(*rows, strict=True)
return list(t), list(v)
def _to_relative_seconds(t_ns, t0):
return [(t - t0) / 1e9 for t in t_ns]
def _resample(t_src, v_src, t_dst):
if not t_src or not t_dst:
return np.zeros(len(t_dst))
return np.interp(t_dst, t_src, v_src)
def _peak_crossing_time(t, desired, actual, fraction=0.5):
if len(desired) == 0:
return None, 0.0
desired = np.asarray(desired)
actual = np.asarray(actual)
peak_idx = int(np.argmax(np.abs(desired)))
peak = desired[peak_idx]
if abs(peak) < 1e-3:
return None, peak
target = fraction * peak
prev = False
for i in range(peak_idx + 1):
crossed = (target > 0 and actual[i] >= target) or (target < 0 and actual[i] <= target)
if crossed and prev:
return t[i], peak
prev = crossed
return None, peak
def _slice_by_alert_debug(msgs):
out = []
active_prev = False
description_prev = None
for msg in msgs:
if msg.which() == "alertDebug":
# Match both the longitudinal daemon ("Maneuver Active: …") and the lateral daemon
# ("Active sine …", "Active +0.5m/s² …", "Complete").
text1 = msg.alertDebug.alertText1
active = "Maneuver Active" in text1 or text1.startswith("Active") or text1 == "Complete"
if active and not active_prev:
if msg.alertDebug.alertText2 == description_prev:
out[-1][1].append([])
else:
out.append((msg.alertDebug.alertText2, [[]]))
description_prev = out[-1][0]
active_prev = active
if active_prev:
out[-1][1][-1].append(msg)
return out
def _slice_auto(msgs):
t_cc, cc_vals = _series(msgs, "carControl")
t_cs, cs_vals = _series(msgs, "carState")
if not t_cc or not t_cs:
return []
v_ego = np.asarray([m.vEgo for m in cs_vals])
curvature = np.asarray([m.actuators.curvature for m in cc_vals])
lat_active = np.asarray([1.0 if m.latActive else 0.0 for m in cc_vals])
t_cc_s = np.asarray([(t - t_cc[0]) / 1e9 for t in t_cc])
t_cs_s = np.asarray([(t - t_cc[0]) / 1e9 for t in t_cs])
v_at_cc = np.interp(t_cc_s, t_cs_s, v_ego)
desired_lat_accel = curvature * v_at_cc ** 2
signal = np.abs(desired_lat_accel) * lat_active * (v_at_cc > AUTO_MIN_V_EGO).astype(float)
cycle_dt = float(np.median(np.diff(t_cc_s))) if len(t_cc_s) > 1 else 0.01
min_frames = max(1, int(AUTO_MIN_DURATION_S / cycle_dt))
windows = []
start = None
for i, s in enumerate(signal):
if s > AUTO_DESIRED_LAT_ACCEL_THRESHOLD and start is None:
start = i
elif s <= AUTO_DESIRED_LAT_ACCEL_THRESHOLD and start is not None:
if i - start > min_frames:
windows.append((t_cc[start], t_cc[i]))
start = None
if start is not None and len(signal) - start > min_frames:
windows.append((t_cc[start], t_cc[-1]))
merged = []
for a, b in windows:
if merged and (a - merged[-1][1]) / 1e9 < AUTO_GAP_S:
merged[-1] = (merged[-1][0], b)
else:
merged.append((a, b))
runs = []
for a, b in merged:
runs.append([m for m in msgs if a <= m.logMonoTime <= b])
if not runs:
return []
return [("auto-detected lateral sweep", runs)]
def _plot_run(description, run_idx, msgs, builder, target_cross_times):
t_cc, carControl = _series(msgs, "carControl")
t_cs, carState = _series(msgs, "carState")
t_lp, livePose = _series(msgs, "livePose")
if not (t_cc and t_cs and t_lp):
builder.append(f"<p style='color:red'>Run #{run_idx + 1}: missing required data, skipping.</p>\n")
return
t0 = min(t_cc[0], t_cs[0], t_lp[0])
t_cc_s = _to_relative_seconds(t_cc, t0)
t_cs_s = _to_relative_seconds(t_cs, t0)
t_lp_s = _to_relative_seconds(t_lp, t0)
v_ego = np.asarray([m.vEgo for m in carState])
steer = np.asarray([m.steeringAngleDeg for m in carState])
curvature = np.asarray([m.actuators.curvature for m in carControl])
v_at_cc = _resample(t_cs_s, v_ego, t_cc_s)
desired_lat_accel = curvature * v_at_cc ** 2
actual_lat_accel = np.asarray([m.accelerationDevice.y for m in livePose])
jerk = np.gradient(actual_lat_accel, t_lp_s)
t_lpar, liveParameters = _series(msgs, "liveParameters")
if liveParameters:
t_lpar_s = _to_relative_seconds(t_lpar, t0)
roll_deg = np.asarray([math.degrees(m.roll) for m in liveParameters])
else:
t_lpar_s = []
roll_deg = np.asarray([])
desired_lat_accel_on_lp = _resample(t_cc_s, desired_lat_accel, t_lp_s)
cross_time, peak = _peak_crossing_time(t_lp_s, desired_lat_accel_on_lp, actual_lat_accel, fraction=0.5)
title = f"Run #{run_idx + 1}"
builder.append(f"<details open><summary><h3 style='display:inline-block;'>{title}</h3></summary>\n")
if cross_time is not None:
builder.append(f"<h3 style='font-weight:normal'>50% peak, <strong>crossed in {cross_time:.3f}s</strong></h3>\n")
target_cross_times[description].append(cross_time)
else:
builder.append("<h3 style='font-weight:normal'>50% peak, <strong>not crossed</strong></h3>\n")
builder.append(f"<h3 style='font-weight:normal'>Peak desired lat accel: <strong>{peak:+.2f} m/s²</strong>, "
f"avg speed: <strong>{np.mean(v_ego) * MPS_TO_MPH:.1f} mph</strong></h3>\n")
plt.rcParams["font.size"] = 32
fig = plt.figure(figsize=(28, 22))
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={"height_ratios": [5, 2, 2, 2]})
ax_la = ax[0]
ax_la.grid(linewidth=2)
ax_la.plot(t_cc_s, desired_lat_accel, label="desired lat accel", linewidth=4)
ax_la.plot(t_lp_s, actual_lat_accel, label="actual lat accel", linewidth=4)
ax_la.set_ylabel("Lateral Accel (m/s²)")
ax_st = ax_la.twinx()
ax_st.plot(t_cs_s, steer, color="tab:green", label="steer angle", linewidth=4)
ax_st.set_ylabel("Steering Angle (deg)")
lines_l, labels_l = ax_la.get_legend_handles_labels()
lines_r, labels_r = ax_st.get_legend_handles_labels()
ax_la.legend(lines_l + lines_r, labels_l + labels_r, loc="upper right", prop={"size": 22})
if cross_time is not None:
cross_val = float(np.interp(cross_time, t_lp_s, actual_lat_accel))
ax_la.plot(cross_time, cross_val, marker="o", markersize=30, markeredgewidth=4,
markeredgecolor="black", markerfacecolor="None")
ax[1].grid(linewidth=2)
ax[1].plot(t_cs_s, v_ego * MPS_TO_MPH, color="tab:blue", label="vEgo", linewidth=4)
ax[1].set_ylabel("Velocity (mph)")
ax[1].legend(loc="upper right", prop={"size": 22})
ax[2].grid(linewidth=2)
ax[2].plot(t_lp_s, jerk, color="tab:blue", label="actual jerk", linewidth=4)
ax[2].set_ylabel("Jerk (m/s³)")
ax[2].legend(loc="upper left", prop={"size": 22})
ax[3].grid(linewidth=2)
if len(roll_deg):
ax[3].plot(t_lpar_s, roll_deg, color="tab:blue", label="roll", linewidth=4)
ax[3].set_ylabel("Roll (deg)")
ax[3].legend(loc="upper right", prop={"size": 22})
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:900px;'>\n")
builder.append("</details>\n")
def report(platform, route, description, CP, ID, maneuvers):
def report(platform, route, _description, CP, ID, maneuvers):
output_path = Path(__file__).resolve().parent / "lateral_reports"
output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html"
output_path.mkdir(exist_ok=True)
safe_route = route.replace("/", "_").replace("|", "_")
output_fn = output_path / f"{platform}_{safe_route}.html"
target_cross_times = defaultdict(list)
builder = [
"<style>summary { cursor: pointer; } td, th { padding: 8px; } body { font-family: Arial, sans-serif; }</style>\n",
"<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 }")
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)
for maneuver_description, runs in maneuvers:
print(f"plotting maneuver: {maneuver_description}, runs: {len(runs)}")
builder.append("<div style='border-top:1px solid #000; margin:20px 0;'></div>\n")
builder.append(f"<h2>{maneuver_description}</h2>\n")
for run_idx, msgs in enumerate(runs):
_plot_run(maneuver_description, run_idx, msgs, builder, target_cross_times)
# 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 any(cs.steeringPressed for cs in 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", "runs", "mean (s)", "min (s)", "max (s)"]
cols = ['maneuver', 'crossed', 'mean', 'min', 'max']
table = []
for maneuver_description, runs in maneuvers:
times = target_cross_times[maneuver_description]
row = [maneuver_description, len(times), len(runs)]
if times:
row.extend([round(np.mean(times), 3), round(np.min(times), 3), round(np.max(times), 3)])
table.append(row)
summary.append(tabulate(table, headers=cols, tablefmt="html", numalign="left") + "\n")
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 }")
sum_idx = builder.index('{ summary }')
builder[sum_idx:sum_idx + 1] = summary
with open(output_fn, "w") as f:
f.write("".join(builder))
f.write(''.join(builder))
print(f"\nOpening report: {output_fn}\n")
webbrowser.open_new_tab(str(output_fn))
def _rank_runs(runs, top_n, min_vego_mph, min_peak):
scored = []
for r in runs:
t_cc, cc = _series(r, "carControl")
t_cs, cs = _series(r, "carState")
if not (t_cc and t_cs):
continue
v_ego = np.mean([m.vEgo for m in cs]) * MPS_TO_MPH
curv = np.asarray([m.actuators.curvature for m in cc])
v_at_cc = np.interp([(t - t_cc[0]) / 1e9 for t in t_cc],
[(t - t_cc[0]) / 1e9 for t in t_cs],
[m.vEgo for m in cs])
peak = float(np.max(np.abs(curv * v_at_cc ** 2)))
if v_ego < min_vego_mph or peak < min_peak:
continue
scored.append((peak, r))
scored.sort(key=lambda x: -x[0])
return [r for _, r in scored[:top_n]] if top_n > 0 else [r for _, r in scored]
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 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)
def main():
parser = argparse.ArgumentParser(description="Generate lateral maneuver compliance report from a route")
parser.add_argument("route", type=str, help="Route name, segment range, local rlog path, or directory of rlogs")
parser.add_argument("description", type=str, nargs="?")
parser.add_argument("--auto", action="store_true",
help="Auto-detect lateral sweeps instead of relying on alertDebug 'Maneuver Active' markers")
parser.add_argument("--top-n", type=int, default=10,
help="Plot only the N largest-peak sweeps (0 = all). Default 10.")
parser.add_argument("--min-vego-mph", type=float, default=15.0,
help="Drop sweeps below this average speed. Default 15 mph.")
parser.add_argument("--min-peak", type=float, default=0.5,
help="Drop sweeps with peak desired lat accel below this (m/s²). Default 0.5.")
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()
if os.path.isdir(args.route):
rlogs = sorted(p for p in Path(args.route).glob("*rlog.zst"))
if not rlogs:
raise SystemExit(f"no *rlog.zst files in {args.route}")
print(f"loading {len(rlogs)} rlogs from {args.route}")
lr = LogReader([str(p) for p in rlogs])
elif os.path.exists(args.route):
lr = LogReader(args.route)
elif "/" 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])
lr = open_route(args.route)
msgs = list(lr)
CP = next(m.carParams for m in msgs if m.which() == "carParams")
ID = next(m.initData for m in msgs if m.which() == "initData")
CP = lr.first('carParams')
ID = lr.first('initData')
platform = CP.carFingerprint
print("processing report for", platform)
print('processing report for', platform)
maneuvers = [] if args.auto else _slice_by_alert_debug(msgs)
if not maneuvers:
print("no alertDebug 'Maneuver Active' windows found; auto-detecting lateral sweeps")
maneuvers = _slice_auto(msgs)
maneuvers: list[tuple[str, list[list]]] = []
active_prev = False
description_prev = None
if not maneuvers:
print("no lateral maneuvers detected — treating the whole route as one run")
maneuvers = [("full route", [msgs])]
else:
filtered = []
for description, runs in maneuvers:
kept = _rank_runs(runs, args.top_n, args.min_vego_mph, args.min_peak)
print(f" {description}: {len(runs)} candidate sweeps → {len(kept)} after rank/filter")
if kept:
filtered.append((description, kept))
maneuvers = filtered or [("filtered out", [])]
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)
if __name__ == "__main__":
main()

29
tools/lateral_maneuvers/lateral_maneuversd.py
View File

@@ -12,7 +12,7 @@ from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver a
# thresholds for starting maneuvers
MAX_SPEED_DEV = 0.7 # deviation in m/s
MAX_CURV = 0.002 # 500 m radius
MAX_CURV = 0.004 # 250 m radius
MAX_ROLL = 0.12 # 6.8°
TIMER = 2.0 # sec stable conditions before starting maneuver
@@ -67,6 +67,12 @@ MANEUVERS = [
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])],
@@ -85,6 +91,12 @@ MANEUVERS = [
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,
),
]
@@ -106,6 +118,8 @@ def main():
maneuvers = iter(MANEUVERS)
maneuver = None
complete_cnt = 0
aborted_cnt = 0
abort_reason = ''
display_holdoff = 0
prev_text = ''
@@ -129,8 +143,14 @@ def main():
alert_msg.alertDebug.alertText1 = 'Completed'
alert_msg.alertDebug.alertText2 = maneuver.description
elif maneuver is not None:
# reset maneuver on steering override or out of range speed
if sm['carState'].steeringPressed or (maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV):
# any driver input aborts the maneuver
CS = sm['carState']
if CS.steeringPressed or CS.gasPressed:
aborted_cnt = int(1.0 / DT_MDL)
abort_reason = ('steering pressed' if CS.steeringPressed 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
@@ -148,6 +168,9 @@ def main():
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:

73
tools/lateral_maneuvers/sim_maneuvers.py Executable file
View File

@@ -0,0 +1,73 @@
#!/usr/bin/env python3
"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog.
./tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat_rlog.zst
./tools/lateral_maneuvers/generate_report.py /tmp/lat_rlog.zst
"""
import argparse
import re
from pathlib import Path
from openpilot.common.constants import CV
from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MANEUVERS
from openpilot.tools.longitudinal_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):
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}
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()
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)
args = parser.parse_args()
sim = ManeuverSim("openpilot.tools.lateral_maneuvers.lateral_maneuversd", LateralPlant(),
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()