[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"Run #{run_idx + 1}: missing required data, skipping.
\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"{title}
\n")
- if cross_time is not None:
- builder.append(f"50% peak, crossed in {cross_time:.3f}s
\n")
- target_cross_times[description].append(cross_time)
- else:
- builder.append("50% peak, not crossed
\n")
- builder.append(f"Peak desired lat accel: {peak:+.2f} m/s², "
- f"avg speed: {np.mean(v_ego) * MPS_TO_MPH:.1f} mph
\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").decode()})
\n")
- builder.append("Lateral maneuver report
\n",
f"{platform}
\n",
f"{route}
\n",
f"{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}
\n",
]
- if description is not None:
- builder.append(f"Description: {description}
\n")
- builder.append(f"CarParams
{format_car_params(CP)}Description: {_description}
\n")
+ builder.append(f"CarParams
{format_car_params(CP)}{description}
\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("\n")
- builder.append(f"{maneuver_description}
\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}' + (' (invalid maneuver!)' if not maneuver_valid else '')
+
+ builder.append(f"{title}
\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('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', crossed in {t:.3f}s')
+ cross_markers.append((t, actual + baseline_accel))
+ if maneuver_valid:
+ target_cross_times[description].append(t)
+ break
+ else:
+ builder.append(', not crossed')
+ builder.append('
')
+ 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'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', crossed in {cross_time:.3f}s')
+ 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(', not crossed')
+ builder.append('
')
+ 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"\n")
+ builder.append("Summary
\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()
diff --git a/tools/lateral_maneuvers/lateral_maneuversd.py b/tools/lateral_maneuvers/lateral_maneuversd.py
index ad398a9..cce125a 100644
--- a/tools/lateral_maneuvers/lateral_maneuversd.py
+++ b/tools/lateral_maneuvers/lateral_maneuversd.py
@@ -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:
diff --git a/tools/lateral_maneuvers/sim_maneuvers.py b/tools/lateral_maneuvers/sim_maneuvers.py
new file mode 100755
index 0000000..22ad54f
--- /dev/null
+++ b/tools/lateral_maneuvers/sim_maneuvers.py
@@ -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()
diff --git a/tools/longitudinal_maneuvers/README.md b/tools/longitudinal_maneuvers/README.md
index 643af7f..28e4231 100644
--- a/tools/longitudinal_maneuvers/README.md
+++ b/tools/longitudinal_maneuvers/README.md
@@ -6,9 +6,9 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
## Instructions
-1. Check out a development branch such as `master` on your comma device.
-2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage openpilot between maneuvers and turn around if there is not enough space.
-3. Turn off the vehicle and set this parameter which will signal to openpilot to start the longitudinal maneuver daemon:
+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
@@ -42,7 +42,19 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te
plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2
plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2
- Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
+ Report written to tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html
```
-You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself.
+ `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them.
+
+## Testing the tooling without a car
+
+`sim_maneuvers.py` runs `maneuversd` as a real process against a synthetic powertrain 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/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long/rlog.zst
+$ python tools/longitudinal_maneuvers/generate_report.py /tmp/long/rlog.zst
+```
+
+The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early.
diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py
index 8c16e30..32bdb5b 100755
--- a/tools/longitudinal_maneuvers/generate_report.py
+++ b/tools/longitudinal_maneuvers/generate_report.py
@@ -9,7 +9,7 @@ import webbrowser
from collections import defaultdict
from pathlib import Path
import matplotlib.pyplot as plt
-from tabulate import tabulate
+from openpilot.common.utils import tabulate
from openpilot.tools.lib.logreader import LogReader
from openpilot.system.hardware.hw import Paths
diff --git a/tools/longitudinal_maneuvers/sim_harness.py b/tools/longitudinal_maneuvers/sim_harness.py
new file mode 100644
index 0000000..d77079f
--- /dev/null
+++ b/tools/longitudinal_maneuvers/sim_harness.py
@@ -0,0 +1,240 @@
+#!/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 generate_report.py 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 cereal import car, messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL, Ratekeeper
+from openpilot.common.basedir import BASEDIR
+
+PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState')
+PUB_20HZ = ('modelV2', 'livePose', 'liveParameters')
+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
+
+ 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.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 == 'livePose':
+ msg.livePose.accelerationDevice.x = float(self.plant.a_ego)
+ msg.livePose.accelerationDevice.y = float(self.plant.lat_accel)
+ msg.livePose.velocityDevice.x = float(self.plant.v_ego)
+ msg.livePose.inputsOK = True
+ msg.livePose.posenetOK = True
+ msg.livePose.sensorsOK = True
+ elif s == 'liveParameters':
+ msg.liveParameters.valid = True
+ msg.liveParameters.roll = 0.0
+ msg.liveParameters.steerRatio = STEER_RATIO
+ return msg
diff --git a/tools/longitudinal_maneuvers/sim_maneuvers.py b/tools/longitudinal_maneuvers/sim_maneuvers.py
new file mode 100755
index 0000000..34e0eae
--- /dev/null
+++ b/tools/longitudinal_maneuvers/sim_maneuvers.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+"""Run maneuversd against a synthetic longitudinal plant and write an rlog.
+
+ ./tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long_rlog.zst
+ ./tools/longitudinal_maneuvers/generate_report.py /tmp/long_rlog.zst
+"""
+import argparse
+from pathlib import Path
+
+from openpilot.tools.longitudinal_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("openpilot.tools.longitudinal_maneuvers.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()