mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-07 17:26:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Controls - Conditional Experimental Mode - Stop Lights and Stop Signs

Browse Source 
Switch to 'Experimental Mode' when a stop light or stop sign is detected.
This commit is contained in:
FrogAi
2024-07-31 17:33:31 -07:00
parent 5fd58ecf21
commit b287e50982
2 changed files with 23 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/frogpilot/controls/frogpilot_planner.py
+5 -1
View File
@@ -13,7 +13,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import MODEL_LENGTH, PLANNER_TIME, ConditionalExperimentalMode
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator, calculate_lane_width, calculate_road_curvature
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY
@@ -25,11 +25,13 @@ class FrogPilotPlanner:
self.cem = ConditionalExperimentalMode(self)
self.model_stopped = False
self.slower_lead = False
self.tracking_lead = False
self.acceleration_jerk = 0
self.danger_jerk = 0
self.model_length = 0
self.road_curvature = 0
self.speed_jerk = 0
self.v_cruise = 0
@@ -59,6 +61,8 @@ class FrogPilotPlanner:
self.lane_width_left = 0
self.lane_width_right = 0
self.model_length = modelData.position.x[MODEL_LENGTH - 1]
self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1
self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles)
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+18
View File
@@ -1,8 +1,12 @@
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
MODEL_LENGTH = ModelConstants.IDX_N
PLANNER_TIME = ModelConstants.T_IDXS[MODEL_LENGTH - 1]
class ConditionalExperimentalMode:
def __init__(self, FrogPilotPlanner):
self.params_memory = Params("/dev/shm/params")
@@ -11,9 +15,11 @@ class ConditionalExperimentalMode:
self.curvature_mac = MovingAverageCalculator()
self.slow_lead_mac = MovingAverageCalculator()
self.stop_light_mac = MovingAverageCalculator()
self.curve_detected = False
self.experimental_mode = False
self.stop_light_detected = False
def update(self, carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles):
if not carState.standstill:
@@ -43,11 +49,16 @@ class ConditionalExperimentalMode:
self.status_value = 13 if v_lead < 1 else 14
return True
if frogpilot_toggles.conditional_stop_lights and self.stop_light_detected:
self.status_value = 15
return True
return False
def update_conditions(self, tracking_lead, v_ego, v_lead, frogpilot_toggles):
self.curve_detection(v_ego, frogpilot_toggles)
self.slow_lead(tracking_lead, v_lead, frogpilot_toggles)
self.stop_sign_and_light(tracking_lead, v_ego, frogpilot_toggles)
def curve_detection(self, v_ego, frogpilot_toggles):
curve_detected = (1 / self.frogpilot_planner.road_curvature)**0.5 < v_ego
@@ -66,3 +77,10 @@ class ConditionalExperimentalMode:
else:
self.slow_lead_mac.reset_data()
self.slow_lead_detected = False
def stop_sign_and_light(self, tracking_lead, v_ego, frogpilot_toggles):
model_projection = PLANNER_TIME - (5 if frogpilot_toggles.less_sensitive_lights else 3)
model_stopping = self.frogpilot_planner.model_length < v_ego * model_projection
self.stop_light_mac.add_data((self.frogpilot_planner.model_stopped or model_stopping) and not (self.curve_detected or tracking_lead))
self.stop_light_detected = self.stop_light_mac.get_moving_average() >= PROBABILITY