From 6dd80f39a3eeb7fd10e5079ccb5fc321fc77d5e0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 20:31:07 -0700 Subject: [PATCH 1/8] check for overrides as well --- tools/longitudinal_maneuvers/maneuversd.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index 30304a37c27ac4..071927070e2608 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -28,8 +28,8 @@ class Maneuver: _ready_cnt: int = 0 _repeated: int = 0 - def get_accel(self, v_ego: float, enabled: bool, standstill: bool) -> float: - ready = abs(v_ego - self.initial_speed) < 0.3 and enabled and not standstill + def get_accel(self, v_ego: float, long_active: bool, standstill: bool) -> float: + ready = abs(v_ego - self.initial_speed) < 0.3 and long_active and not standstill self._ready_cnt = (self._ready_cnt + 1) if ready else 0 if self._ready_cnt > (3. / DT_MDL): @@ -136,7 +136,7 @@ def main(): v_ego = max(cs.vEgo, 0) if maneuver is not None: - accel = maneuver.get_accel(v_ego, cs.cruiseState.enabled, cs.cruiseState.standstill) + accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, cs.cruiseState.standstill) if maneuver.active: alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2' From cb0c08c73b0eb94754adbfeb1d944d7c0918e1d3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 20:31:27 -0700 Subject: [PATCH 2/8] only used twice --- tools/longitudinal_maneuvers/maneuversd.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index 071927070e2608..15f0c7a4e80cf4 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -132,11 +132,10 @@ def main(): longitudinalPlan = plan_send.longitudinalPlan accel = 0 - cs = sm['carState'] - v_ego = max(cs.vEgo, 0) + v_ego = max(sm['carState'].vEgo, 0) if maneuver is not None: - accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, cs.cruiseState.standstill) + accel = maneuver.get_accel(v_ego, sm['carControl'].longActive, sm['carState'].cruiseState.standstill) if maneuver.active: alert_msg.alertDebug.alertText1 = f'Maneuver Active: {accel:0.2f} m/s^2' From 361d66c5e83808a01c1dd16763935b52000efc48 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 20:33:06 -0700 Subject: [PATCH 3/8] longActive is false for gas pressed --- tools/longitudinal_maneuvers/generate_report.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index b3ab8d9a7a6570..f4abdb0091a82a 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -34,16 +34,19 @@ def report(platform, route, CP, maneuvers): t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True) t_longitudinalPlan, longitudinalPlan = zip(*[(m.logMonoTime, m.longitudinalPlan) for m in msgs if m.which() == 'longitudinalPlan'], strict=True) + # make time relative seconds t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl] t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput] t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState] t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan] - longActive = [m.longActive for m in carControl] - gasPressed = [m.gasPressed for m in carState] - brakePressed = [m.brakePressed for m in carState] + # get first acceleration target and get first + aTarget = longitudinalPlan[0].aTarget + f.write(f'first target: {aTarget}') - maneuver_valid = all(longActive) and not (any(gasPressed) or any(brakePressed)) + # maneuver validity + longActive = [m.longActive for m in carControl] + maneuver_valid = all(longActive) _open = 'open' if maneuver_valid else '' title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') @@ -70,8 +73,8 @@ def report(platform, route, CP, maneuvers): ax[1].legend() ax[2].plot(t_carControl, longActive, label='longActive', linewidth=6) - ax[3].plot(t_carState, gasPressed, label='gasPressed', linewidth=6) - ax[3].plot(t_carState, brakePressed, label='brakePressed', linewidth=6) + ax[3].plot(t_carState, [m.gasPressed for m in carState], label='gasPressed', linewidth=6) + ax[3].plot(t_carState, [m.brakePressed for m in carState], label='brakePressed', linewidth=6) for i in (2, 3): ax[i].set_yticks([0, 1], minor=False) ax[i].set_ylim(-1, 2) From a37de35abe032850f1363cc77fd2cd0fc7ec4cea Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 21:14:12 -0700 Subject: [PATCH 4/8] label cross time --- .../longitudinal_maneuvers/generate_report.py | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index f4abdb0091a82a..5761f905cb8447 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -3,7 +3,7 @@ import base64 import io import os -import json +import pprint from pathlib import Path import matplotlib.pyplot as plt @@ -12,12 +12,12 @@ def format_car_params(CP): - return json.dumps({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2) + return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith('DEPRECATED')}, indent=2) def report(platform, route, CP, maneuvers): output_path = Path(__file__).resolve().parent / "longitudinal_reports" - output_fn = output_path / f"{platform}_{route}.html" + output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html" output_path.mkdir(exist_ok=True) with open(output_fn, "w") as f: f.write("

Longitudinal maneuver report

\n") @@ -40,10 +40,6 @@ def report(platform, route, CP, maneuvers): t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState] t_longitudinalPlan = [(t - t_longitudinalPlan[0]) / 1e9 for t in t_longitudinalPlan] - # get first acceleration target and get first - aTarget = longitudinalPlan[0].aTarget - f.write(f'first target: {aTarget}') - # maneuver validity longActive = [m.longActive for m in carControl] maneuver_valid = all(longActive) @@ -53,8 +49,21 @@ def report(platform, route, CP, maneuvers): f.write(f"

{title}

\n") + # get first acceleration target and first intersection + aTarget = longitudinalPlan[0].aTarget + target_cross_time = None + f.write(f'

