diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d029650aafcb56..fb58d56472136b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -614,7 +614,7 @@ def publish_logs(self, CS, start_time, actuators, lac_log): clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric]) - self.AM.add_many(self.sm.frame, alerts, self.enabled) + self.AM.add_many(self.sm.frame, alerts) self.AM.process_alerts(self.sm.frame, clear_event) CC.hudControl.visualAlert = self.AM.visual_alert diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 22d7f3eac473da..c8e702c5c7c1d0 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -8,7 +8,6 @@ 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 @@ -33,12 +32,14 @@ class AlertEntry: start_frame: int = -1 end_frame: int = -1 + def active(self, frame: int) -> bool: + return frame <= self.end_frame class AlertManager: def __init__(self): self.reset() - self.activealerts: Dict[str, AlertEntry] = defaultdict(AlertEntry) + self.alerts: Dict[str, AlertEntry] = defaultdict(AlertEntry) def reset(self) -> None: self.alert: Optional[Alert] = None @@ -51,25 +52,27 @@ def reset(self) -> None: self.audible_alert = car.CarControl.HUDControl.AudibleAlert.none self.alert_rate: float = 0. - def add_many(self, frame: int, alerts: List[Alert], enabled: bool = True) -> None: + def add_many(self, frame: int, alerts: List[Alert]) -> None: for alert in alerts: - 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) + key = alert.alert_type + self.alerts[key].alert = alert + if not self.alerts[key].active(frame): + self.alerts[key].start_frame = frame + min_end_frame = self.alerts[key].start_frame + alert.duration + self.alerts[key].end_frame = max(frame + 1, min_end_frame) def process_alerts(self, frame: int, clear_event_type=None) -> None: current_alert = AlertEntry() - for k, v in self.activealerts.items(): + for k, v in self.alerts.items(): if v.alert is None: continue - if v.alert.event_type == clear_event_type: - self.activealerts[k].end_frame = -1 + if clear_event_type is not None and v.alert.event_type == clear_event_type: + self.alerts[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: + if v.active(frame) and greater: current_alert = v # clear current alert diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index e278a00cc5ffa8..6254786b83b362 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -123,7 +123,7 @@ def __init__(self, self.visual_alert = visual_alert self.audible_alert = audible_alert - self.duration = duration + self.duration = int(duration / DT_CTRL) self.alert_rate = alert_rate self.creation_delay = creation_delay diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py new file mode 100755 index 00000000000000..bb6664303b7e24 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import random +import unittest + +from selfdrive.controls.lib.events import Alert, EVENTS +from selfdrive.controls.lib.alertmanager import AlertManager + + +class TestAlertManager(unittest.TestCase): + + def test_duration(self): + """ + Enforce that an alert lasts for max(alert duration, duration the alert is added) + """ + for duration in range(1, 100): + alert = None + while not isinstance(alert, Alert): + event = random.choice([e for e in EVENTS.values() if len(e)]) + alert = random.choice(list(event.values())) + + alert.duration = duration + + # check two cases: + # - alert is added to AM for <= the alert's duration + # - alert is added to AM for > alert's duration + for greater in (True, False): + if greater: + add_duration = duration + random.randint(1, 10) + else: + add_duration = random.randint(1, duration) + show_duration = max(duration, add_duration) + + AM = AlertManager() + for frame in range(duration+10): + if frame < add_duration: + AM.add_many(frame, [alert, ]) + AM.process_alerts(frame) + + shown = AM.alert is not None + should_show = frame <= show_duration + self.assertEqual(shown, should_show, msg=f"{frame=} {add_duration=} {duration=}") + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1c523cd77e04c2..c162de2bb26bf2 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c09fc7b1409529a9991428845ee14f0e37d95b2d \ No newline at end of file +0ae46ae318a63476d8905aa0c32b0e587177868a \ No newline at end of file