From 590649eaf2eb830075821a14d12eccf0b93391c6 Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Traffic Mode --- frogpilot/controls/frogpilot_card.py | 4 ++++ frogpilot/controls/frogpilot_planner.py | 5 ++++- .../controls/lib/frogpilot_acceleration.py | 4 +++- frogpilot/controls/lib/frogpilot_events.py | 9 +++++++++ frogpilot/controls/lib/frogpilot_following.py | 18 ++++++++++++++++-- frogpilot/ui/frogpilot_ui.cc | 1 + frogpilot/ui/frogpilot_ui.h | 1 + .../ui/qt/onroad/frogpilot_annotated_camera.cc | 2 ++ selfdrive/selfdrived/events.py | 16 ++++++++++++++++ selfdrive/ui/qt/onroad/buttons.cc | 2 ++ selfdrive/ui/ui.cc | 2 ++ selfdrive/ui/ui.h | 2 ++ 12 files changed, 62 insertions(+), 4 deletions(-) diff --git a/frogpilot/controls/frogpilot_card.py b/frogpilot/controls/frogpilot_card.py index eca6e388..a5757979 100644 --- a/frogpilot/controls/frogpilot_card.py +++ b/frogpilot/controls/frogpilot_card.py @@ -22,6 +22,7 @@ class FrogPilotCard: self.force_coast = False self.pause_lateral = False self.pause_longitudinal = False + self.traffic_mode_enabled = False self.gap_counter = 0 @@ -42,6 +43,8 @@ class FrogPilotCard: self.pause_lateral = not self.pause_lateral elif sm["carControl"].longActive and getattr(frogpilot_toggles, f"pause_longitudinal_via_{key}"): self.pause_longitudinal = not self.pause_longitudinal + elif getattr(frogpilot_toggles, f"traffic_mode_via_{key}"): + self.traffic_mode_enabled = not self.traffic_mode_enabled def handle_experimental_mode(self, sm, frogpilot_toggles): if frogpilot_toggles.conditional_experimental_mode: @@ -108,5 +111,6 @@ class FrogPilotCard: frogpilotCarState.forceCoast = self.force_coast frogpilotCarState.pauseLateral = self.pause_lateral frogpilotCarState.pauseLongitudinal = self.pause_longitudinal + frogpilotCarState.trafficModeEnabled = self.traffic_mode_enabled return frogpilotCarState diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index a4732e4f..2857f491 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -144,7 +144,10 @@ class FrogPilotPlanner: frogpilotPlan.frogpilotToggles = json.dumps(vars(frogpilot_toggles)) - frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance + if sm["frogpilotCarState"].trafficModeEnabled: + frogpilotPlan.increasedStoppedDistance = 0 + else: + frogpilotPlan.increasedStoppedDistance = frogpilot_toggles.increase_stopped_distance frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 6833b517..96382973 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -52,7 +52,9 @@ class FrogPilotAcceleration: eco_gear = sm["frogpilotCarState"].ecoGear sport_gear = sm["frogpilotCarState"].sportGear - if (eco_gear or sport_gear) and frogpilot_toggles.map_acceleration: + if sm["frogpilotCarState"].trafficModeEnabled: + self.max_accel = get_max_accel(v_ego) + elif (eco_gear or sport_gear) and frogpilot_toggles.map_acceleration: if eco_gear: self.max_accel = get_max_accel_eco(v_ego) else: diff --git a/frogpilot/controls/lib/frogpilot_events.py b/frogpilot/controls/lib/frogpilot_events.py index 19470287..8ae182b8 100644 --- a/frogpilot/controls/lib/frogpilot_events.py +++ b/frogpilot/controls/lib/frogpilot_events.py @@ -22,6 +22,7 @@ class FrogPilotEvents: self.events = Events(frogpilot=True) self.always_on_lateral_enabled_previously = False + self.previous_traffic_mode = False self.random_event_playing = False self.startup_seen = False self.stopped_for_light = False @@ -183,4 +184,12 @@ class FrogPilotEvents: self.startup_seen |= sm["frogpilotSelfdriveState"].alertText1 == frogpilot_toggles.startup_alert_top and sm["frogpilotSelfdriveState"].alertText2 == frogpilot_toggles.startup_alert_bottom + if sm["frogpilotCarState"].trafficModeEnabled != self.previous_traffic_mode: + if self.previous_traffic_mode: + self.events.add(FrogPilotEventName.trafficModeInactive) + else: + self.events.add(FrogPilotEventName.trafficModeActive) + + self.previous_traffic_mode = sm["frogpilotCarState"].trafficModeEnabled + self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names) diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index c3ce7b33..e034dd13 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -3,6 +3,10 @@ import numpy as np from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW +from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT + +TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] + class FrogPilotFollowing: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner @@ -16,7 +20,17 @@ class FrogPilotFollowing: self.t_follow = 0 def update(self, long_control_active, v_ego, sm, frogpilot_toggles): - if long_control_active: + if long_control_active and sm["frogpilotCarState"].trafficModeEnabled: + if sm["carState"].aEgo >= 0: + self.base_acceleration_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration) + self.base_speed_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed) + else: + self.base_acceleration_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_deceleration) + self.base_speed_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed_decrease) + + self.base_danger_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger) + self.t_follow = np.interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_follow) + elif long_control_active: if sm["carState"].aEgo >= 0: self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor( frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed, @@ -52,7 +66,7 @@ class FrogPilotFollowing: self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego if long_control_active and self.frogpilot_planner.tracking_lead: - if frogpilot_toggles.human_following: + if not sm["frogpilotCarState"].trafficModeEnabled and frogpilot_toggles.human_following: self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead) self.desired_follow_distance = desired_follow_distance(v_ego, self.frogpilot_planner.lead_one.vLead, self.t_follow) else: diff --git a/frogpilot/ui/frogpilot_ui.cc b/frogpilot/ui/frogpilot_ui.cc index 5cd45c11..b3dcce74 100644 --- a/frogpilot/ui/frogpilot_ui.cc +++ b/frogpilot/ui/frogpilot_ui.cc @@ -13,6 +13,7 @@ static void update_state(FrogPilotUIState *fs) { if (fpsm.updated("frogpilotCarState")) { const cereal::FrogPilotCarState::Reader &frogpilotCarState = fpsm["frogpilotCarState"].getFrogpilotCarState(); frogpilot_scene.always_on_lateral_active = !frogpilot_scene.enabled && frogpilotCarState.getAlwaysOnLateralEnabled(); + frogpilot_scene.traffic_mode_enabled = frogpilotCarState.getTrafficModeEnabled(); } if (fpsm.updated("frogpilotPlan")) { const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); diff --git a/frogpilot/ui/frogpilot_ui.h b/frogpilot/ui/frogpilot_ui.h index 32019d0c..9eeb5316 100644 --- a/frogpilot/ui/frogpilot_ui.h +++ b/frogpilot/ui/frogpilot_ui.h @@ -15,6 +15,7 @@ struct FrogPilotUIScene { bool reverse; bool sidebars_open; bool standstill; + bool traffic_mode_enabled; bool wake_up_screen; int conditional_status; diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 79b902b1..563c4b35 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -730,6 +730,8 @@ void FrogPilotAnnotatedCameraWidget::paintPathEdges(QPainter &p, int height) { setPathEdgeColors(bg_colors[STATUS_CEM_DISABLED]); } else if (experimentalMode) { setPathEdgeColors(bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]); + } else if (frogpilot_scene.traffic_mode_enabled) { + setPathEdgeColors(bg_colors[STATUS_TRAFFIC_MODE_ENABLED]); } else if (frogpilot_toggles.value("color_scheme").toString() != "stock") { setPathEdgeColors(QColor(frogpilot_toggles.value("path_edges_color").toString())); } else { diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index a91b526e..71f44ba4 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1188,6 +1188,22 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), }, + FrogPilotEventName.trafficModeActive: { + ET.WARNING: Alert( + "Traffic Mode enabled", + "", + FrogPilotAlertStatus.frogpilot, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), + }, + + FrogPilotEventName.trafficModeInactive: { + ET.WARNING: Alert( + "Traffic Mode Disabled", + "", + FrogPilotAlertStatus.frogpilot, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), + }, + # Random Events FrogPilotEventName.accel30: { ET.WARNING: Alert( diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc index f224bf09..5bcf52f0 100644 --- a/selfdrive/ui/qt/onroad/buttons.cc +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -102,6 +102,8 @@ void ExperimentalButton::updateBackgroundColor() { background_color = bg_colors[STATUS_CEM_DISABLED]; } else if (experimental_mode) { background_color = bg_colors[STATUS_EXPERIMENTAL_MODE_ENABLED]; + } else if (frogpilot_scene.traffic_mode_enabled) { + background_color = bg_colors[STATUS_TRAFFIC_MODE_ENABLED]; } else { background_color = QColor(0, 0, 0, 166); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 606bb891..c617b272 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -102,6 +102,8 @@ void UIState::updateStatus(FrogPilotUIState *fs) { status = STATUS_OVERRIDE; } else if (frogpilot_scene.always_on_lateral_active) { status = STATUS_ALWAYS_ON_LATERAL_ACTIVE; + } else if (frogpilot_scene.traffic_mode_enabled && ss.getEnabled()) { + status = STATUS_TRAFFIC_MODE_ENABLED; } else { status = ss.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index a4880809..7b17347b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -49,6 +49,7 @@ typedef enum UIStatus { STATUS_ALWAYS_ON_LATERAL_ACTIVE, STATUS_CEM_DISABLED, STATUS_EXPERIMENTAL_MODE_ENABLED, + STATUS_TRAFFIC_MODE_ENABLED, } UIStatus; const QColor bg_colors [] = { @@ -60,6 +61,7 @@ const QColor bg_colors [] = { [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), [STATUS_CEM_DISABLED] = QColor(0xff, 0xff, 0x00, 0xf1), [STATUS_EXPERIMENTAL_MODE_ENABLED] = QColor(0xda, 0x6f, 0x25, 0xf1), + [STATUS_TRAFFIC_MODE_ENABLED] = QColor(0xc9, 0x22, 0x31, 0xf1), }; typedef struct UIScene {