Conditional Experimental Mode
Added toggles for "Conditional Experimental Mode". Conditions based on road curvature, turn signals, speed, lead speed, navigation instructions, and stop signs/stop lights are all individually toggleable. Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
parent
66fb36d641
commit
7725dafdcc
|
|
@ -16,9 +16,12 @@ struct FrogPilotDeviceState @0xaedffd8f31e7b55d {
|
|||
}
|
||||
|
||||
struct FrogPilotNavigation @0xf35cc4560bbf6ec2 {
|
||||
approachingIntersection @0 :Bool;
|
||||
approachingTurn @1 :Bool;
|
||||
}
|
||||
|
||||
struct FrogPilotPlan @0xda96579883444c35 {
|
||||
conditionalExperimental @1 :Bool;
|
||||
laneWidthLeft @3 :Float32;
|
||||
laneWidthRight @4 :Float32;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -218,6 +218,20 @@ std::unordered_map<std::string, uint32_t> keys = {
|
|||
{"ApiCache_DriveStats", PERSISTENT},
|
||||
{"BlindSpotPath", PERSISTENT},
|
||||
{"CameraView", 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},
|
||||
{"CustomUI", PERSISTENT},
|
||||
{"DecelerationProfile", PERSISTENT},
|
||||
|
|
|
|||
|
|
@ -558,5 +558,6 @@ 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_functions.py
|
||||
selfdrive/frogpilot/functions/frogpilot_planner.py
|
||||
|
|
|
|||
|
|
@ -690,6 +690,10 @@ class Controls:
|
|||
# FrogPilot functions
|
||||
frogpilot_plan = self.sm['frogpilotPlan']
|
||||
|
||||
# Update Experimental Mode
|
||||
if self.frogpilot_variables.conditional_experimental_mode:
|
||||
self.experimental_mode = frogpilot_plan.conditionalExperimental
|
||||
|
||||
# Gear Check
|
||||
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
|
||||
|
||||
|
|
@ -984,7 +988,11 @@ 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:
|
||||
if not self.frogpilot_variables.conditional_experimental_mode:
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
else:
|
||||
self.experimental_mode = False
|
||||
if self.CP.notCar:
|
||||
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
|
||||
time.sleep(0.1)
|
||||
|
|
@ -1006,6 +1014,8 @@ class Controls:
|
|||
t.join()
|
||||
|
||||
def update_frogpilot_params(self):
|
||||
self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
|
||||
|
||||
custom_alerts = self.params.get_bool("CustomAlerts")
|
||||
|
||||
lateral_tune = self.params.get_bool("LateralTune")
|
||||
|
|
|
|||
|
|
@ -133,7 +133,10 @@ class VCruiseHelper:
|
|||
if self.CP.pcmCruise:
|
||||
return
|
||||
|
||||
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
|
||||
if frogpilot_variables.conditional_experimental_mode:
|
||||
initial = V_CRUISE_INITIAL
|
||||
else:
|
||||
initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if 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:
|
||||
|
|
|
|||
|
|
@ -337,6 +337,7 @@ class LongitudinalMpc:
|
|||
|
||||
def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, 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
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
After Width: | Height: | Size: 13 KiB |
|
|
@ -0,0 +1,193 @@
|
|||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import interp
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, MovingAverageCalculator
|
||||
|
||||
CITY_SPEED_LIMIT = 25 # Speed limit for turn signal check
|
||||
PROBABILITY = 0.6 # 60% chance of condition being true
|
||||
|
||||
# Lookup table for stop sign / stop light detection
|
||||
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, params_memory):
|
||||
self.params_memory = params_memory
|
||||
|
||||
self.curve_detected = False
|
||||
self.experimental_mode = False
|
||||
self.lead_detected = False
|
||||
self.lead_stopping = 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()
|
||||
|
||||
def update(self, carState, enabled, frogpilotNavigation, modelData, radarState, road_curvature, stop_distance, t_follow, v_ego):
|
||||
lead = radarState.leadOne
|
||||
v_lead = lead.vLead
|
||||
|
||||
# Update Experimental Mode based on the current driving conditions
|
||||
condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, stop_distance, v_ego)
|
||||
if (not self.experimental_mode and condition_met) and enabled:
|
||||
self.experimental_mode = True
|
||||
elif (self.experimental_mode and not condition_met) or not enabled:
|
||||
self.experimental_mode = False
|
||||
self.status_value = 0
|
||||
|
||||
# Update the onroad status bar
|
||||
if self.status_value != self.previous_status_value:
|
||||
self.params_memory.put_int("CEStatus", self.status_value)
|
||||
self.previous_status_value = self.status_value
|
||||
|
||||
self.update_conditions(lead, modelData, radarState, road_curvature, t_follow, v_ego, v_lead)
|
||||
|
||||
# Check conditions for the appropriate state of Experimental Mode
|
||||
def check_conditions(self, carState, frogpilotNavigation, lead, modelData, stop_distance, v_ego):
|
||||
if carState.standstill:
|
||||
self.status_value = 0
|
||||
return self.experimental_mode
|
||||
|
||||
# Keep Experimental Mode active if stopping for a red light
|
||||
if self.status_value == 13 and self.slowing_down(v_ego) and lead.dRel > stop_distance:
|
||||
return True
|
||||
|
||||
# Navigation check
|
||||
if self.navigation and (frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn) and (self.navigation_lead or not self.lead_detected):
|
||||
self.status_value = 5 if frogpilotNavigation.approachingIntersection else 6
|
||||
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 = 8 if self.lead_detected else 9
|
||||
return True
|
||||
|
||||
# Slower lead check
|
||||
if self.slower_lead and self.slower_lead_detected:
|
||||
self.status_value = 10
|
||||
return True
|
||||
|
||||
# Turn signal check
|
||||
if self.signal and v_ego <= CITY_SPEED_LIMIT and (carState.leftBlinker or carState.rightBlinker):
|
||||
self.status_value = 11
|
||||
return True
|
||||
|
||||
# Road curvature check
|
||||
if self.curves and self.curve_detected:
|
||||
self.status_value = 12
|
||||
return True
|
||||
|
||||
# Stop sign and light check
|
||||
if self.stop_lights and self.red_light_detected:
|
||||
self.status_value = 13
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def update_conditions(self, lead, modelData, radarState, road_curvature, t_follow, v_ego, v_lead):
|
||||
self.lead_detection(lead)
|
||||
self.lead_slowing_down(lead, t_follow, v_lead)
|
||||
self.road_curvature(road_curvature)
|
||||
self.slow_lead(lead, t_follow, v_ego, v_lead)
|
||||
self.stop_sign_and_light(modelData, v_ego)
|
||||
|
||||
# Lead detection
|
||||
def lead_detection(self, lead):
|
||||
lead_status = 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, t_follow, v_lead):
|
||||
if self.lead_detected:
|
||||
lead_close = lead.dRel <= v_lead * t_follow
|
||||
lead_slowing_down = v_lead < self.previous_v_lead
|
||||
lead_stopped = v_lead <= 0
|
||||
|
||||
self.previous_v_lead = v_lead
|
||||
|
||||
self.lead_slowing_down_mac.add_data(lead_close or lead_slowing_down or lead_stopped)
|
||||
self.lead_stopping = self.lead_slowing_down_mac.get_moving_average() >= PROBABILITY
|
||||
else:
|
||||
self.lead_slowing_down_mac.reset_data()
|
||||
self.lead_stopping = False
|
||||
self.previous_v_lead = 0
|
||||
|
||||
# Determine if we're slowing down for a potential stop
|
||||
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
|
||||
|
||||
# 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 5.0 helps prevent it triggering for red lights
|
||||
curve_detected = 5.0 >= road_curvature > 1.6
|
||||
curve_active = 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
|
||||
|
||||
# Slower lead detection - Credit goes to the DragonPilot team!
|
||||
def slow_lead(self, lead, t_follow, v_ego, v_lead):
|
||||
if self.lead_detected:
|
||||
slower_lead_ahead = lead.dRel < (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
|
||||
|
||||
# Stop sign/stop light detection - Credit goes to the DragonPilot team!
|
||||
def stop_sign_and_light(self, modelData, v_ego):
|
||||
lead_check = self.stop_lights_lead or not self.lead_stopping
|
||||
|
||||
# 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)
|
||||
|
||||
# Filter out any other reasons the model may want to slow down
|
||||
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, params):
|
||||
self.curves = params.get_bool("CECurves")
|
||||
self.curves_lead = self.curves and 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.navigation_lead = self.navigation and params.get_bool("CENavigationLead")
|
||||
|
||||
self.signal = params.get_bool("CESignal")
|
||||
|
||||
self.slower_lead = params.get_bool("CESlowerLead")
|
||||
|
||||
self.stop_lights = params.get_bool("CEStopLights")
|
||||
self.stop_lights_lead = self.stop_lights and params.get_bool("CEStopLightsLead")
|
||||
|
|
@ -22,6 +22,28 @@ A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30, .091]
|
|||
A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60]
|
||||
A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2]
|
||||
|
||||
|
||||
class MovingAverageCalculator:
|
||||
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 FrogPilotFunctions:
|
||||
def __init__(self) -> None:
|
||||
self.params = Params()
|
||||
|
|
@ -55,3 +77,9 @@ class FrogPilotFunctions:
|
|||
distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp))
|
||||
|
||||
return min(distance_to_lane, distance_to_road_edge)
|
||||
|
||||
@staticmethod
|
||||
def road_curvature(modelData, v_ego):
|
||||
predicted_velocities = np.array(modelData.velocity.x)
|
||||
curvature_ratios = np.abs(np.array(modelData.acceleration.y)) / (predicted_velocities**2)
|
||||
return np.amax(curvature_ratios * (v_ego**2))
|
||||
|
|
|
|||
|
|
@ -3,10 +3,13 @@ import cereal.messaging as messaging
|
|||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
|
||||
|
||||
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
|
||||
|
||||
class FrogPilotPlanner:
|
||||
def __init__(self, CP, params, params_memory):
|
||||
self.CP = CP
|
||||
|
|
@ -14,6 +17,10 @@ class FrogPilotPlanner:
|
|||
|
||||
self.fpf = FrogPilotFunctions()
|
||||
|
||||
self.cem = ConditionalExperimentalMode(self.params_memory)
|
||||
|
||||
self.road_curvature = 0
|
||||
self.stop_distance = 0
|
||||
self.v_cruise = 0
|
||||
|
||||
self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)]
|
||||
|
|
@ -45,6 +52,10 @@ class FrogPilotPlanner:
|
|||
|
||||
self.accel_limits = [min_accel, max_accel]
|
||||
|
||||
# Update Conditional Experimental Mode
|
||||
if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl:
|
||||
self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego)
|
||||
|
||||
# Update the current lane widths
|
||||
check_lane_width = self.blind_spot_path
|
||||
if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN:
|
||||
|
|
@ -54,6 +65,12 @@ class FrogPilotPlanner:
|
|||
self.lane_width_left = 0
|
||||
self.lane_width_right = 0
|
||||
|
||||
# Update the current road curvature
|
||||
self.road_curvature = self.fpf.road_curvature(modelData, v_ego)
|
||||
|
||||
# Update the desired stopping distance
|
||||
self.stop_distance = STOP_DISTANCE
|
||||
|
||||
# Update the max allowed speed
|
||||
self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego)
|
||||
|
||||
|
|
@ -75,6 +92,8 @@ 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.laneWidthLeft = self.lane_width_left
|
||||
frogpilotPlan.laneWidthRight = self.lane_width_right
|
||||
|
||||
|
|
@ -83,6 +102,11 @@ 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)
|
||||
params.put_bool("ExperimentalMode", True)
|
||||
|
||||
custom_ui = params.get_bool("CustomUI")
|
||||
self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,13 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
|
|||
const std::vector<std::tuple<QString, QString, QString, QString>> 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"},
|
||||
|
||||
{"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
|
||||
|
|
@ -34,6 +41,44 @@ 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), this, false, " kph");
|
||||
conditionalSpeedsMetric = new FrogPilotDualParamControl(CESpeedMetric, CESpeedLeadMetric, this);
|
||||
addItem(conditionalSpeedsMetric);
|
||||
|
||||
std::vector<QString> curveToggles{"CECurvesLead"};
|
||||
std::vector<QString> curveToggleNames{tr("With Lead")};
|
||||
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, curveToggles, curveToggleNames);
|
||||
} else if (param == "CENavigation") {
|
||||
std::vector<QString> navigationToggles{"CENavigationIntersections", "CENavigationTurns", "CENavigationLead"};
|
||||
std::vector<QString> navigationToggleNames{tr("Intersections"), tr("Turns"), tr("With Lead")};
|
||||
toggle = new FrogPilotParamToggleControl(param, title, desc, icon, navigationToggles, navigationToggleNames);
|
||||
} else if (param == "CEStopLights") {
|
||||
std::vector<QString> stopLightToggles{"CEStopLightsLead"};
|
||||
std::vector<QString> 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]() {
|
||||
|
|
@ -147,6 +192,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.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion));
|
||||
params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
|
||||
}
|
||||
|
||||
if (isMetric) {
|
||||
|
|
@ -157,6 +204,9 @@ void FrogPilotControlsPanel::updateMetric() {
|
|||
}
|
||||
|
||||
void FrogPilotControlsPanel::parentToggleClicked() {
|
||||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
|
||||
openParentToggle();
|
||||
}
|
||||
|
||||
|
|
@ -165,6 +215,9 @@ void FrogPilotControlsPanel::subParentToggleClicked() {
|
|||
}
|
||||
|
||||
void FrogPilotControlsPanel::hideSubToggles() {
|
||||
conditionalSpeedsImperial->setVisible(false);
|
||||
conditionalSpeedsMetric->setVisible(false);
|
||||
|
||||
for (auto &[key, toggle] : toggles) {
|
||||
bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() ||
|
||||
fireTheBabysitterKeys.find(key.c_str()) != fireTheBabysitterKeys.end() ||
|
||||
|
|
|
|||
|
|
@ -29,7 +29,10 @@ private:
|
|||
void updateState(const UIState &s);
|
||||
void updateToggles();
|
||||
|
||||
std::set<QString> conditionalExperimentalKeys = {};
|
||||
FrogPilotDualParamControl *conditionalSpeedsImperial;
|
||||
FrogPilotDualParamControl *conditionalSpeedsMetric;
|
||||
|
||||
std::set<QString> conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"};
|
||||
std::set<QString> fireTheBabysitterKeys = {};
|
||||
std::set<QString> laneChangeKeys = {};
|
||||
std::set<QString> lateralTuneKeys = {};
|
||||
|
|
|
|||
|
|
@ -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,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_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 +326,34 @@ 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])
|
||||
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 +405,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)
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -175,7 +175,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;
|
||||
|
|
@ -563,7 +563,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;
|
||||
|
|
@ -762,9 +762,14 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
|
|||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -796,9 +801,28 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
|
|||
p.setOpacity(1.0);
|
||||
p.drawRoundedRect(statusBarRect, 30, 30);
|
||||
|
||||
std::map<int, QString> 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" + (mapOpen ? " intersection" : QString(" upcoming intersection"))},
|
||||
{6, "Experimental Mode activated for" + (mapOpen ? " turn" : QString(" upcoming turn"))},
|
||||
{7, "Experimental Mode activated due to" + (mapOpen ? "SLC" : QString(" no speed limit set"))},
|
||||
{8, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeedLead) + (is_metric ? " kph" : " mph"))},
|
||||
{9, "Experimental Mode activated due to" + (mapOpen ? " speed" : " speed being less than " + QString::number(conditionalSpeed) + (is_metric ? " kph" : " mph"))},
|
||||
{10, "Experimental Mode activated for slower lead"},
|
||||
{11, "Experimental Mode activated for turn" + (mapOpen ? "" : QString(" / lane change"))},
|
||||
{12, "Experimental Mode activated for curve"},
|
||||
{13, "Experimental Mode activated for stop" + (mapOpen ? "" : QString(" sign / stop light"))},
|
||||
};
|
||||
|
||||
// Update status text
|
||||
if (alwaysOnLateralActive) {
|
||||
newStatus = QString("Always On Lateral active. Press the \"Cruise Control\" button to disable");
|
||||
} else if (conditionalExperimental) {
|
||||
newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
|
||||
}
|
||||
|
||||
// Check if status has changed
|
||||
|
|
|
|||
|
|
@ -122,9 +122,13 @@ private:
|
|||
bool alwaysOnLateralActive;
|
||||
bool blindSpotLeft;
|
||||
bool blindSpotRight;
|
||||
bool conditionalExperimental;
|
||||
bool experimentalMode;
|
||||
|
||||
int cameraView;
|
||||
int conditionalSpeed;
|
||||
int conditionalSpeedLead;
|
||||
int conditionalStatus;
|
||||
|
||||
protected:
|
||||
void paintGL() override;
|
||||
|
|
|
|||
|
|
@ -262,6 +262,10 @@ void ui_update_frogpilot_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");
|
||||
|
||||
bool custom_onroad_ui = params.getBool("CustomUI");
|
||||
scene.acceleration_path = custom_onroad_ui && params.getBool("AccelerationPath");
|
||||
scene.blind_spot_path = custom_onroad_ui && params.getBool("BlindSpotPath");
|
||||
|
|
@ -337,6 +341,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) {
|
||||
|
|
|
|||
|
|
@ -175,6 +175,7 @@ typedef struct UIScene {
|
|||
bool blind_spot_left;
|
||||
bool blind_spot_path;
|
||||
bool blind_spot_right;
|
||||
bool conditional_experimental;
|
||||
bool enabled;
|
||||
bool experimental_mode;
|
||||
|
||||
|
|
@ -182,6 +183,9 @@ typedef struct UIScene {
|
|||
float lane_width_right;
|
||||
|
||||
int camera_view;
|
||||
int conditional_speed;
|
||||
int conditional_speed_lead;
|
||||
int conditional_status;
|
||||
|
||||
QPolygonF track_adjacent_vertices[6];
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue