diff --git a/selfdrive/ui/layouts/settings/developer.py b/selfdrive/ui/layouts/settings/developer.py index 437376d94..15daff0e6 100644 --- a/selfdrive/ui/layouts/settings/developer.py +++ b/selfdrive/ui/layouts/settings/developer.py @@ -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( - "WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).

" + + "WARNING: openpilot longitudinal control is in alpha for this car and may disable Automatic Emergency Braking (AEB).

" + "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"

{self._alpha_long_toggle.title}


" + f"

{self._alpha_long_toggle.description}

") - 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() diff --git a/selfdrive/ui/qt/offroad/developer_panel.cc b/selfdrive/ui/qt/offroad/developer_panel.cc index a9c3b6890..6c9512a40 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.cc +++ b/selfdrive/ui/qt/offroad/developer_panel.cc @@ -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)"), diff --git a/selfdrive/ui/qt/offroad/developer_panel.h b/selfdrive/ui/qt/offroad/developer_panel.h index dc9665b9a..5a0cdc47b 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.h +++ b/selfdrive/ui/qt/offroad/developer_panel.h @@ -20,6 +20,7 @@ private: ParamControl* adbToggle; ParamControl* joystickToggle; ParamControl* longManeuverToggle; + ParamControl* latManeuverToggle; ParamControl* experimentalLongitudinalToggle; bool is_release; bool offroad = false; diff --git a/tools/lateral_maneuvers/generate_report.py b/tools/lateral_maneuvers/generate_report.py index cc47346f9..c2ef7e99e 100755 --- a/tools/lateral_maneuvers/generate_report.py +++ b/tools/lateral_maneuvers/generate_report.py @@ -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"

{route}

\n", f"

{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}

\n", ] - if description_override is not None: - builder.append(f"

Description: {description_override}

\n") + if _description is not None: + builder.append(f"

Description: {_description}

\n") builder.append(f"

CarParams

{format_car_params(CP)}
\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("
\n") builder.append(f"

{description}

\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}" + (" (invalid maneuver!)" if not maneuver_valid else "") - builder.append(f"

{title}

\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}' + (' (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"): - 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("

50% peak") - for t, cs, v in zip(t_controls_state, controls_state, v_ego, strict=False): + 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") + 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("

") + builder.append(', not crossed') + builder.append('') + 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"

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'

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", crossed in {cross_time:.3f}s") - cross_markers.append((t, action_target + baseline_accel)) + 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("

") + builder.append(', not crossed') + builder.append('') + 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"\n") builder.append("
\n") summary = ["

Summary

\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([]) diff --git a/tools/lateral_maneuvers/lateral_maneuversd.py b/tools/lateral_maneuvers/lateral_maneuversd.py index b7cca090f..625912d60 100755 --- a/tools/lateral_maneuvers/lateral_maneuversd.py +++ b/tools/lateral_maneuvers/lateral_maneuversd.py @@ -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