mawei/onepilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Traffic Mode

Browse Source
This commit is contained in:
James
2025-12-01 12:00:00 -07:00
parent b9d0f979d2
commit 590649eaf2
12 changed files with 62 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/controls/frogpilot_card.py
+4
View File
@@ -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
frogpilot/controls/frogpilot_planner.py
+4 -1
View File
@@ -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
frogpilot/controls/lib/frogpilot_acceleration.py
+3 -1
View File
@@ -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:
frogpilot/controls/lib/frogpilot_events.py
+9
View File
@@ -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)
frogpilot/controls/lib/frogpilot_following.py
+16 -2
View File
@@ -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:
frogpilot/ui/frogpilot_ui.cc
+1
View File
@@ -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();
frogpilot/ui/frogpilot_ui.h
+1
View File
@@ -15,6 +15,7 @@ struct FrogPilotUIScene {
bool reverse;
bool sidebars_open;
bool standstill;
bool traffic_mode_enabled;
bool wake_up_screen;
int conditional_status;
frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc
+2
View File
@@ -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 {
selfdrive/selfdrived/events.py
+16
View File
@@ -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(
selfdrive/ui/qt/onroad/buttons.cc
+2
View File
@@ -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);
}
selfdrive/ui/ui.cc
+2
View File
@@ -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;
}
selfdrive/ui/ui.h
+2
View File
@@ -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 {