mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-22 08:33:44 +08:00
add LateralManeuverMode toggle to UI
This commit is contained in:
Test User
@@ -20,7 +20,7 @@ DESCRIPTIONS = {
|
||||
"other than your own. A comma employee will NEVER ask you to add their GitHub username."
|
||||
),
|
||||
'alpha_longitudinal': tr_noop(
|
||||
"<b>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</b><br><br>" +
|
||||
"<b>WARNING: openpilot longitudinal control is in alpha for this car and may disable Automatic Emergency Braking (AEB).</b><br><br>" +
|
||||
"On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. " +
|
||||
"Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. " +
|
||||
"Changing this setting will restart openpilot if the car is powered on."
|
||||
@@ -36,6 +36,7 @@ class DeveloperLayout(Widget):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._params = Params()
|
||||
self._is_release = self._params.get_bool("IsReleaseBranch")
|
||||
|
||||
# Build items and keep references for callbacks/state updates
|
||||
self._adb_toggle = toggle_item(
|
||||
@@ -78,6 +79,13 @@ class DeveloperLayout(Widget):
|
||||
callback=self._on_long_maneuver_mode,
|
||||
)
|
||||
|
||||
self._lat_maneuver_toggle = toggle_item(
|
||||
lambda: tr("Lateral Maneuver Mode"),
|
||||
description="",
|
||||
initial_state=self._params.get_bool("LateralManeuverMode"),
|
||||
callback=self._on_lat_maneuver_mode,
|
||||
)
|
||||
|
||||
self._alpha_long_toggle = toggle_item(
|
||||
lambda: tr("openpilot Longitudinal Control (Alpha)"),
|
||||
description=lambda: tr(DESCRIPTIONS["alpha_longitudinal"]),
|
||||
@@ -101,6 +109,7 @@ class DeveloperLayout(Widget):
|
||||
self._ssh_keys,
|
||||
self._joystick_toggle,
|
||||
self._long_maneuver_toggle,
|
||||
self._lat_maneuver_toggle,
|
||||
self._alpha_long_toggle,
|
||||
self._ui_debug_toggle,
|
||||
], line_separator=True, spacing=0)
|
||||
@@ -112,16 +121,22 @@ class DeveloperLayout(Widget):
|
||||
self._scroller.render(rect)
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._scroller.show_event()
|
||||
self._update_toggles()
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
|
||||
# Hide non-release toggles on release builds
|
||||
# TODO: we can do an onroad cycle, but alpha long toggle requires a deinit function to re-enable radar and not fault
|
||||
for item in (self._joystick_toggle, self._long_maneuver_toggle, self._lat_maneuver_toggle, self._alpha_long_toggle):
|
||||
item.set_visible(not self._is_release)
|
||||
|
||||
# CP gating
|
||||
if ui_state.CP is not None:
|
||||
alpha_avail = ui_state.CP.alphaLongitudinalAvailable
|
||||
if not alpha_avail:
|
||||
if not alpha_avail or self._is_release:
|
||||
self._alpha_long_toggle.set_visible(False)
|
||||
self._params.remove("AlphaLongitudinalEnabled")
|
||||
else:
|
||||
@@ -129,11 +144,10 @@ class DeveloperLayout(Widget):
|
||||
|
||||
long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad()
|
||||
self._long_maneuver_toggle.action_item.set_enabled(long_man_enabled)
|
||||
if not long_man_enabled:
|
||||
self._long_maneuver_toggle.action_item.set_state(False)
|
||||
self._params.put_bool("LongitudinalManeuverMode", False)
|
||||
self._lat_maneuver_toggle.action_item.set_enabled(ui_state.is_offroad())
|
||||
else:
|
||||
self._long_maneuver_toggle.action_item.set_enabled(False)
|
||||
self._lat_maneuver_toggle.action_item.set_enabled(False)
|
||||
self._alpha_long_toggle.set_visible(False)
|
||||
|
||||
# TODO: make a param control list item so we don't need to manage internal state as much here
|
||||
@@ -144,41 +158,54 @@ class DeveloperLayout(Widget):
|
||||
("SshEnabled", self._ssh_toggle),
|
||||
("JoystickDebugMode", self._joystick_toggle),
|
||||
("LongitudinalManeuverMode", self._long_maneuver_toggle),
|
||||
("LateralManeuverMode", self._lat_maneuver_toggle),
|
||||
("AlphaLongitudinalEnabled", self._alpha_long_toggle),
|
||||
("ShowDebugInfo", self._ui_debug_toggle),
|
||||
):
|
||||
item.action_item.set_state(self._params.get_bool(key))
|
||||
|
||||
def _on_enable_ui_debug(self, state: bool):
|
||||
self._params.put_bool("ShowDebugInfo", state)
|
||||
self._params.put_bool("ShowDebugInfo", state, block=True)
|
||||
gui_app.set_show_touches(state)
|
||||
gui_app.set_show_fps(state)
|
||||
|
||||
def _on_enable_adb(self, state: bool):
|
||||
self._params.put_bool("AdbEnabled", state)
|
||||
self._params.put_bool("AdbEnabled", state, block=True)
|
||||
|
||||
def _on_enable_ssh(self, state: bool):
|
||||
self._params.put_bool("SshEnabled", state)
|
||||
self._params.put_bool("SshEnabled", state, block=True)
|
||||
|
||||
def _on_use_prebuilt(self, state: bool):
|
||||
self._params.put_bool("UsePrebuilt", state)
|
||||
self._params.put_bool("UsePrebuilt", state, block=True)
|
||||
|
||||
def _on_joystick_debug_mode(self, state: bool):
|
||||
self._params.put_bool("JoystickDebugMode", state)
|
||||
self._params.put_bool("LongitudinalManeuverMode", False)
|
||||
self._params.put_bool("JoystickDebugMode", state, block=True)
|
||||
self._params.put_bool("LongitudinalManeuverMode", False, block=True)
|
||||
self._long_maneuver_toggle.action_item.set_state(False)
|
||||
self._params.put_bool("LateralManeuverMode", False, block=True)
|
||||
self._lat_maneuver_toggle.action_item.set_state(False)
|
||||
|
||||
def _on_long_maneuver_mode(self, state: bool):
|
||||
self._params.put_bool("LongitudinalManeuverMode", state)
|
||||
self._params.put_bool("JoystickDebugMode", False)
|
||||
self._params.put_bool("LongitudinalManeuverMode", state, block=True)
|
||||
self._params.put_bool("JoystickDebugMode", False, block=True)
|
||||
self._joystick_toggle.action_item.set_state(False)
|
||||
self._params.put_bool("LateralManeuverMode", False, block=True)
|
||||
self._lat_maneuver_toggle.action_item.set_state(False)
|
||||
|
||||
def _on_lat_maneuver_mode(self, state: bool):
|
||||
self._params.put_bool("LateralManeuverMode", state, block=True)
|
||||
self._params.put_bool("ExperimentalMode", False, block=True)
|
||||
self._params.put_bool("JoystickDebugMode", False, block=True)
|
||||
self._joystick_toggle.action_item.set_state(False)
|
||||
self._params.put_bool("LongitudinalManeuverMode", False, block=True)
|
||||
self._long_maneuver_toggle.action_item.set_state(False)
|
||||
|
||||
def _on_alpha_long_enabled(self, state: bool):
|
||||
if state:
|
||||
def confirm_callback(result: int):
|
||||
def confirm_callback(result: DialogResult):
|
||||
if result == DialogResult.CONFIRM:
|
||||
self._params.put_bool("AlphaLongitudinalEnabled", True)
|
||||
self._params.put_bool("OnroadCycleRequested", True)
|
||||
self._params.put_bool("AlphaLongitudinalEnabled", True, block=True)
|
||||
self._params.put_bool("OnroadCycleRequested", True, block=True)
|
||||
self._update_toggles()
|
||||
else:
|
||||
self._alpha_long_toggle.action_item.set_state(False)
|
||||
@@ -187,9 +214,10 @@ class DeveloperLayout(Widget):
|
||||
content = (f"<h1>{self._alpha_long_toggle.title}</h1><br>" +
|
||||
f"<p>{self._alpha_long_toggle.description}</p>")
|
||||
|
||||
gui_app.push_widget(ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback))
|
||||
dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
|
||||
gui_app.push_widget(dlg)
|
||||
|
||||
else:
|
||||
self._params.put_bool("AlphaLongitudinalEnabled", False)
|
||||
self._params.put_bool("OnroadCycleRequested", True)
|
||||
self._params.put_bool("AlphaLongitudinalEnabled", False, block=True)
|
||||
self._params.put_bool("OnroadCycleRequested", True, block=True)
|
||||
self._update_toggles()
|
||||
|
||||
@@ -36,18 +36,37 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : QFrame(parent) {
|
||||
|
||||
joystickToggle = new ParamControl("JoystickDebugMode", tr("Joystick Debug Mode"), "", "");
|
||||
QObject::connect(joystickToggle, &ParamControl::toggleFlipped, [=](bool state) {
|
||||
params.putBool("LongitudinalManeuverMode", false);
|
||||
longManeuverToggle->refresh();
|
||||
if (state) {
|
||||
params.putBool("LongitudinalManeuverMode", false);
|
||||
params.putBool("LateralManeuverMode", false);
|
||||
longManeuverToggle->refresh();
|
||||
latManeuverToggle->refresh();
|
||||
}
|
||||
});
|
||||
mainList->addItem(joystickToggle);
|
||||
|
||||
longManeuverToggle = new ParamControl("LongitudinalManeuverMode", tr("Longitudinal Maneuver Mode"), "", "");
|
||||
QObject::connect(longManeuverToggle, &ParamControl::toggleFlipped, [=](bool state) {
|
||||
params.putBool("JoystickDebugMode", false);
|
||||
joystickToggle->refresh();
|
||||
if (state) {
|
||||
params.putBool("JoystickDebugMode", false);
|
||||
params.putBool("LateralManeuverMode", false);
|
||||
joystickToggle->refresh();
|
||||
latManeuverToggle->refresh();
|
||||
}
|
||||
});
|
||||
mainList->addItem(longManeuverToggle);
|
||||
|
||||
latManeuverToggle = new ParamControl("LateralManeuverMode", tr("Lateral Maneuver Mode"), "", "");
|
||||
QObject::connect(latManeuverToggle, &ParamControl::toggleFlipped, [=](bool state) {
|
||||
if (state) {
|
||||
params.putBool("JoystickDebugMode", false);
|
||||
params.putBool("LongitudinalManeuverMode", false);
|
||||
joystickToggle->refresh();
|
||||
longManeuverToggle->refresh();
|
||||
}
|
||||
});
|
||||
mainList->addItem(latManeuverToggle);
|
||||
|
||||
experimentalLongitudinalToggle = new ParamControl(
|
||||
"AlphaLongitudinalEnabled",
|
||||
tr("openpilot Longitudinal Control (Alpha)"),
|
||||
|
||||
@@ -20,6 +20,7 @@ private:
|
||||
ParamControl* adbToggle;
|
||||
ParamControl* joystickToggle;
|
||||
ParamControl* longManeuverToggle;
|
||||
ParamControl* latManeuverToggle;
|
||||
ParamControl* experimentalLongitudinalToggle;
|
||||
bool is_release;
|
||||
bool offroad = false;
|
||||
|
||||
@@ -3,29 +3,28 @@ 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
|
||||
import numpy as np
|
||||
from tabulate import tabulate
|
||||
from openpilot.common.utils import tabulate
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
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
|
||||
|
||||
|
||||
def lat_accel(curvature, v_ego):
|
||||
return curvature * max(v_ego, 1.0) ** 2
|
||||
def lat_accel(curvature, v):
|
||||
return curvature * max(v, 1.0) ** 2
|
||||
|
||||
|
||||
def report(platform, route, description_override, 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('/', '_')}.html"
|
||||
output_path.mkdir(exist_ok=True)
|
||||
@@ -38,193 +37,206 @@ def report(platform, route, description_override, CP, ID, maneuvers):
|
||||
f"<h3>{route}</h3>\n",
|
||||
f"<h3>{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}</h3>\n",
|
||||
]
|
||||
if description_override is not None:
|
||||
builder.append(f"<h3>Description: {description_override}</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 }")
|
||||
|
||||
builder.append('{ summary }') # to be replaced below
|
||||
for description, runs in maneuvers:
|
||||
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)}")
|
||||
# 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}')
|
||||
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, start=1):
|
||||
t_car_control, car_control = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == "carControl"], strict=True)
|
||||
t_car_state, car_state = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == "carState"], strict=True)
|
||||
t_controls_state, controls_state = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == "controlsState"], strict=True)
|
||||
t_lateral_plan, lateral_plan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == "lateralManeuverPlan" and m.valid], strict=True)
|
||||
t_car_output, car_output = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == "carOutput"], strict=True)
|
||||
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)
|
||||
|
||||
t_car_control = [(t - t_car_control[0]) / 1e9 for t in t_car_control]
|
||||
t_car_state = [(t - t_car_state[0]) / 1e9 for t in t_car_state]
|
||||
t_controls_state = [(t - t_controls_state[0]) / 1e9 for t in t_controls_state]
|
||||
t_lateral_plan = [(t - t_lateral_plan[0]) / 1e9 for t in t_lateral_plan]
|
||||
t_car_output = [(t - t_car_output[0]) / 1e9 for t in t_car_output]
|
||||
# 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]
|
||||
|
||||
lat_active = [m.latActive for m in car_control]
|
||||
maneuver_valid = all(lat_active) and not any(cs.steeringPressed for cs in car_state)
|
||||
details_open = "open" if maneuver_valid else ""
|
||||
title = f"Run #{run}" + (" <span style='color: red'>(invalid maneuver!)</span>" if not maneuver_valid else "")
|
||||
builder.append(f"<details {details_open}><summary><h3 style='display: inline-block;'>{title}</h3></summary>\n")
|
||||
# maneuver validity
|
||||
latActive = [m.latActive for m in carControl]
|
||||
maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState)
|
||||
|
||||
baseline_accel = lat_accel(controls_state[0].curvature, car_state[0].vEgo)
|
||||
v_ego = [m.vEgo for m in car_state]
|
||||
_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"):
|
||||
amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel) for lp, v in zip(lateral_plan, v_ego, strict=False))
|
||||
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_controls_state, controls_state, v_ego, strict=False):
|
||||
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>")
|
||||
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>")
|
||||
builder.append(', <strong>not crossed</strong>')
|
||||
builder.append('</h3>')
|
||||
if maneuver_valid:
|
||||
target_cross_times.setdefault(description, [])
|
||||
else:
|
||||
action_targets = [(0, lat_accel(lateral_plan[0].desiredCurvature, v_ego[0]) - baseline_accel)]
|
||||
for i in range(1, min(len(lateral_plan), len(v_ego))):
|
||||
if abs(lateral_plan[i].desiredCurvature - lateral_plan[i - 1].desiredCurvature) > 0.001:
|
||||
desired = lat_accel(lateral_plan[i].desiredCurvature, v_ego[i]) - baseline_accel
|
||||
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 action_idx, (start_idx, action_target) in enumerate(action_targets):
|
||||
start_time = t_lateral_plan[start_idx]
|
||||
end_time = t_lateral_plan[action_targets[action_idx + 1][0]] if action_idx + 1 < len(action_targets) else t_controls_state[-1]
|
||||
builder.append(f"<h3 style='font-weight: normal'>aTarget: {round(action_target, 1)} m/s^2")
|
||||
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_controls_state, controls_state, v_ego, strict=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 < action_target < actual_accel) or (0 > action_target > actual_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, action_target + baseline_accel))
|
||||
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>")
|
||||
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, 30))
|
||||
ax = fig.subplots(4, 1, sharex=True, gridspec_kw={"height_ratios": [5, 3, 3, 3]})
|
||||
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_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateral_plan, v_ego, strict=False)]
|
||||
if description.startswith("sine"):
|
||||
ax[0].plot(t_lateral_plan[:len(desired_lat_accel)], desired_lat_accel, label="desired lat accel", linewidth=6)
|
||||
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_lateral_plan[0]] + t_lateral_plan[:len(desired_lat_accel)]
|
||||
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, label="desired lat accel", linewidth=6, where="post")
|
||||
|
||||
actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controls_state, v_ego, strict=False)]
|
||||
ax[0].plot(t_controls_state[:len(actual_lat_accel)], actual_lat_accel, label="actual lat accel", linewidth=6)
|
||||
ax[0].set_ylabel("Lateral Accel (m/s^2)")
|
||||
|
||||
for cross_time, cross_value in cross_markers:
|
||||
ax[0].plot(cross_time, cross_value, marker="o", markersize=50, markeredgewidth=7, markeredgecolor="black", markerfacecolor="None")
|
||||
|
||||
ax2 = ax[0].twinx()
|
||||
if CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
ax2.plot(t_car_output, [-m.actuatorsOutput.steeringAngleDeg for m in car_output], "C2", label="steer angle", linewidth=6)
|
||||
else:
|
||||
ax2.plot(t_car_output, [-m.actuatorsOutput.torque for m in car_output], "C2", label="steer torque", linewidth=6)
|
||||
|
||||
h1, l1 = ax[0].get_legend_handles_labels()
|
||||
h2, l2 = ax2.get_legend_handles_labels()
|
||||
ax[0].legend(h1 + h2, l1 + l2, prop={"size": 30})
|
||||
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)
|
||||
ax[1].plot(t_car_state, [v * CV.MS_TO_MPH for v in v_ego], label="vEgo", linewidth=6)
|
||||
ax[1].set_ylabel("Velocity (mph)")
|
||||
ax[1].yaxis.set_major_formatter(plt.FormatStrFormatter("%.1f"))
|
||||
ax[1].legend()
|
||||
if CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)'
|
||||
elif CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||
steer_field, steer_ylabel = 'curvature', 'Curvature (1/m)'
|
||||
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})
|
||||
|
||||
t_accel = np.array(t_controls_state[:len(actual_lat_accel)])
|
||||
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[2].grid(linewidth=4)
|
||||
ax[2].plot(t_accel, filtered_jerk, label="actual jerk", linewidth=6)
|
||||
if CP.steerControlType == car.CarParams.SteerControlType.torque:
|
||||
desired_jerk = [cs.lateralControlState.torqueState.desiredLateralJerk for cs in controls_state]
|
||||
ax[2].plot(t_controls_state[:len(controls_state)], desired_jerk, label="desired jerk", linewidth=6)
|
||||
ax[2].set_ylabel("Jerk (m/s^3)")
|
||||
ax[2].legend()
|
||||
|
||||
ax[3].grid(linewidth=4)
|
||||
ax[3].plot(t_car_control, [math.degrees(m.orientationNED[0]) for m in car_control], label="roll", linewidth=6)
|
||||
ax[3].set_ylabel("Roll (deg)")
|
||||
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]) 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")
|
||||
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"]
|
||||
rows = []
|
||||
cols = ['maneuver', 'crossed', 'mean', 'min', 'max']
|
||||
table = []
|
||||
for description, times in target_cross_times.items():
|
||||
row = [description, len(times)]
|
||||
if times:
|
||||
row.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)])
|
||||
rows.append(row)
|
||||
summary.append(tabulate(rows, headers=["maneuver", "crossed", "mean", "min", "max"], tablefmt="html", numalign="left") + "\n")
|
||||
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')
|
||||
|
||||
summary_index = builder.index("{ summary }")
|
||||
builder[summary_index:summary_index + 1] = 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))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Generate lateral maneuver report from route")
|
||||
parser.add_argument("route", type=str, help="Route name (e.g. 00000000--5f742174be)")
|
||||
parser.add_argument("description", type=str, nargs="?")
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Generate lateral 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:
|
||||
if '/' in args.route or '|' in args.route:
|
||||
lr = LogReader(args.route, only_union_types=True)
|
||||
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], only_union_types=True)
|
||||
lr = LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True)
|
||||
|
||||
CP = lr.first("carParams")
|
||||
ID = lr.first("initData")
|
||||
CP = lr.first('carParams')
|
||||
ID = lr.first('initData')
|
||||
platform = CP.carFingerprint
|
||||
print("processing report for", platform)
|
||||
print('processing report for', platform)
|
||||
|
||||
maneuvers = []
|
||||
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 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([])
|
||||
|
||||
@@ -17,8 +17,8 @@ STATUS_PARAM = "LateralManeuverStatus"
|
||||
|
||||
# thresholds for starting maneuvers
|
||||
MAX_SPEED_DEV = 0.7 # deviation in m/s
|
||||
MAX_CURV = 0.002 # 500 m radius
|
||||
MAX_ROLL = 0.08 # 4.56 deg
|
||||
MAX_CURV = 0.004 # 250 m radius
|
||||
MAX_ROLL = 0.12 # 6.8°
|
||||
TIMER = 2.0 # sec stable conditions before starting maneuver
|
||||
|
||||
|
||||
@@ -141,6 +141,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])],
|
||||
@@ -159,6 +165,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,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
@@ -175,6 +187,8 @@ def main():
|
||||
maneuvers = iter(MANEUVERS)
|
||||
maneuver = None
|
||||
complete_cnt = 0
|
||||
aborted_cnt = 0
|
||||
abort_reason = ''
|
||||
display_holdoff = 0
|
||||
prev_text = ""
|
||||
last_started_run = None
|
||||
@@ -233,7 +247,13 @@ def main():
|
||||
state = "completed"
|
||||
phase = "holdoff"
|
||||
elif maneuver is not None:
|
||||
if sm['carState'].steeringPressed or (maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV):
|
||||
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
|
||||
@@ -272,6 +292,12 @@ def main():
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
state = "running"
|
||||
phase = "active"
|
||||
elif aborted_cnt > 0:
|
||||
aborted_cnt -= 1
|
||||
alert_msg.alertDebug.alertText1 = abort_reason
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
state = "aborted"
|
||||
phase = "abort"
|
||||
elif not (speed_ready and lat_ready):
|
||||
alert_msg.alertDebug.alertText1 = f"Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph"
|
||||
alert_msg.alertDebug.alertText2 = maneuver.description
|
||||
|
||||
Reference in New Issue
Block a user