From 768a15034d9186039ce69bd15fceb64537283193 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 24 Jun 2024 20:38:56 -0700
Subject: [PATCH] Visuals - Custom Alerts - Lead Departing Alert

Get an alert when the lead vehicle starts departing when at a standstill.
---
 selfdrive/controls/controlsd.py                   |  3 +++
 selfdrive/controls/lib/events.py                  |  8 ++++++++
 selfdrive/frogpilot/controls/frogpilot_planner.py | 12 ++++++++++++
 3 files changed, 23 insertions(+)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 913141f69..651df83fc 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -914,6 +914,9 @@ class Controls:
       if self.sm['frogpilotPlan'].greenLight and not self.sm['longitudinalPlan'].hasLead:
         self.events.add(EventName.greenLight)
 
+    if self.sm['frogpilotPlan'].leadDeparting:
+      self.events.add(EventName.leadDeparting)
+
     if not self.openpilot_crashed_triggered and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')):
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 8a5b95157..f980b2b8f 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -994,6 +994,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
 
+  EventName.leadDeparting: {
+    ET.PERMANENT: Alert(
+      "Lead departed",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   EventName.noLaneAvailable : {
     ET.PERMANENT: no_lane_available_alert,
   },
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index d9c7a3e6f..42b9eb4e1 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -45,6 +45,7 @@ class FrogPilotPlanner:
     self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
+    self.lead_departing = False
     self.override_force_stop = False
     self.override_slc = False
     self.slower_lead = False
@@ -59,6 +60,7 @@ class FrogPilotPlanner:
     self.slc_target = 0
     self.speed_jerk = 0
     self.tracked_model_length = 0
+    self.tracking_lead_distance = 0
     self.v_cruise = 0
     self.vtsc_target = 0
 
@@ -92,6 +94,12 @@ class FrogPilotPlanner:
       self.lane_width_left = 0
       self.lane_width_right = 0
 
+    if frogpilot_toggles.lead_departing_alert and self.tracking_lead and carState.standstill:
+      self.lead_departing = self.lead_one.dRel - self.tracking_lead_distance > 1
+      self.lead_departing &= v_lead > 1
+    else:
+      self.lead_departing = False
+
     self.model_length = modelData.position.x[TRAJECTORY_SIZE - 1]
     self.road_curvature = abs(float(calculate_road_curvature(modelData, v_ego)))
 
@@ -99,6 +107,8 @@ class FrogPilotPlanner:
       self.override_force_stop = False
       self.tracking_lead = self.lead_one.status
       self.tracked_model_length = 0
+    elif not carState.standstill and self.tracking_lead and self.lead_one.dRel < CITY_SPEED_LIMIT:
+      self.tracking_lead_distance = self.lead_one.dRel
     elif carState.standstill and frogpilot_toggles.force_stops:
       self.override_force_stop = True
     else:
@@ -298,6 +308,8 @@ class FrogPilotPlanner:
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
 
+    frogpilotPlan.leadDeparting = self.lead_departing
+
     frogpilotPlan.maxAcceleration = self.max_accel
     frogpilotPlan.minAcceleration = self.min_accel