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

Controls - Conditional Experimental Mode - Stop Lights and Stop Signs 

Switch to 'Experimental Mode' when a stop light or stop sign is detected.
This commit is contained in:
FrogAi 2024-06-16 18:42:11 -07:00
parent 9545e93bd3
commit a1f808f056
1 changed files with 39 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
View File

@ -1,4 +1,5 @@
from openpilot.common.params import Params
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY
@ -9,13 +10,21 @@ class ConditionalExperimentalMode:
self.curve_detected = False
self.experimental_mode = False
self.lead_stopped = False
self.red_light_detected = False
self.previous_v_lead = 0
self.curvature_mac = MovingAverageCalculator()
self.lead_detection_mac = MovingAverageCalculator()
self.lead_slowing_down_mac = MovingAverageCalculator()
self.slow_lead_mac = MovingAverageCalculator()
self.stop_light_mac = MovingAverageCalculator()
def update(self, carState, enabled, frogpilotNavigation, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
lead_distance = lead.dRel
self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles)
condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles) and enabled
self.experimental_mode = condition_met
@ -44,17 +53,37 @@ class ConditionalExperimentalMode:
self.status_value = 15
return True
if frogpilot_toggles.conditional_stop_lights and self.red_light_detected:
self.status_value = 16
return True
return False
def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles):
def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
self.lead_detection(lead_status)
self.road_curvature(road_curvature, v_ego, frogpilot_toggles)
self.slow_lead(slower_lead, v_lead)
self.stop_sign_and_light(lead_distance, modelData, v_ego, v_lead, frogpilot_toggles)
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 self.red_light_detected)
lead_slowing_down = v_lead < self.previous_v_lead
self.previous_v_lead = v_lead
self.lead_slowing_down_mac.add_data((lead_close or lead_slowing_down or self.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
def road_curvature(self, road_curvature, v_ego, frogpilot_toggles):
if frogpilot_toggles.conditional_curves_lead or not self.lead_detected:
curve_detected = (1 / road_curvature)**0.5 < v_ego
@ -75,3 +104,11 @@ class ConditionalExperimentalMode:
self.slow_lead_mac.reset_data()
self.lead_stopped = False
self.slower_lead_detected = False
def stop_sign_and_light(self, lead_distance, modelData, v_ego, v_lead, frogpilot_toggles):
lead_check = frogpilot_toggles.conditional_stop_lights_lead or not self.lead_slowing_down(lead_distance, v_ego, v_lead)
model_stopped = modelData.position.x[ModelConstants.IDX_N - 1] < ModelConstants.IDX_N
model_stopping = modelData.position.x[ModelConstants.IDX_N - 1] < v_ego * ModelConstants.T_IDXS[ModelConstants.IDX_N - 2]
self.stop_light_mac.add_data((lead_check and model_stopping) or model_stopped)
self.red_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY