From 2bd2a74b2ce0e9830f6193497098285b0fe12afe Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 10 Apr 2024 19:09:57 -0700 Subject: [PATCH] Green light alert Added toggle to alert the user when a red light turns to green. --- cereal/car.capnp | 1 + cereal/custom.capnp | 1 + common/params.cc | 1 + selfdrive/controls/controlsd.py | 21 ++++++++++++++++++- selfdrive/controls/lib/events.py | 8 +++++++ .../frogpilot/controls/frogpilot_planner.py | 5 ++++- .../ui/qt/offroad/visual_settings.cc | 1 + .../frogpilot/ui/qt/offroad/visual_settings.h | 2 +- 8 files changed, 37 insertions(+), 3 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index a7cad41b6..ca0cfd80c 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -120,6 +120,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { # FrogPilot events blockUser @123; goatSteerSaturated @125; + greenLight @126; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 40cef22e9..696a0e669 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -42,6 +42,7 @@ struct FrogPilotPlan @0x80ae746ee2596b11 { laneWidthRight @9 :Float32; minAcceleration @10 :Float32; maxAcceleration @11 :Float32; + redLight @12 :Bool; safeObstacleDistance @13 :Int16; safeObstacleDistanceStock @14 :Int16; stoppedEquivalenceFactor @19 :Int16; diff --git a/common/params.cc b/common/params.cc index e50bf754c..77ca6a1a4 100644 --- a/common/params.cc +++ b/common/params.cc @@ -277,6 +277,7 @@ std::unordered_map keys = { {"FullMap", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"GoatScream", PERSISTENT}, + {"GreenLightAlert", PERSISTENT}, {"HideAOLStatusBar", PERSISTENT}, {"HideCEMStatusBar", PERSISTENT}, {"HideDisableOpenpilotLongitudinal", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8ce14dfc4..d9879d179 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -33,7 +33,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_short_branch -from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -183,6 +183,10 @@ class Controls: self.always_on_lateral = self.params.get_bool("AlwaysOnLateral") self.always_on_lateral_main = self.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain") + self.previously_enabled = False + + self.green_light_mac = MovingAverageCalculator() + self.update_frogpilot_params() def set_initial_state(self): @@ -892,6 +896,17 @@ class Controls: self.events.add(EventName.blockUser) return + if self.green_light_alert: + green_light = not self.sm['frogpilotPlan'].redLight + green_light &= not CS.gasPressed + green_light &= not self.sm['longitudinalPlan'].hasLead + green_light &= self.previously_enabled + green_light &= CS.standstill + + self.green_light_mac.add_data(green_light) + if self.green_light_mac.get_moving_average() >= PROBABILITY: + self.events.add(EventName.greenLight) + def update_frogpilot_variables(self, CS): self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) @@ -912,6 +927,9 @@ class Controls: else: self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + self.previously_enabled |= (self.enabled or self.FPCC.alwaysOnLateral) and CS.vEgo > CRUISING_SPEED + self.previously_enabled &= self.driving_gear + fpcc_send = messaging.new_message('frogpilotCarControl') fpcc_send.valid = CS.canValid fpcc_send.frogpilotCarControl = self.FPCC @@ -926,6 +944,7 @@ class Controls: self.frogpilot_variables.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental") custom_alerts = self.params.get_bool("CustomAlerts") + self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert") custom_theme = self.params.get_bool("CustomTheme") custom_sounds = self.params.get_int("CustomSounds") if custom_theme else 0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 9d0821bfc..400138626 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -975,6 +975,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.goat, 2.), }, + + EventName.greenLight: { + ET.PERMANENT: Alert( + "Light turned green", + "", + AlertStatus.frogpilot, AlertSize.small, + Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), + }, } diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 62c1cc2b4..ae72e7143 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -105,7 +105,7 @@ class FrogPilotPlanner: self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego) - if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl: + if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl or self.green_light_alert: self.cem.update(carState, controlsState.enabled, frogpilotNavigation, modelData, radarState, road_curvature, self.t_follow, v_ego) def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead): @@ -162,6 +162,8 @@ class FrogPilotPlanner: frogpilotPlan.tFollow = float(self.t_follow) frogpilotPlan.vCruise = float(self.v_cruise) + frogpilotPlan.redLight = self.cem.red_light_detected + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self): @@ -172,6 +174,7 @@ class FrogPilotPlanner: self.cem.update_frogpilot_params() custom_alerts = self.params.get_bool("CustomAlerts") + self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert") self.custom_personalities = self.params.get_bool("CustomPersonalities") self.aggressive_jerk = self.params.get_float("AggressiveJerk") diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc index 150cd6ebf..db7dd5d4a 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc @@ -15,6 +15,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"WarningImmediateVolume", tr("Warning Immediate Volume"), tr("Related alerts:\n\nDISENGAGE IMMEDIATELY, Driver Distracted\nDISENGAGE IMMEDIATELY, Driver Unresponsive"), ""}, {"CustomAlerts", tr("Custom Alerts"), tr("Enable custom alerts for openpilot events."), "../frogpilot/assets/toggle_icons/icon_green_light.png"}, + {"GreenLightAlert", tr("Green Light Alert"), tr("Get an alert when a traffic light changes from red to green."), ""}, {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"}, {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h index c0ad37fdd..a251cb60b 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/visual_settings.h @@ -24,7 +24,7 @@ private: void updateToggles(); std::set alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"}; - std::set customAlertsKeys = {}; + std::set customAlertsKeys = {"GreenLightAlert"}; std::set customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "FPSCounter", "LeadInfo"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};