Initial aTarget: {aTarget} m/s^2') + for t, cs in zip(t_carState, carState, strict=True): + if (aTarget > 0 and cs.aEgo > aTarget) or (aTarget < 0 and cs.aEgo < aTarget): + f.write(f', crossed in {t:.3f}s') + target_cross_time = t + break + else: + f.write(', not crossed') + f.write('

') + plt.rcParams['font.size'] = 40 - fig = plt.figure(figsize=(30, 25)) + fig = plt.figure(figsize=(30, 26)) ax = fig.subplots(4, 1, sharex=True, gridspec_kw={'height_ratios': [5, 3, 1, 1]}) ax[0].grid(linewidth=4) @@ -67,6 +76,9 @@ def report(platform, route, CP, maneuvers): #ax[0].set_ylim(-6.5, 6.5) ax[0].legend() + if target_cross_time is not None: + ax[0].plot(target_cross_time, aTarget, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None') + ax[1].grid(linewidth=4) ax[1].plot(t_carState, [m.vEgo for m in carState], 'g', label='vEgo', linewidth=6) ax[1].set_ylabel('Velocity (m/s)') From 300e32bf65f7d28cfcc593737381777f14183b32 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 21:14:47 -0700 Subject: [PATCH 5/8] love pycharm --- tools/longitudinal_maneuvers/generate_report.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index 5761f905cb8447..1592a97d65ec3b 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -54,7 +54,7 @@ def report(platform, route, CP, maneuvers): target_cross_time = None f.write(f'

Initial aTarget: {aTarget} m/s^2') for t, cs in zip(t_carState, carState, strict=True): - if (aTarget > 0 and cs.aEgo > aTarget) or (aTarget < 0 and cs.aEgo < aTarget): + if (0 < aTarget < cs.aEgo) or (0 > aTarget > cs.aEgo): f.write(f', crossed in {t:.3f}s') target_cross_time = t break From 3cafeffb508ebe04f8e98a2aa8970229865cdd6a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 22:08:36 -0700 Subject: [PATCH 6/8] quick summary --- tools/longitudinal_maneuvers/generate_report.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index 1592a97d65ec3b..38779dc6343729 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -4,6 +4,7 @@ import io import os import pprint +from collections import defaultdict from pathlib import Path import matplotlib.pyplot as plt @@ -19,6 +20,7 @@ def report(platform, route, CP, maneuvers): output_path = Path(__file__).resolve().parent / "longitudinal_reports" output_fn = output_path / f"{platform}_{route.replace('/', '_')}.html" output_path.mkdir(exist_ok=True) + target_cross_times = defaultdict(list) with open(output_fn, "w") as f: f.write("

Longitudinal maneuver report

\n") f.write(f"

{platform}

\n") @@ -57,6 +59,8 @@ def report(platform, route, CP, maneuvers): if (0 < aTarget < cs.aEgo) or (0 > aTarget > cs.aEgo): f.write(f', crossed in {t:.3f}s') target_cross_time = t + if maneuver_valid: + target_cross_times[description].append(t) break else: f.write(', not crossed') @@ -101,6 +105,13 @@ def report(platform, route, CP, maneuvers): f.write(f"\n") f.write("

\n") + f.write("

Summary

\n") + for idx, (description, times) in enumerate(target_cross_times.items()): + f.write(f"

{description}

\n") + f.write(f"

Target crossed {len(times)} out of {len(maneuvers[idx][1])} runs

\n") + if len(times): + f.write(f"

Mean time to cross: {sum(times) / len(times):.3f}s

\n") + print(f"\nReport written to {output_fn}\n") From 5055a07144b917d2eeb727ca218df7e76d964fad Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 22:15:46 -0700 Subject: [PATCH 7/8] add min/max --- tools/longitudinal_maneuvers/generate_report.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index 38779dc6343729..bf6875b4afc238 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -110,7 +110,7 @@ def report(platform, route, CP, maneuvers): f.write(f"

{description}

\n") f.write(f"

Target crossed {len(times)} out of {len(maneuvers[idx][1])} runs

\n") if len(times): - f.write(f"

Mean time to cross: {sum(times) / len(times):.3f}s

\n") + f.write(f"

Mean time to cross: {sum(times) / len(times):.3f}s, min: {min(times):.3f}s, max: {max(times):.3f}s

\n") print(f"\nReport written to {output_fn}\n") From 74b1bbe799cb3fc8245040187c141e6ca691edc5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 12 Sep 2024 22:23:56 -0700 Subject: [PATCH 8/8] show unreached maneuvers --- tools/longitudinal_maneuvers/generate_report.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index bf6875b4afc238..88f78416b4c1c0 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -106,9 +106,10 @@ def report(platform, route, CP, maneuvers): f.write("\n") f.write("

Summary

\n") - for idx, (description, times) in enumerate(target_cross_times.items()): + for description, runs in maneuvers: + times = target_cross_times[description] f.write(f"

{description}

\n") - f.write(f"

Target crossed {len(times)} out of {len(maneuvers[idx][1])} runs

\n") + f.write(f"

Target crossed {len(times)} out of {len(runs)} runs

\n") if len(times): f.write(f"

Mean time to cross: {sum(times) / len(times):.3f}s, min: {min(times):.3f}s, max: {max(times):.3f}s

\n")