From 93ba52c76c20baf36e07c25d28a98bc8e9ea6219 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 10 Nov 2021 10:54:18 -0800 Subject: [PATCH] controlsd: optimize alert management (#22846) * controlsd: optimize alert management * little cleanup * dataclass is nice * fix tests * update refs * sort by start frame * update refs Co-authored-by: Comma Device --- selfdrive/controls/lib/alertmanager.py | 88 ++++++++++++------------ selfdrive/controls/lib/events.py | 9 ++- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 50 insertions(+), 49 deletions(-) diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 44d6f98770..1c28236fb2 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -1,14 +1,15 @@ -import os import copy +import os import json -from typing import List, Optional +from collections import defaultdict +from dataclasses import dataclass +from typing import List, Dict, Optional from cereal import car, log from common.basedir import BASEDIR from common.params import Params from common.realtime import DT_CTRL from selfdrive.controls.lib.events import Alert -from selfdrive.swaglog import cloudlog with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: @@ -26,13 +27,20 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = Params().delete(alert) +@dataclass +class AlertEntry: + alert: Optional[Alert] = None + start_frame: int = -1 + end_frame: int = -1 + + class AlertManager: def __init__(self): - self.activealerts: List[Alert] = [] - self.clear_current_alert() + self.reset() + self.activealerts: Dict[str, AlertEntry] = defaultdict(AlertEntry) - def clear_current_alert(self) -> None: + def reset(self) -> None: self.alert_type: str = "" self.alert_text_1: str = "" self.alert_text_2: str = "" @@ -44,42 +52,36 @@ class AlertManager: def add_many(self, frame: int, alerts: List[Alert], enabled: bool = True) -> None: for alert in alerts: - added_alert = copy.copy(alert) - added_alert.start_time = frame * DT_CTRL - - # if new alert is higher priority, log it - if not len(self.activealerts) or added_alert.alert_priority > self.activealerts[0].alert_priority: - cloudlog.event('alert_add', alert_type=added_alert.alert_type, enabled=enabled) - - self.activealerts.append(added_alert) + alert_duration = max(alert.duration_sound, alert.duration_hud_alert, alert.duration_text) + self.activealerts[alert.alert_type].alert = alert + self.activealerts[alert.alert_type].start_frame = frame + self.activealerts[alert.alert_type].end_frame = frame + int(alert_duration / DT_CTRL) def process_alerts(self, frame: int, clear_event_type=None) -> None: - cur_time = frame * DT_CTRL - - # first get rid of all the expired alerts - self.activealerts = [a for a in self.activealerts if a.event_type != clear_event_type and - a.start_time + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] - - # sort by priority first and then by start_time - self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True) - - # start with assuming no alerts - self.clear_current_alert() - - if len(self.activealerts): - current_alert = self.activealerts[0] - - self.alert_type = current_alert.alert_type - - if current_alert.start_time + current_alert.duration_sound > cur_time: - self.audible_alert = current_alert.audible_alert - - if current_alert.start_time + current_alert.duration_hud_alert > cur_time: - self.visual_alert = current_alert.visual_alert - - if current_alert.start_time + current_alert.duration_text > cur_time: - self.alert_text_1 = current_alert.alert_text_1 - self.alert_text_2 = current_alert.alert_text_2 - self.alert_status = current_alert.alert_status - self.alert_size = current_alert.alert_size - self.alert_rate = current_alert.alert_rate + current_alert = AlertEntry() + for k, v in self.activealerts.items(): + if v.alert is None: + continue + + if v.alert.event_type == clear_event_type: + self.activealerts[k].end_frame = -1 + + # sort by priority first and then by start_frame + active = self.activealerts[k].end_frame > frame + greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame) + if active and greater: + current_alert = v + + # clear current alert + self.reset() + + a = current_alert.alert + if a is not None: + self.alert_type = a.alert_type + self.audible_alert = a.audible_alert + self.visual_alert = a.visual_alert + self.alert_text_1 = a.alert_text_1 + self.alert_text_2 = a.alert_text_2 + self.alert_status = a.alert_status + self.alert_size = a.alert_size + self.alert_rate = a.alert_rate diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bc5d6a0b10..110b1e2037 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -108,7 +108,7 @@ class Alert: alert_text_2: str, alert_status: log.ControlsState.AlertStatus, alert_size: log.ControlsState.AlertSize, - alert_priority: Priority, + priority: Priority, visual_alert: car.CarControl.HUDControl.VisualAlert, audible_alert: car.CarControl.HUDControl.AudibleAlert, duration_sound: float, @@ -121,7 +121,7 @@ class Alert: self.alert_text_2 = alert_text_2 self.alert_status = alert_status self.alert_size = alert_size - self.alert_priority = alert_priority + self.priority = priority self.visual_alert = visual_alert self.audible_alert = audible_alert @@ -132,15 +132,14 @@ class Alert: self.alert_rate = alert_rate self.creation_delay = creation_delay - self.start_time = 0. self.alert_type = "" self.event_type = None def __str__(self) -> str: - return f"{self.alert_text_1}/{self.alert_text_2} {self.alert_priority} {self.visual_alert} {self.audible_alert}" + return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" def __gt__(self, alert2) -> bool: - return self.alert_priority > alert2.alert_priority + return self.priority > alert2.priority class NoEntryAlert(Alert): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index fa079b323a..e16d08dddd 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -2f47fc0d3bd112be26fee56c782d152d678c76d2 \ No newline at end of file +e3619d2382fa67c2c2d5b7a3d89f74f7d85c411d \ No newline at end of file