diff --git a/cereal/custom.capnp b/cereal/custom.capnp index a861756..da99881 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -22,10 +22,12 @@ struct FrogPilotLateralPlan @0xda96579883444c35 { } struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 { + conditionalExperimental @1 :Bool; distances @3 :List(Float32); } struct FrogPilotNavigation @0xa5cd762cd951a455 { + navigationConditionMet @0 :Bool; } struct CustomReserved6 @0xf98d843bfd7004a3 { diff --git a/common/params.cc b/common/params.cc index 14d2694..c98a24c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -219,6 +219,17 @@ std::unordered_map keys = { {"ApiCache_DriveStats", PERSISTENT}, {"AverageCurvature", PERSISTENT}, {"CameraView", PERSISTENT}, + {"CECurves", PERSISTENT}, + {"CECurvesLead", PERSISTENT}, + {"CENavigation", PERSISTENT}, + {"CESignal", PERSISTENT}, + {"CESlowerLead", PERSISTENT}, + {"CESpeed", PERSISTENT}, + {"CESpeedLead", PERSISTENT}, + {"CEStatus", PERSISTENT}, + {"CEStopLights", PERSISTENT}, + {"CEStopLightsLead", PERSISTENT}, + {"ConditionalExperimental", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"LateralTune", PERSISTENT}, diff --git a/release/files_common b/release/files_common index e6f12a3..bae3be1 100644 --- a/release/files_common +++ b/release/files_common @@ -557,4 +557,5 @@ tinygrad_repo/tinygrad/runtime/ops_gpu.py tinygrad_repo/tinygrad/shape/* tinygrad_repo/tinygrad/*.py +selfdrive/frogpilot/functions/conditional_experimental_mode.py selfdrive/frogpilot/functions/frogpilot_planner.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b2c7e4b..17d659b 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -571,7 +571,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.conditional_experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -602,6 +602,10 @@ class Controls: CC = car.CarControl.new_message() CC.enabled = self.enabled + # Update Experimental Mode + if self.conditional_experimental_mode: + self.experimental_mode = self.sm['frogpilotLongitudinalPlan'].conditionalExperimental + # Gear Check gear = car.CarState.GearShifter driving_gear = CS.gearShifter not in (gear.neutral, gear.park, gear.reverse, gear.unknown) @@ -918,7 +922,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.conditional_experimental_mode: + self.experimental_mode = self.params.get_bool("ExperimentalMode") if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) @@ -946,6 +951,7 @@ class Controls: obj.update_frogpilot_params(self.params) self.average_desired_curvature = self.params.get_bool("AverageCurvature") + self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental") longitudinal_tune = self.params.get_bool("LongitudinalTune") self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f8cfcb4..202f34f 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -128,12 +128,12 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, conditional_experimental_mode) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and not conditional_experimental_mode else V_CRUISE_INITIAL # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py old mode 100755 new mode 100644 index ab44406..25e15dd --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -333,6 +333,7 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) + self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status 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 0000000..9834f86 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_conditional.png differ diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py new file mode 100644 index 0000000..e37f7a0 --- /dev/null +++ b/selfdrive/frogpilot/functions/conditional_experimental_mode.py @@ -0,0 +1,197 @@ +import numpy as np +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +# Constants +PROBABILITY = 0.6 # 60% chance of condition being true +THRESHOLD = 5 # Time threshold (0.25s) + +SPEED_LIMIT = 25 # Speed limit for turn signal check + +# Lookup table for stop sign / stop light detection +SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55.] +SLOW_DOWN_DISTANCE = [10, 30., 50., 70., 80., 90., 120.] +TRAJECTORY_SIZE = 33 + + +class GenericMovingAverageCalculator: + def __init__(self): + self.data = [] + self.total = 0 + + def add_data(self, value): + if len(self.data) == THRESHOLD: + self.total -= self.data.pop(0) + self.data.append(value) + self.total += value + + def get_moving_average(self): + if len(self.data) == 0: + return None + return self.total / len(self.data) + + def reset_data(self): + self.data = [] + self.total = 0 + + +class ConditionalExperimentalMode: + def __init__(self): + self.params_memory = Params("/dev/shm/params") + + self.curve_detected = False + self.experimental_mode = False + self.lead_detected = False + self.lead_detected_previously = False + self.lead_slowing_down = False + self.red_light_detected = False + self.slower_lead_detected = False + self.slowing_down = False + + self.previous_status_value = 0 + self.previous_v_ego = 0 + self.previous_v_lead = 0 + self.status_value = 0 + + self.curvature_gmac = GenericMovingAverageCalculator() + self.lead_detection_gmac = GenericMovingAverageCalculator() + self.lead_slowing_down_gmac = GenericMovingAverageCalculator() + self.slow_down_gmac = GenericMovingAverageCalculator() + self.slow_lead_gmac = GenericMovingAverageCalculator() + self.slowing_down_gmac = GenericMovingAverageCalculator() + + def update(self, carState, frogpilotNavigation, modelData, mpc, radarState, standstill, v_ego): + # Update Experimental Mode based on the current driving conditions + condition_met = self.check_conditions(carState, frogpilotNavigation, modelData, standstill, v_ego) + if (not self.experimental_mode and condition_met): + self.experimental_mode = True + elif (self.experimental_mode and not condition_met): + self.experimental_mode = False + self.status_value = 0 + + # Update the onroad status bar + if self.status_value != self.previous_status_value: + self.previous_status_value = self.status_value + self.params_memory.put_int("CEStatus", self.status_value) + + self.update_conditions(modelData, mpc, radarState, v_ego) + + # Check conditions for the appropriate state of Experimental Mode + def check_conditions(self, carState, frogpilotNavigation, modelData, standstill, v_ego): + if standstill: + return self.experimental_mode + + # Keep Experimental Mode active if slowing down for a red light + if self.slowing_down and self.status_value == 12 and not self.lead_slowing_down: + return True + + # Navigation check + if self.navigation and modelData.navEnabled and frogpilotNavigation.navigationConditionMet: + self.status_value = 5 + return True + + # Speed check + if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead): + self.status_value = 7 if self.lead_detected else 8 + return True + + # Slower lead check + if self.slower_lead and self.slower_lead_detected: + self.status_value = 9 + return True + + # Turn signal check + if self.signal and v_ego < SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker): + self.status_value = 10 + return True + + # Road curvature check + if self.curves and self.curve_detected and (self.curves_lead or not self.lead_detected): + self.status_value = 11 + return True + + # Stop sign and light check + if self.stop_lights and self.red_light_detected and (self.stop_lights_lead or not self.lead_slowing_down): + self.status_value = 12 + return True + + return False + + def update_conditions(self, modelData, mpc, radarState, v_ego): + self.lead_detection(radarState) + self.road_curvature(modelData, v_ego) + self.slow_lead(mpc, radarState, v_ego) + self.stop_sign_and_light(modelData, v_ego) + self.v_ego_slowing_down(v_ego) + self.v_lead_slowing_down(mpc, radarState) + + def v_ego_slowing_down(self, v_ego): + self.slowing_down_gmac.add_data(v_ego < self.previous_v_ego) + self.slowing_down = self.slowing_down_gmac.get_moving_average() >= PROBABILITY + self.previous_v_ego = v_ego + + def v_lead_slowing_down(self, mpc, radarState): + if self.lead_detected: + v_lead = radarState.leadOne.vLead + self.lead_slowing_down_gmac.add_data(v_lead < self.previous_v_lead or v_lead < 1 or radarState.leadOne.dRel <= v_lead * mpc.t_follow) + self.lead_slowing_down = self.lead_slowing_down_gmac.get_moving_average() >= PROBABILITY + self.previous_v_lead = v_lead + else: + self.lead_slowing_down_gmac.reset_data() + self.lead_slowing_down = False + self.previous_v_lead = 0 + + # Lead detection + def lead_detection(self, radarState): + self.lead_detection_gmac.add_data(radarState.leadOne.status) + self.lead_detected = self.lead_detection_gmac.get_moving_average() >= PROBABILITY + + # Determine the road curvature - Credit goes to to Pfeiferj! + def road_curvature(self, modelData, v_ego): + predicted_velocities = np.array(modelData.velocity.x) + if predicted_velocities.size: + curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2) + curvature = np.amax(curvature_ratios * (v_ego**2)) + # Setting an upper limit of "5.0" helps prevent it activating at stop lights + if curvature > 5.0: + self.curvature_gmac.reset_data() + self.curve_detected = False + else: + self.curvature_gmac.add_data(curvature > 1.6 or self.curve_detected and curvature > 1.1) + self.curve_detected = self.curvature_gmac.get_moving_average() >= PROBABILITY + else: + self.curvature_gmac.reset_data() + + # Slower lead detection - Credit goes to the DragonPilot team! + def slow_lead(self, mpc, radarState, v_ego): + if not self.lead_detected and self.lead_detected_previously: + self.slow_lead_gmac.reset_data() + self.slower_lead_detected = False + + if self.lead_detected and v_ego >= 0.01: + self.slow_lead_gmac.add_data(radarState.leadOne.dRel < (v_ego - 1) * mpc.t_follow) + self.slower_lead_detected = self.slow_lead_gmac.get_moving_average() >= PROBABILITY + + self.lead_detected_previously = self.lead_detected + + # Stop sign/stop light detection - Credit goes to the DragonPilot team! + def stop_sign_and_light(self, modelData, v_ego): + # Check if the model data is consistent and wants to stop + 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) + self.slow_down_gmac.add_data(model_check and model_stopping and not self.curve_detected and not self.slower_lead_detected) + + self.red_light_detected = self.slow_down_gmac.get_moving_average() >= PROBABILITY + + def update_frogpilot_params(self, is_metric, params): + self.curves = params.get_bool("CECurves") + self.curves_lead = params.get_bool("CECurvesLead") + self.limit = params.get_int("CESpeed") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + self.limit_lead = params.get_int("CESpeedLead") * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) + self.navigation = params.get_bool("CENavigation") + self.signal = params.get_bool("CESignal") + self.slower_lead = params.get_bool("CESlowerLead") + self.stop_lights = params.get_bool("CEStopLights") + self.stop_lights_lead = params.get_bool("CEStopLightsLead") diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index a6f5a7d..d7f77eb 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -8,6 +8,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode + # Acceleration profiles - Credit goes to the DragonPilot team! # MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123] A_CRUISE_MIN_BP_CUSTOM = [0., 2.0, 2.01, 11., 11.01, 18., 18.01, 28., 28.01, 33., 55.] @@ -35,6 +37,8 @@ def get_max_accel_sport_tune(v_ego): class FrogPilotPlanner: def __init__(self, params): + self.cem = ConditionalExperimentalMode() + self.v_cruise = 0 self.x_desired_trajectory = np.zeros(CONTROL_N) @@ -58,6 +62,10 @@ class FrogPilotPlanner: else: self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + # Conditional Experimental Mode + if self.conditional_experimental_mode and enabled: + self.cem.update(carState, sm['frogpilotNavigation'], sm['modelV2'], mpc, sm['radarState'], carState.standstill, v_ego) + self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego) self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution) @@ -79,6 +87,7 @@ class FrogPilotPlanner: frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan + frogpilotLongitudinalPlan.conditionalExperimental = self.cem.experimental_mode frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist() pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send) @@ -86,6 +95,12 @@ class FrogPilotPlanner: def update_frogpilot_params(self, params): self.is_metric = params.get_bool("IsMetric") + self.conditional_experimental_mode = params.get_bool("ConditionalExperimental") + if self.conditional_experimental_mode: + self.cem.update_frogpilot_params(self.is_metric, params) + if not params.get_bool("ExperimentalMode"): + params.put_bool("ExperimentalMode", True) + lateral_tune = params.get_bool("LateralTune") self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index f9ae4c5..740e319 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -5,6 +5,13 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil const std::vector> controlToggles { {"AlwaysOnLateral", "Always on Lateral", "Maintain openpilot lateral control when the brake or gas pedals are used.\n\nDeactivation occurs only through the 'Cruise Control' button.", "../frogpilot/assets/toggle_icons/icon_always_on_lateral.png"}, + {"ConditionalExperimental", "Conditional Experimental Mode", "Automatically switches to 'Experimental Mode' under predefined conditions.", "../frogpilot/assets/toggle_icons/icon_conditional.png"}, + {"CECurves", "Curve Detected Ahead", "Switch to 'Experimental Mode' when a curve is detected.", ""}, + {"CENavigation", "Navigation Based", "Switch to 'Experimental Mode' based on navigation data. (i.e. Intersections, stop signs, etc.)", ""}, + {"CESlowerLead", "Slower Lead Detected Ahead", "Switch to 'Experimental Mode' when a slower lead vehicle is detected ahead.", ""}, + {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, + {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""}, @@ -31,6 +38,40 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } }); + } else if (param == "ConditionalExperimental") { + FrogPilotParamManageControl *conditionalExperimentalToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(conditionalExperimentalToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + 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", "Below", "Switch to 'Experimental Mode' below this speed in absence of a lead vehicle.", "", 0, 99, + std::map(), this, false, " mph"); + FrogPilotParamValueControl *CESpeedLeadImperial = new FrogPilotParamValueControl("CESpeedLead", " w/Lead", "Switch to 'Experimental Mode' below this speed when following a lead vehicle.", "", 0, 99, + std::map(), this, false, " mph"); + conditionalSpeedsImperial = new FrogPilotDualParamControl(CESpeedImperial, CESpeedLeadImperial, this); + addItem(conditionalSpeedsImperial); + + FrogPilotParamValueControl *CESpeedMetric = new FrogPilotParamValueControl("CESpeed", "Below", "Switch to 'Experimental Mode' below this speed in absence of a lead vehicle.", "", 0, 150, + std::map(), this, false, " kph"); + FrogPilotParamValueControl *CESpeedLeadMetric = new FrogPilotParamValueControl("CESpeedLead", " w/Lead", "Switch to 'Experimental Mode' below this speed when following a lead vehicle.", "", + 0, 150, std::map(), this, false, " kph"); + conditionalSpeedsMetric = new FrogPilotDualParamControl(CESpeedMetric, CESpeedLeadMetric, this); + addItem(conditionalSpeedsMetric); + + std::vector curveToggles{tr("CECurvesLead")}; + std::vector curveToggleNames{tr("With Lead")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, curveToggles, curveToggleNames); + } else if (param == "CEStopLights") { + std::vector stopLightToggles{tr("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]() { @@ -95,7 +136,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil }); } - conditionalExperimentalKeys = {}; + conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"}; fireTheBabysitterKeys = {}; laneChangeKeys = {}; lateralTuneKeys = {"AverageCurvature"}; @@ -126,6 +167,8 @@ void FrogPilotControlsPanel::updateMetric() { if (isMetric != previousIsMetric) { double distanceConversion = isMetric ? FOOT_TO_METER : METER_TO_FOOT; double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; + params.putInt("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); + params.putInt("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); } if (isMetric) { @@ -136,12 +179,18 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::parentToggleClicked() { + conditionalSpeedsImperial->setVisible(false); + conditionalSpeedsMetric->setVisible(false); + this->openParentToggle(); } void FrogPilotControlsPanel::hideSubToggles() { + conditionalSpeedsImperial->setVisible(false); + conditionalSpeedsMetric->setVisible(false); + for (auto &[key, toggle] : toggles) { - const bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || + bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || fireTheBabysitterKeys.find(key.c_str()) != fireTheBabysitterKeys.end() || laneChangeKeys.find(key.c_str()) != laneChangeKeys.end() || lateralTuneKeys.find(key.c_str()) != lateralTuneKeys.end() || diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 202c7f3..629772a 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -22,6 +22,9 @@ private: void updateMetric(); void updateToggles(); + FrogPilotDualParamControl *conditionalSpeedsImperial; + FrogPilotDualParamControl *conditionalSpeedsMetric; + std::set conditionalExperimentalKeys; std::set fireTheBabysitterKeys; std::set laneChangeKeys; diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 863e251..7688580 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,11 @@ class RouteEngine: self.mapbox_host = "https://maps.comma.ai" # FrogPilot variables + self.stop_coord = [] + self.stop_signal = [] + self.nav_condition = False + self.noo_condition = False + self.update_frogpilot_params() def update(self): @@ -169,6 +175,16 @@ 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: + 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 +325,36 @@ class RouteEngine: self.params.remove("NavDestination") self.clear_route() + # 5-10 Seconds to stop condition based on v_ego 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]) + if distance_to_condition < max((seconds_to_stop * v_ego), 25): + self.nav_condition = True + else: + self.nav_condition = False # Not approaching any stopSign or trafficLight + else: + self.nav_condition = False # No more stopSign or trafficLight in array + + # Determine if NoO distance to maneuver is upcoming + if distance_to_maneuver_along_geometry < max((seconds_to_stop * v_ego), 25): + self.noo_condition = True + else: + self.noo_condition = False # Not approaching any NoO maneuver + frogpilot_plan_send = messaging.new_message('frogpilotNavigation') frogpilotNavigation = frogpilot_plan_send.frogpilotNavigation + frogpilotNavigation.navigationConditionMet = self.conditional_navigation and (self.nav_condition or self.noo_condition) + self.pm.send('frogpilotNavigation', frogpilot_plan_send) def send_route(self): @@ -363,10 +406,11 @@ class RouteEngine: # TODO: Check for going wrong way in segment def update_frogpilot_params(self): + self.conditional_navigation = self.params.get_bool("CENavigation") 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 79b81ca..810de07 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -172,7 +172,7 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); if (hasLongitudinalControl(CP)) { // normal description and toggle - experimental_mode_toggle->setEnabled(true); + experimental_mode_toggle->setEnabled(!params.getBool("ConditionalExperimental")); 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 e678b47..75d26ea 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -178,7 +178,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; - int offset = scene.always_on_lateral ? 25 : 0; + int offset = scene.always_on_lateral || scene.conditional_experimental ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; @@ -548,7 +548,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) // base icon int offset = UI_BORDER_SIZE + btn_size / 2; - offset += alwaysOnLateral ? 25 : 0; + offset += alwaysOnLateral || conditionalExperimental ? 25 : 0; int x = rightHandDM ? width() - offset : offset; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; @@ -655,7 +655,7 @@ void AnnotatedCameraWidget::paintGL() { // for replay of old routes, never go to widecam wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; } - CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER : + CameraWidget::setStreamType(cameraView == 3 ? VISION_STREAM_DRIVER : wide_cam_requested && cameraView != 1 ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD; @@ -742,9 +742,13 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { alwaysOnLateral = scene.always_on_lateral_active; cameraView = scene.camera_view; + conditionalExperimental = scene.conditional_experimental; + conditionalSpeed = scene.conditional_speed; + conditionalSpeedLead = scene.conditional_speed_lead; + conditionalStatus = scene.conditional_status; experimentalMode = scene.experimental_mode; - if (alwaysOnLateral) { + if (alwaysOnLateral || conditionalExperimental) { drawStatusBar(p); } @@ -775,8 +779,26 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.setOpacity(1.0); p.drawRoundedRect(statusBarRect, 30, 30); + QMap conditionalStatusMap = { + {0, "Conditional Experimental Mode ready"}, + {1, "Conditional Experimental overridden"}, + {2, "Experimental Mode manually activated"}, + {3, "Conditional Experimental overridden"}, + {4, "Experimental Mode manually activated"}, + {5, "Experimental Mode activated for navigation" + (QString(" instructions input"))}, + {6, "Experimental Mode activated due to" + (QString(" no speed limit set"))}, + {7, "Experimental Mode activated due to" + (" speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))}, + {8, "Experimental Mode activated due to" + (" speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))}, + {9, "Experimental Mode activated for slower lead"}, + {10, "Experimental Mode activated for turn" + (QString(" / lane change"))}, + {11, "Experimental Mode activated for curve"}, + {12, "Experimental Mode activated for stop" + (QString(" sign / stop light"))} + }; + if (alwaysOnLateral) { newStatus = QString("Always On Lateral active") + (". Press the \"Cruise Control\" button to disable"); + } else if (conditionalExperimental) { + newStatus = conditionalStatusMap.contains(conditionalStatus) && status != STATUS_DISENGAGED ? conditionalStatusMap[conditionalStatus] : conditionalStatusMap[0]; } // Check if status has changed diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index dd3bf71..2124f36 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -118,8 +118,12 @@ private: QHBoxLayout *bottom_layout; bool alwaysOnLateral; + bool conditionalExperimental; bool experimentalMode; int cameraView; + int conditionalSpeed; + int conditionalSpeedLead; + int conditionalStatus; protected: void paintGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1f8205d..38b679f 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -250,6 +250,10 @@ void ui_update_params(UIState *s) { scene.always_on_lateral = params.getBool("AlwaysOnLateral"); scene.camera_view = params.getInt("CameraView"); + + scene.conditional_experimental = params.getBool("ConditionalExperimental"); + scene.conditional_speed = params.getInt("CESpeed"); + scene.conditional_speed_lead = params.getInt("CESpeedLead"); } void UIState::updateStatus() { @@ -318,6 +322,9 @@ void UIState::update() { } // FrogPilot live variables that need to be constantly checked + if (scene.conditional_experimental) { + scene.conditional_status = paramsMemory.getInt("CEStatus"); + } } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 13893f7..43f7a8d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -171,9 +171,13 @@ typedef struct UIScene { // FrogPilot variables bool always_on_lateral; bool always_on_lateral_active; + bool conditional_experimental; bool enabled; bool experimental_mode; int camera_view; + int conditional_speed; + int conditional_speed_lead; + int conditional_status; } UIScene;