mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-16 15:12:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improve lateral maneuvers (#38159)

Browse Source 
* improve lat maneuver report

* rm torque jerk

* add jitter maneuver

* add to 30mph

* add jerk

* color

* more state

* override

* abort reason

* abort

* rm text

* pad for small text
This commit is contained in:
Daniel Koepping
2026-06-11 18:59:56 -07:00
committed by GitHub
parent cd052f124e
commit f9cc67896c
4 changed files with 62 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/ui/layouts/settings/developer.py
+1
View File
@@ -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)
selfdrive/ui/mici/layouts/settings/developer.py
+1
View File
@@ -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)
tools/lateral_maneuvers/generate_report.py
+34 -32
View File
@@ -51,6 +51,8 @@ def report(platform, route, _description, CP, ID, maneuvers):
builder.append("<div style='border-top: 1px solid #000; margin: 20px 0;'></div>\n")
builder.append(f"<h2>{description}</h2>\n")
for run, msgs in enumerate(completed_runs):
last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid)
msgs = [m for m in msgs if m.logMonoTime <= last_active]
t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True)
t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True)
t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True)
@@ -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()
tools/lateral_maneuvers/lateral_maneuversd.py
+26 -3
View File
@@ -12,7 +12,7 @@ from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver a
# thresholds for starting maneuvers
MAX_SPEED_DEV = 0.7 # deviation in m/s
MAX_CURV = 0.002 # 500 m radius
MAX_CURV = 0.004 # 250 m radius
MAX_ROLL = 0.12 # 6.8°
TIMER = 2.0 # sec stable conditions before starting maneuver
@@ -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: