mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-01 21:53:42 +08:00
firestar5683
102 lines
5.3 KiB
Python
102 lines
5.3 KiB
Python
#!/usr/bin/env python3
|
|
from openpilot.common.filter_simple import FirstOrderFilter
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory, scale_threshold
|
|
|
|
class ConditionalExperimentalMode:
|
|
def __init__(self, FrogPilotPlanner):
|
|
self.frogpilot_planner = FrogPilotPlanner
|
|
|
|
self.curvature_filter = FirstOrderFilter(0, 1, DT_MDL)
|
|
self.slow_lead_filter = FirstOrderFilter(0, 1, DT_MDL)
|
|
self.stop_light_filter = FirstOrderFilter(0, 1, DT_MDL)
|
|
|
|
self.curve_detected = False
|
|
self.experimental_mode = False
|
|
self.stop_light_detected = False
|
|
|
|
def update(self, carState, frogpilotCarState, frogpilotNavigation, v_ego, frogpilot_toggles):
|
|
if frogpilot_toggles.experimental_mode_via_press:
|
|
self.status_value = params_memory.get_int("CEStatus")
|
|
else:
|
|
self.status_value = 0
|
|
|
|
if self.status_value not in {1, 2} and not carState.standstill:
|
|
self.update_conditions(frogpilotCarState, v_ego, frogpilot_toggles)
|
|
self.experimental_mode = self.check_conditions(carState, frogpilotNavigation, v_ego, frogpilot_toggles)
|
|
params_memory.put_int("CEStatus", self.status_value if self.experimental_mode else 0)
|
|
else:
|
|
self.experimental_mode = self.status_value == 2 or carState.standstill and self.experimental_mode and self.frogpilot_planner.model_stopped
|
|
self.stop_light_detected &= self.status_value not in {1, 2}
|
|
self.stop_light_filter.x = 0
|
|
|
|
def check_conditions(self, carState, frogpilotNavigation, v_ego, frogpilot_toggles):
|
|
below_speed = frogpilot_toggles.conditional_limit > v_ego >= 1 and not self.frogpilot_planner.frogpilot_following.following_lead
|
|
below_speed_with_lead = frogpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.frogpilot_planner.frogpilot_following.following_lead
|
|
if below_speed or below_speed_with_lead:
|
|
self.status_value = 3 if self.frogpilot_planner.frogpilot_following.following_lead else 4
|
|
return True
|
|
|
|
desired_lane = self.frogpilot_planner.lane_width_left if carState.leftBlinker else self.frogpilot_planner.lane_width_right
|
|
lane_available = desired_lane >= frogpilot_toggles.lane_detection_width or not frogpilot_toggles.conditional_signal_lane_detection
|
|
if v_ego < frogpilot_toggles.conditional_signal and (carState.leftBlinker or carState.rightBlinker) and not lane_available:
|
|
self.status_value = 5
|
|
return True
|
|
|
|
approaching_maneuver = frogpilotNavigation.approachingIntersection or frogpilotNavigation.approachingTurn
|
|
if frogpilot_toggles.conditional_navigation and approaching_maneuver and (frogpilot_toggles.conditional_navigation_lead or not self.frogpilot_planner.frogpilot_following.following_lead):
|
|
self.status_value = 6 if frogpilotNavigation.approachingIntersection else 7
|
|
return True
|
|
|
|
if frogpilot_toggles.conditional_curves and self.curve_detected and (frogpilot_toggles.conditional_curves_lead or not self.frogpilot_planner.frogpilot_following.following_lead):
|
|
self.status_value = 8
|
|
return True
|
|
|
|
if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 29.1:
|
|
self.status_value = 9 if self.frogpilot_planner.lead_one.vLead < 1 else 10
|
|
return True
|
|
|
|
if frogpilot_toggles.conditional_model_stop_time != 0 and self.stop_light_detected:
|
|
self.status_value = 11 if not self.frogpilot_planner.frogpilot_vcruise.forcing_stop else 12
|
|
return True
|
|
|
|
if self.frogpilot_planner.frogpilot_vcruise.slc.experimental_mode:
|
|
self.status_value = 13
|
|
return True
|
|
|
|
return False
|
|
|
|
def update_conditions(self, frogpilotCarState, v_ego, frogpilot_toggles):
|
|
self.curve_detection(v_ego, frogpilot_toggles)
|
|
self.slow_lead(frogpilot_toggles, v_ego)
|
|
self.stop_sign_and_light(frogpilotCarState, v_ego, frogpilot_toggles)
|
|
|
|
def curve_detection(self, v_ego, frogpilot_toggles):
|
|
curve_active = self.curve_detected and (0.9 / abs(self.frogpilot_planner.road_curvature))**0.5 < v_ego
|
|
|
|
self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or curve_active)
|
|
self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED
|
|
|
|
def slow_lead(self, frogpilot_toggles, v_ego):
|
|
v_lead = self.frogpilot_planner.lead_one.vLead
|
|
if self.frogpilot_planner.tracking_lead:
|
|
slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead
|
|
stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1
|
|
lead_threshold = scale_threshold(v_ego)
|
|
self.slow_lead_filter.update(slower_lead or stopped_lead)
|
|
self.slow_lead_detected = self.slow_lead_filter.x >= lead_threshold
|
|
else:
|
|
self.slow_lead_filter.x = 0
|
|
self.slow_lead_detected = False
|
|
|
|
def stop_sign_and_light(self, frogpilotCarState, v_ego, frogpilot_toggles):
|
|
if not (self.curve_detected or frogpilotCarState.trafficMode):
|
|
model_stopping = self.frogpilot_planner.model_length < v_ego * frogpilot_toggles.conditional_model_stop_time
|
|
|
|
self.stop_light_filter.update(self.frogpilot_planner.model_stopped or model_stopping)
|
|
self.stop_light_detected = self.stop_light_filter.x >= THRESHOLD**2 and not self.frogpilot_planner.tracking_lead
|
|
else:
|
|
self.stop_light_filter.x = 0
|
|
self.stop_light_detected = False
|