From 82a483fe273111bf2e6615b8ec847b227ef9b63e Mon Sep 17 00:00:00 2001 From: MoreTore Date: Sun, 11 May 2025 23:23:26 -0500 Subject: [PATCH] debug tune --- .../test/process_replay/process_replay.py | 2 +- tools/tuning/measure_steering_accuracy.py | 43 ++++++++++--------- 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1144b7955..122825904 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -123,7 +123,7 @@ class ProcessConfig: should_recv_callback: Callable | None = None tolerance: float | None = None processing_time: float = 0.001 - timeout: int = 30 + timeout: int = 300 simulation: bool = True # Set to service process receives on first main_pub: str | None = None diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index 7e4e97574..e4e137cd4 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -46,7 +46,7 @@ class SteeringAccuracyTool: control_state = lateralControlState.__getattr__(control_type) v_ego = sm['carState'].vEgo - active = sm['controlsState'].active + active = control_state.active steer = sm['carOutput'].actuatorsOutput.torque standstill = sm['carState'].standstill steer_limited_by_safety = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 @@ -59,36 +59,37 @@ class SteeringAccuracyTool: # wait 5 seconds after engage / standstill / override / lane change if self.cnt >= 500: - actual_angle = control_state.steeringAngleDeg - desired_angle = control_state.steeringAngleDesiredDeg + actual_accel = control_state.actualLateralAccel + desired_accel = control_state.desiredLateralAccel # calculate error before rounding, then round for stats grouping - angle_error = abs(desired_angle - actual_angle) - actual_angle = round(actual_angle, 1) - desired_angle = round(desired_angle, 1) - angle_error = round(angle_error, 2) - angle_abs = int(abs(round(desired_angle, 0))) + accel_error = abs(desired_accel - actual_accel) + actual_accel = round(actual_accel, 2) + desired_accel = round(desired_accel, 2) + accel_error = round(accel_error, 3) + accel_abs = round(abs(desired_accel), 1) # bin by 0.1 m/s^2 for group, group_props in self.all_groups.items(): if v_ego > group_props[0]: # collect stats - self.speed_group_stats[group][angle_abs]["cnt"] += 1 - self.speed_group_stats[group][angle_abs]["err"] += angle_error - self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) + self.speed_group_stats[group][accel_abs]["cnt"] += 1 + self.speed_group_stats[group][accel_abs]["err"] += accel_error + self.speed_group_stats[group][accel_abs]["steer"] += abs(steer) if len(model_points): self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0]) if steer_limited_by_safety: self.speed_group_stats[group][angle_abs]["limited"] += 1 if control_state.saturated: - self.speed_group_stats[group][angle_abs]["saturated"] += 1 - if actual_angle == desired_angle: - self.speed_group_stats[group][angle_abs]["="] += 1 + self.speed_group_stats[group][accel_abs]["saturated"] += 1 + # allow a small tolerance for float comparison + if abs(actual_accel - desired_accel) < 0.01: + self.speed_group_stats[group][accel_abs]["="] += 1 else: - if desired_angle == 0.: + if desired_accel == 0.: overshoot = True else: - overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle - self.speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1 + overshoot = desired_accel < actual_accel if desired_accel > 0. else desired_accel > actual_accel + self.speed_group_stats[group][accel_abs]["+" if overshoot else "-"] += 1 break else: self.cnt = 0 @@ -105,8 +106,8 @@ class SteeringAccuracyTool: print(f" {'-'*118}") for k in sorted(self.speed_group_stats[group].keys()): v = self.speed_group_stats[group][k] - print(f' {k:#2}° | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% ' + - f'| error: {round(v["err"] / v["cnt"], 2):2.2f}° | -:{int(v["-"] / v["cnt"] * 100):#3}% ' + + print(f' {k:>4.1f} m/s² | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% ' + + f'| error: {round(v["err"] / v["cnt"], 3):.3f} m/s² | -:{int(v["-"] / v["cnt"] * 100):#3}% ' + f'| =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#3}% | lim:{v["limited"]:#5} ' + f'| sat:{v["saturated"]:#5} | path dev: {round(v["dpp"] / v["cnt"], 2):2.2f}m | total: {v["cnt"]:#5}') print("") @@ -140,8 +141,10 @@ if __name__ == "__main__": sm['controlsState'] = msg.controlsState elif msg.which() == 'modelV2': sm['modelV2'] = msg.modelV2 + elif msg.which() == 'carOutput': + sm['carOutput'] = msg.carOutput - if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'modelV2' in sm: + if msg.which() == 'carControl' and 'carState' in sm and 'controlsState' in sm and 'modelV2' in sm and 'carOutput' in sm: tool.update(sm) else: