diff --git a/cereal/custom.capnp b/cereal/custom.capnp index bf7a67ea9..46206913e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -19,9 +19,12 @@ struct FrogPilotDeviceState @0xf35cc4560bbf6ec2 { } struct FrogPilotNavigation @0xda96579883444c35 { + approachingIntersection @0 :Bool; + approachingTurn @1 :Bool; } struct FrogPilotPlan @0x80ae746ee2596b11 { + conditionalExperimental @3 :Bool; jerk @7 :Float32; laneWidthLeft @8 :Float32; laneWidthRight @9 :Float32; diff --git a/common/params.cc b/common/params.cc index f148c3acc..572764a00 100644 --- a/common/params.cc +++ b/common/params.cc @@ -220,6 +220,20 @@ std::unordered_map keys = { {"BlindSpotPath", PERSISTENT}, {"CameraView", PERSISTENT}, {"CameraViewReset", PERSISTENT}, + {"CECurves", PERSISTENT}, + {"CECurvesLead", PERSISTENT}, + {"CENavigation", PERSISTENT}, + {"CENavigationIntersections", PERSISTENT}, + {"CENavigationLead", PERSISTENT}, + {"CENavigationTurns", PERSISTENT}, + {"CESignal", PERSISTENT}, + {"CESlowerLead", PERSISTENT}, + {"CESpeed", PERSISTENT}, + {"CESpeedLead", PERSISTENT}, + {"CEStatus", PERSISTENT}, + {"CEStopLights", PERSISTENT}, + {"CEStopLightsLead", PERSISTENT}, + {"ConditionalExperimental", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomPaths", PERSISTENT}, {"CustomUI", PERSISTENT}, @@ -229,6 +243,7 @@ std::unordered_map keys = { {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, {"HideAOLStatusBar", PERSISTENT}, + {"HideCEMStatusBar", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"ManualUpdateInitiated", PERSISTENT}, diff --git a/release/files_common b/release/files_common index aec0788bf..d10c1bc8e 100644 --- a/release/files_common +++ b/release/files_common @@ -566,4 +566,5 @@ tinygrad_repo/tinygrad/*.py selfdrive/frogpilot/frogpilot_process.py selfdrive/frogpilot/controls/frogpilot_planner.py +selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py selfdrive/frogpilot/controls/lib/frogpilot_functions.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6730ffd91..51316699f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -866,7 +866,8 @@ class Controls: def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + if self.CP.openpilotLongitudinalControl and not self.frogpilot_variables.conditional_experimental_mode: + self.experimental_mode = self.params.get_bool("ExperimentalMode") self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -898,6 +899,9 @@ class Controls: self.FPCC.alwaysOnLateral &= self.driving_gear self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill + if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode: + self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental + fpcc_send = messaging.new_message('frogpilotCarControl') fpcc_send.valid = CS.canValid fpcc_send.frogpilotCarControl = self.FPCC @@ -909,6 +913,8 @@ class Controls: def update_frogpilot_params(self): self.always_on_lateral_pause_speed = self.always_on_lateral and self.params.get_int("PauseAOLOnBrake") + self.frogpilot_variables.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental") + custom_alerts = self.params.get_bool("CustomAlerts") lateral_tune = self.params.get_bool("LateralTune") diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png b/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png new file mode 100644 index 000000000..9834f8636 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 5fa0743d5..2008d93e1 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, from openpilot.system.version import get_short_branch +from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature # Acceleration profiles - Credit goes to the DragonPilot team! @@ -47,6 +48,8 @@ class FrogPilotPlanner: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.cem = ConditionalExperimentalMode() + self.staging = get_short_branch() in ["FrogPilot-Development", "FrogPilot-Staging", "FrogPilot-Testing"] self.jerk = 0 @@ -95,6 +98,9 @@ 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: + 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): lead_distance = radarState.leadOne.dRel @@ -132,6 +138,7 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan + frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.jerk = float(self.jerk) frogpilotPlan.laneWidthLeft = self.lane_width_left frogpilotPlan.laneWidthRight = self.lane_width_right @@ -145,6 +152,10 @@ class FrogPilotPlanner: def update_frogpilot_params(self): self.is_metric = self.params.get_bool("IsMetric") + self.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental") + if self.conditional_experimental_mode: + self.cem.update_frogpilot_params() + custom_alerts = self.params.get_bool("CustomAlerts") custom_ui = self.params.get_bool("CustomUI") diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py new file mode 100644 index 000000000..0cf0451ea --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -0,0 +1,182 @@ +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY, MovingAverageCalculator + +SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] +SLOW_DOWN_DISTANCE = [20, 30., 50., 70., 80., 90., 105., 120.] +TRAJECTORY_SIZE = 33 + +class ConditionalExperimentalMode: + def __init__(self): + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.curve_detected = False + self.experimental_mode = False + self.lead_detected = False + self.red_light_detected = False + self.slower_lead_detected = False + + self.previous_status_value = 0 + self.previous_v_ego = 0 + self.previous_v_lead = 0 + self.status_value = 0 + + self.curvature_mac = MovingAverageCalculator() + self.lead_detection_mac = MovingAverageCalculator() + self.lead_slowing_down_mac = MovingAverageCalculator() + self.slow_lead_mac = MovingAverageCalculator() + self.slowing_down_mac = MovingAverageCalculator() + self.stop_light_mac = MovingAverageCalculator() + + self.update_frogpilot_params() + + def update(self, carState, enabled, frogpilotNavigation, modelData, radarState, road_curvature, t_follow, v_ego): + lead = radarState.leadOne + lead_distance = lead.dRel + standstill = carState.standstill + v_lead = lead.vLead + + self.update_conditions(lead_distance, lead.status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead) + + condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, standstill, v_ego) and enabled + if condition_met: + self.experimental_mode = True + else: + self.experimental_mode = False + self.status_value = 0 + + if self.status_value != self.previous_status_value: + self.params_memory.put_int("CEStatus", self.status_value) + self.previous_status_value = self.status_value + + if self.params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + def check_conditions(self, carState, frogpilotNavigation, lead, modelData, standstill, v_ego): + if standstill: + self.status_value = 0 + return self.experimental_mode + + # Keep Experimental Mode active if stopping for a red light + if self.status_value == 15 and self.slowing_down(v_ego): + return True + + if self.navigation and modelData.navEnabled and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn) and (self.navigation_lead or not self.lead_detected): + self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8 + return True + + if (not self.lead_detected and v_ego <= self.limit) or (self.lead_detected and v_ego <= self.limit_lead): + self.status_value = 10 if self.lead_detected else 11 + return True + + if self.slower_lead and self.slower_lead_detected: + self.status_value = 12 + return True + + if self.signal and v_ego <= CITY_SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker): + self.status_value = 13 + return True + + if self.curves and self.curve_detected: + self.status_value = 14 + return True + + if self.stop_lights and self.red_light_detected: + self.status_value = 15 + return True + + return False + + def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, standstill, t_follow, v_ego, v_lead): + self.lead_detection(lead_status) + self.road_curvature(road_curvature) + self.slow_lead(lead_distance, t_follow, v_ego) + self.stop_sign_and_light(lead_distance, modelData, standstill, v_ego, v_lead) + + def lead_detection(self, lead_status): + self.lead_detection_mac.add_data(lead_status) + self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY + + def lead_slowing_down(self, lead_distance, v_ego, v_lead): + if self.lead_detected: + lead_close = lead_distance < CITY_SPEED_LIMIT + lead_far = lead_distance >= CITY_SPEED_LIMIT and (v_lead >= self.previous_v_lead > 1 or v_lead > v_ego) + lead_slowing_down = v_lead < self.previous_v_lead + lead_stopped = v_lead < 1 + + self.previous_v_lead = v_lead + + self.lead_slowing_down_mac.add_data((lead_close or lead_slowing_down or lead_stopped) and not lead_far) + return self.lead_slowing_down_mac.get_moving_average() >= PROBABILITY + else: + self.lead_slowing_down_mac.reset_data() + self.previous_v_lead = 0 + return False + + # Determine the road curvature - Credit goes to to Pfeiferj! + def road_curvature(self, road_curvature): + lead_check = self.curves_lead or not self.lead_detected + + if lead_check and not self.red_light_detected: + # Setting a limit of 3.5 helps prevent it triggering for red lights + curve_detected = 3.5 >= road_curvature > 1.6 + curve_active = 3.5 >= road_curvature > 1.1 and self.curve_detected + + self.curvature_mac.add_data(curve_detected or curve_active) + self.curve_detected = self.curvature_mac.get_moving_average() >= PROBABILITY + else: + self.curvature_mac.reset_data() + self.curve_detected = False + + def slow_lead(self, lead_distance, t_follow, v_ego): + if self.lead_detected: + slower_lead_ahead = lead_distance < (v_ego - 1) * t_follow + + self.slow_lead_mac.add_data(slower_lead_ahead) + self.slower_lead_detected = self.slow_lead_mac.get_moving_average() >= PROBABILITY + else: + self.slow_lead_mac.reset_data() + self.slower_lead_detected = False + + def slowing_down(self, v_ego): + slowing_down = v_ego <= self.previous_v_ego + speed_check = v_ego < CRUISING_SPEED + + self.previous_v_ego = v_ego + + self.slowing_down_mac.add_data(slowing_down and speed_check) + return self.slowing_down_mac.get_moving_average() >= PROBABILITY + + # Stop sign/stop light detection - Credit goes to the DragonPilot team! + def stop_sign_and_light(self, lead_distance, modelData, standstill, v_ego, v_lead): + lead_check = self.stop_lights_lead or not self.lead_slowing_down(lead_distance, v_ego, v_lead) or standstill + + model_check = len(modelData.orientation.x) == len(modelData.position.x) == TRAJECTORY_SIZE + model_stopping = modelData.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego * CV.MS_TO_KPH, SLOW_DOWN_BP, SLOW_DOWN_DISTANCE) + + model_filtered = not (self.curve_detected or self.slower_lead_detected) + + self.stop_light_mac.add_data(lead_check and model_check and model_stopping and model_filtered) + self.red_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY + + def update_frogpilot_params(self): + is_metric = self.params.get_bool("IsMetric") + + self.curves = self.params.get_bool("CECurves") + self.curves_lead = self.curves and self.params.get_bool("CECurvesLead") + + self.limit = self.params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + self.limit_lead = self.params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + + self.navigation = self.params.get_bool("CENavigation") + self.navigation_lead = self.navigation and self.params.get_bool("CENavigationLead") + + self.signal = self.params.get_bool("CESignal") + + self.slower_lead = self.params.get_bool("CESlowerLead") + + self.stop_lights = self.params.get_bool("CEStopLights") + self.stop_lights_lead = self.stop_lights and self.params.get_bool("CEStopLightsLead") diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 53f3b0471..aa04731d0 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -10,6 +10,14 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"PauseAOLOnBrake", tr("Pause On Brake"), tr("Pause 'Always On Lateral' when the brake pedal is being pressed below the set speed."), ""}, {"HideAOLStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Always On Lateral'."), ""}, + {"ConditionalExperimental", tr("Conditional Experimental Mode"), tr("Automatically switches to 'Experimental Mode' under predefined conditions."), "../frogpilot/assets/toggle_icons/icon_conditional.png"}, + {"CECurves", tr("Curve Detected Ahead"), tr("Switch to 'Experimental Mode' when a curve is detected."), ""}, + {"CENavigation", tr("Navigation Based"), tr("Switch to 'Experimental Mode' based on navigation data. (i.e. Intersections, stop signs, upcoming turns, etc.)"), ""}, + {"CESlowerLead", tr("Slower Lead Detected Ahead"), tr("Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead."), ""}, + {"CEStopLights", tr("Stop Lights and Stop Signs"), tr("Switch to 'Experimental Mode' when a stop light or stop sign is detected."), ""}, + {"CESignal", tr("Turn Signal When Below Highway Speeds"), tr("Switch to 'Experimental Mode' when using turn signals below highway speeds to help assist with turns."), ""}, + {"HideCEMStatusBar", tr("Hide the Status Bar"), tr("Don't use the status bar for 'Conditional Experimental Mode'."), ""}, + {"LateralTune", tr("Lateral Tuning"), tr("Modify openpilot's steering behavior."), "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"}, @@ -35,6 +43,44 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } else if (param == "PauseAOLOnBrake") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map(), this, false, tr(" mph")); + } else if (param == "ConditionalExperimental") { + FrogPilotParamManageControl *conditionalExperimentalToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(conditionalExperimentalToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + openParentToggle(); + conditionalSpeedsImperial->setVisible(!isMetric); + conditionalSpeedsMetric->setVisible(isMetric); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end()); + } + }); + toggle = conditionalExperimentalToggle; + } else if (param == "CECurves") { + FrogPilotParamValueControl *CESpeedImperial = new FrogPilotParamValueControl("CESpeed", tr("Below"), tr("Switch to 'Experimental Mode' below this speed when not following a lead vehicle."), "", 0, 99, + std::map(), this, false, tr(" mph")); + FrogPilotParamValueControl *CESpeedLeadImperial = new FrogPilotParamValueControl("CESpeedLead", tr(" w/Lead"), tr("Switch to 'Experimental Mode' below this speed when following a lead vehicle."), "", 0, 99, + std::map(), this, false, tr(" mph")); + conditionalSpeedsImperial = new FrogPilotDualParamControl(CESpeedImperial, CESpeedLeadImperial, this); + addItem(conditionalSpeedsImperial); + + FrogPilotParamValueControl *CESpeedMetric = new FrogPilotParamValueControl("CESpeed", tr("Below"), tr("Switch to 'Experimental Mode' below this speed in absence of a lead vehicle."), "", 0, 150, + std::map(), this, false, tr(" kph")); + FrogPilotParamValueControl *CESpeedLeadMetric = new FrogPilotParamValueControl("CESpeedLead", tr(" w/Lead"), tr("Switch to 'Experimental Mode' below this speed when following a lead vehicle."), "", 0, 150, + std::map(), this, false, tr(" kph")); + conditionalSpeedsMetric = new FrogPilotDualParamControl(CESpeedMetric, CESpeedLeadMetric, this); + addItem(conditionalSpeedsMetric); + + std::vector curveToggles{"CECurvesLead"}; + std::vector curveToggleNames{tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, curveToggles, curveToggleNames); + } else if (param == "CENavigation") { + std::vector navigationToggles{"CENavigationIntersections", "CENavigationTurns", "CENavigationLead"}; + std::vector navigationToggleNames{tr("Intersections"), tr("Turns"), tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, navigationToggles, navigationToggleNames); + } else if (param == "CEStopLights") { + std::vector stopLightToggles{"CEStopLightsLead"}; + std::vector stopLightToggleNames{tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -189,6 +235,8 @@ void FrogPilotControlsPanel::updateMetric() { double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; + params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); + params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion)); } @@ -204,7 +252,10 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::hideToggles() { - std::set longitudinalKeys = {"LongitudinalTune"}; + conditionalSpeedsImperial->setVisible(false); + conditionalSpeedsMetric->setVisible(false); + + std::set longitudinalKeys = {"ConditionalExperimental", "LongitudinalTune"}; for (auto &[key, toggle] : toggles) { toggle->setVisible(false); diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index e0dcaf971..923b2cad7 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -25,8 +25,11 @@ private: void updateState(); void updateToggles(); + FrogPilotDualParamControl *conditionalSpeedsImperial; + FrogPilotDualParamControl *conditionalSpeedsMetric; + std::set aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"}; - std::set conditionalExperimentalKeys = {}; + std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"}; std::set deviceManagementKeys = {}; std::set experimentalModeActivationKeys = {}; std::set laneChangeKeys = {}; diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 6d887b4a6..09e7aea58 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -9,6 +9,7 @@ import requests import cereal.messaging as messaging from cereal import log from openpilot.common.api import Api +from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, @@ -61,6 +62,12 @@ class RouteEngine: self.mapbox_host = "https://maps.comma.ai" # FrogPilot variables + self.stop_coord = [] + self.stop_signal = [] + + self.approaching_intersection = False + self.approaching_turn = False + self.update_frogpilot_params() def update(self): @@ -169,6 +176,17 @@ class RouteEngine: self.route = r['routes'][0]['legs'][0]['steps'] self.route_geometry = [] + # Iterate through the steps in self.route to find "stop_sign" and "traffic_light" + if self.conditional_navigation_intersections: + self.stop_signal = [] + self.stop_coord = [] + + for step in self.route: + for intersection in step["intersections"]: + if "stop_sign" in intersection or "traffic_signal" in intersection: + self.stop_signal.append(intersection["geometry_index"]) + self.stop_coord.append(Coordinate.from_mapbox_tuple(intersection["location"])) + maxspeed_idx = 0 maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed'] @@ -309,9 +327,35 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() + # 5-10 Seconds to stop condition based on the current speed or minimum of 25 meters + if self.conditional_navigation: + v_ego = self.sm['carState'].vEgo + seconds_to_stop = interp(v_ego, [0, 22.3, 44.7], [5, 10, 10]) + + # Determine the location of the closest upcoming stopSign or trafficLight + closest_condition_indices = [idx for idx in self.stop_signal if idx >= closest_idx] + if closest_condition_indices: + closest_condition_index = min(closest_condition_indices, key=lambda idx: abs(closest_idx - idx)) + index = self.stop_signal.index(closest_condition_index) + + # Calculate the distance to the stopSign or trafficLight + distance_to_condition = self.last_position.distance_to(self.stop_coord[index]) + self.approaching_intersection = self.conditional_navigation_intersections and distance_to_condition < max((seconds_to_stop * v_ego), 25) + else: + self.approaching_intersection = False # No more stopSign or trafficLight in array + + # Determine if NoO distance to maneuver is upcoming + self.approaching_turn = self.conditional_navigation_turns and distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25) + else: + self.approaching_intersection = False + self.approaching_turn = False + frogpilot_plan_send = messaging.new_message('frogpilotNavigation') frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation + frogpilotNavigation.approachingIntersection = self.approaching_intersection + frogpilotNavigation.approachingTurn = self.approaching_turn + self.pm.send('frogpilotNavigation', frogpilot_plan_send) def send_route(self): @@ -363,10 +407,13 @@ class RouteEngine: # TODO: Check for going wrong way in segment def update_frogpilot_params(self): + self.conditional_navigation = self.params.get_bool("CENavigation") + self.conditional_navigation_intersections = self.conditional_navigation and self.params.get_bool("CENavigationIntersections") + self.conditional_navigation_turns = self.conditional_navigation and self.params.get_bool("CENavigationTurns") def main(): pm = messaging.PubMaster(['navInstruction', 'navRoute', 'frogpilotNavigation']) - sm = messaging.SubMaster(['liveLocationKalman', 'managerState']) + sm = messaging.SubMaster(['carState', 'liveLocationKalman', 'managerState']) rk = Ratekeeper(1.0) route_engine = RouteEngine(sm, pm) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 7fe1818a3..1887150b5 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -194,7 +194,12 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); if (hasLongitudinalControl(CP)) { // normal description and toggle - experimental_mode_toggle->setEnabled(true); + bool conditional_experimental = params.getBool("ConditionalExperimental"); + if (conditional_experimental) { + params.putBool("ExperimentalMode", true); + experimental_mode_toggle->refresh(); + } + experimental_mode_toggle->setEnabled(!conditional_experimental); experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); } else { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index fd9ca07cd..2325ee588 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -173,7 +173,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = scene.show_aol_status_bar ? 25 : 0; + int offset = scene.show_aol_status_bar || scene.show_cem_status_bar ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; @@ -563,7 +563,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) // base icon int offset = UI_BORDER_SIZE + btn_size / 2; int x = rightHandDM ? width() - offset : offset; - offset += showAlwaysOnLateralStatusBar ? 25 : 0; + offset += showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar ? 25 : 0; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); @@ -760,13 +760,16 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { cameraView = scene.camera_view; + conditionalStatus = scene.conditional_status; + showConditionalExperimentalStatusBar = scene.show_cem_status_bar; + experimentalMode = scene.experimental_mode; mapOpen = scene.map_open; } void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { - if (showAlwaysOnLateralStatusBar) { + if (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar) { drawStatusBar(p); } @@ -797,8 +800,29 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + std::map conditionalStatusMap = { + {0, tr("Conditional Experimental Mode ready")}, + {1, tr("Conditional Experimental overridden")}, + {2, tr("Experimental Mode manually activated")}, + {3, tr("Conditional Experimental overridden")}, + {4, tr("Experimental Mode manually activated")}, + {5, tr("Conditional Experimental overridden")}, + {6, tr("Experimental Mode manually activated")}, + {7, tr("Experimental Mode activated for") + (mapOpen ? tr(" intersection") : tr(" upcoming intersection"))}, + {8, tr("Experimental Mode activated for") + (mapOpen ? tr(" turn") : tr(" upcoming turn"))}, + {9, tr("Experimental Mode activated due to") + (mapOpen ? tr(" SLC") : tr(" no speed limit set"))}, + {10, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(scene.conditional_speed_lead) + (is_metric ? tr(" kph") : tr(" mph")))}, + {11, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(scene.conditional_speed) + (is_metric ? tr(" kph") : tr(" mph")))}, + {12, tr("Experimental Mode activated for slower lead")}, + {13, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))}, + {14, tr("Experimental Mode activated for curve")}, + {15, tr("Experimental Mode activated for stop") + (mapOpen ? "" : tr(" sign / stop light"))}, + }; + if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) { newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable")); + } else if (showConditionalExperimentalStatusBar) { + newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0]; } if (newStatus != lastShownStatus) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 3b5391ada..aab6d9d3a 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -124,9 +124,11 @@ private: bool experimentalMode; bool mapOpen; bool showAlwaysOnLateralStatusBar; + bool showConditionalExperimentalStatusBar; int alertSize; int cameraView; + int conditionalStatus; protected: void paintGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3c44ae795..b2f79ec14 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -263,6 +263,11 @@ void ui_update_frogpilot_params(UIState *s) { bool always_on_lateral = params.getBool("AlwaysOnLateral"); scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar"); + scene.conditional_experimental = scene.longitudinal_control && params.getBool("ConditionalExperimental"); + scene.conditional_speed = scene.conditional_experimental ? params.getInt("CESpeed") : 0; + scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0; + scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar"); + bool custom_onroad_ui = params.getBool("CustomUI"); bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths"); scene.acceleration_path = custom_paths && params.getBool("AccelerationPath"); @@ -340,6 +345,7 @@ void UIState::update() { } // FrogPilot live variables that need to be constantly checked + scene.conditional_status = scene.conditional_experimental ? paramsMemory.getInt("CEStatus") : 0; } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index dd50f9ef4..86a67556e 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -176,11 +176,13 @@ typedef struct UIScene { bool blind_spot_left; bool blind_spot_path; bool blind_spot_right; + bool conditional_experimental; bool enabled; bool experimental_mode; bool map_open; bool right_hand_drive; bool show_aol_status_bar; + bool show_cem_status_bar; float acceleration; float lane_width_left; @@ -188,6 +190,9 @@ typedef struct UIScene { int alert_size; int camera_view; + int conditional_speed; + int conditional_speed_lead; + int conditional_status; QPolygonF track_adjacent_vertices[6];