Visuals - Custom Alerts - Lead Departing Alert

Get an alert when the lead vehicle starts departing when at a standstill.

selfdrive/controls/controlsd.py

@@ -921,6 +921,9 @@ class Controls:
     else:
       self.stopped_for_light = False
 
+    if 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

selfdrive/controls/lib/events.py

@@ -1002,6 +1002,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,
   },

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -51,6 +51,7 @@ class FrogPilotPlanner:
     self.mtsc = MapTurnSpeedController()
 
     self.forcing_stop = False
+    self.lead_departing = False
     self.model_stopped = False
     self.override_force_stop = False
     self.override_slc = False
@@ -66,6 +67,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
 
@@ -104,6 +106,16 @@ class FrogPilotPlanner:
       self.lane_width_left = 0
       self.lane_width_right = 0
 
+    if frogpilot_toggles.lead_departing_alert and self.tracking_lead and driving_gear and carState.standstill:
+      if self.tracking_lead_distance == 0:
+        self.tracking_lead_distance = lead_distance
+
+      self.lead_departing = lead_distance - self.tracking_lead_distance > 1
+      self.lead_departing &= v_lead > 1
+    else:
+      self.lead_departing = False
+      self.tracking_lead_distance = 0
+
     self.model_length = modelData.position.x[MODEL_LENGTH - 1]
     self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
     self.model_stopped |= self.forcing_stop
@@ -323,6 +335,8 @@ class FrogPilotPlanner:
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
 
+    frogpilotPlan.leadDeparting = self.lead_departing
+
     frogpilotPlan.maxAcceleration = float(self.max_accel)
     frogpilotPlan.minAcceleration = float(self.min_accel)