diff --git a/selfdrive/ui/layouts/settings/developer.py b/selfdrive/ui/layouts/settings/developer.py index 6417909322..2b17436943 100644 --- a/selfdrive/ui/layouts/settings/developer.py +++ b/selfdrive/ui/layouts/settings/developer.py @@ -131,6 +131,7 @@ 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) + 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) diff --git a/selfdrive/ui/mici/layouts/settings/developer.py b/selfdrive/ui/mici/layouts/settings/developer.py index 2f0250ff5f..2d7dc0899d 100644 --- a/selfdrive/ui/mici/layouts/settings/developer.py +++ b/selfdrive/ui/mici/layouts/settings/developer.py @@ -152,6 +152,7 @@ class DeveloperLayoutMici(NavScroller): long_man_enabled = ui_state.has_longitudinal_control and ui_state.is_offroad() self._long_maneuver_toggle.set_enabled(long_man_enabled) + self._lat_maneuver_toggle.set_enabled(ui_state.is_offroad()) else: self._long_maneuver_toggle.set_enabled(False) self._lat_maneuver_toggle.set_enabled(False) diff --git a/tools/lateral_maneuvers/generate_report.py b/tools/lateral_maneuvers/generate_report.py index 9a6fe1b979..c2ef7e99ea 100755 --- a/tools/lateral_maneuvers/generate_report.py +++ b/tools/lateral_maneuvers/generate_report.py @@ -51,6 +51,8 @@ def report(platform, route, _description, CP, ID, maneuvers): builder.append("
\n") builder.append(f"

{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) @@ -77,7 +79,7 @@ def report(platform, route, _description, CP, ID, maneuvers): v_ego = [m.vEgo for m in carState] cross_markers = [] - if description.startswith('sine'): + 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 @@ -128,58 +130,58 @@ def report(platform, route, _description, CP, ID, maneuvers): 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]}) + 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'): - ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, label='desired lat accel', linewidth=6) + 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, label='desired lat accel', linewidth=6, where='post') + 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, label='actual lat accel', linewidth=6) + 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') - - ax2 = ax[0].twinx() - if CP.steerControlType == car.CarParams.SteerControlType.angle: - ax2.plot(t_carOutput, [-m.actuatorsOutput.steeringAngleDeg for m in carOutput], 'C2', label='steer angle', linewidth=6) - else: - ax2.plot(t_carOutput, [-m.actuatorsOutput.torque for m in carOutput], '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].legend(prop={'size': 30}) ax[1].grid(linewidth=4) - ax[1].plot(t_carState, [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}) + + 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 controlsState] - ax[2].plot(t_controlsState[:len(controlsState)], 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_carControl, [math.degrees(m.orientationNED[0]) for m in carControl], 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() diff --git a/tools/lateral_maneuvers/lateral_maneuversd.py b/tools/lateral_maneuvers/lateral_maneuversd.py index 4f68d9be08..1cc2d3560e 100755 --- 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 @@ -66,6 +66,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])], @@ -84,6 +90,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, + ), ] @@ -98,6 +110,8 @@ def main(): maneuvers = iter(MANEUVERS) maneuver = None complete_cnt = 0 + aborted_cnt = 0 + abort_reason = '' display_holdoff = 0 prev_text = '' @@ -121,8 +135,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 @@ -140,6 +160,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: