mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 06:33:50 +08:00
FrogAi
38df29bd11
Enjoy a bit of unpredictability with random events that can occur during certain driving conditions. This is purely cosmetic and has no impact on driving controls!
359 lines
18 KiB
Python
359 lines
18 KiB
Python
import cereal.messaging as messaging
|
|
|
|
from cereal import car
|
|
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.numpy_fast import clip, interp
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, COMFORT_BRAKE, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE, \
|
|
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 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
|
|
from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
|
|
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
|
|
|
GearShifter = car.CarState.GearShifter
|
|
|
|
A_CRUISE_MIN_ECO = A_CRUISE_MIN / 5
|
|
A_CRUISE_MIN_SPORT = A_CRUISE_MIN / 2
|
|
# MPH = [ 0., 11, 22, 34, 45, 56, 89]
|
|
A_CRUISE_MAX_BP_CUSTOM = [ 0., 5., 10., 15., 20., 25., 40.]
|
|
A_CRUISE_MAX_VALS_ECO = [1.4, 1.2, 1.0, 0.8, 0.6, 0.4, 0.2]
|
|
A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.0, 0.9, 0.8, 0.6]
|
|
A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 1.0, 0.9, 0.8, 0.6]
|
|
|
|
TARGET_LAT_A = 1.9
|
|
|
|
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
|
|
|
|
def get_max_accel_eco(v_ego):
|
|
return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
|
|
|
|
def get_max_accel_sport(v_ego):
|
|
return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)
|
|
|
|
def get_max_accel_sport_plus(v_ego):
|
|
return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)
|
|
|
|
class FrogPilotPlanner:
|
|
def __init__(self):
|
|
self.params_memory = Params("/dev/shm/params")
|
|
|
|
self.cem = ConditionalExperimentalMode(self)
|
|
self.lead_one = Lead()
|
|
self.mtsc = MapTurnSpeedController()
|
|
|
|
self.forcing_stop = False
|
|
self.lead_departing = False
|
|
self.model_stopped = False
|
|
self.override_force_stop = False
|
|
self.override_slc = False
|
|
self.slower_lead = False
|
|
self.taking_curve_quickly = False
|
|
self.tracking_lead = False
|
|
|
|
self.acceleration_jerk = 0
|
|
self.danger_jerk = 0
|
|
self.model_length = 0
|
|
self.mtsc_target = 0
|
|
self.overridden_speed = 0
|
|
self.road_curvature = 0
|
|
self.slc_target = 0
|
|
self.speed_jerk = 0
|
|
self.tracked_model_length = 0
|
|
self.tracking_lead_distance = 0
|
|
self.v_cruise = 0
|
|
self.vtsc_target = 0
|
|
|
|
self.tracking_lead_mac = MovingAverageCalculator()
|
|
|
|
def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles):
|
|
if frogpilot_toggles.radarless_model:
|
|
model_leads = list(modelData.leadsV3)
|
|
if len(model_leads) > 0:
|
|
model_lead = model_leads[0]
|
|
self.lead_one.update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob)
|
|
else:
|
|
self.lead_one.reset()
|
|
else:
|
|
self.lead_one = radarState.leadOne
|
|
|
|
v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS
|
|
v_ego = max(carState.vEgo, 0)
|
|
v_lead = self.lead_one.vLead
|
|
|
|
driving_gear = carState.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
|
|
|
|
distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) if not frogpilotCarState.trafficModeActive else 0
|
|
lead_distance = self.lead_one.dRel - distance_offset
|
|
stopping_distance = STOP_DISTANCE + distance_offset
|
|
|
|
run_cem = frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.force_stops or frogpilot_toggles.show_stopping_point
|
|
if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear:
|
|
self.cem.update(carState, frogpilotNavigation, modelData, v_ego, v_lead, frogpilot_toggles)
|
|
|
|
check_lane_width = frogpilot_toggles.adjacent_lanes or frogpilot_toggles.blind_spot_path or frogpilot_toggles.lane_detection
|
|
if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed:
|
|
self.lane_width_left = calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])
|
|
self.lane_width_right = calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])
|
|
else:
|
|
self.lane_width_left = 0
|
|
self.lane_width_right = 0
|
|
|
|
if frogpilot_toggles.lead_departing_alert and self.tracking_lead and driving_gear and carState.standstill:
|
|
if self.tracking_lead_distance == 0:
|
|
self.tracking_lead_distance = lead_distance
|
|
|
|
self.lead_departing = lead_distance - self.tracking_lead_distance > 1
|
|
self.lead_departing &= v_lead > 1
|
|
else:
|
|
self.lead_departing = False
|
|
self.tracking_lead_distance = 0
|
|
|
|
self.model_length = modelData.position.x[MODEL_LENGTH - 1]
|
|
self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
|
|
self.model_stopped |= self.forcing_stop
|
|
self.override_force_stop |= carState.gasPressed
|
|
self.override_force_stop |= frogpilot_toggles.force_stops and carState.standstill and self.tracking_lead
|
|
self.override_force_stop |= frogpilotCarControl.resumePressed
|
|
self.road_curvature = calculate_road_curvature(modelData, v_ego) if not carState.standstill and driving_gear else 1
|
|
|
|
if frogpilot_toggles.random_events and v_ego > CRUISING_SPEED and driving_gear:
|
|
self.taking_curve_quickly = v_ego > (1 / self.road_curvature)**0.5 * 2 > CRUISING_SPEED * 2 and abs(carState.steeringAngleDeg) > 30
|
|
|
|
self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles)
|
|
self.set_follow_values(controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles)
|
|
self.set_lead_status(lead_distance, stopping_distance, v_ego)
|
|
self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles)
|
|
|
|
def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles):
|
|
eco_gear = frogpilotCarState.ecoGear
|
|
sport_gear = frogpilotCarState.sportGear
|
|
|
|
if self.tracking_lead and frogpilot_toggles.aggressive_acceleration:
|
|
self.max_accel = clip(self.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), 2.0 if v_ego >= 20 else 4.0)
|
|
elif frogpilot_toggles.map_acceleration and (eco_gear or sport_gear):
|
|
if eco_gear:
|
|
self.max_accel = get_max_accel_eco(v_ego)
|
|
else:
|
|
if frogpilot_toggles.acceleration_profile == 3:
|
|
self.max_accel = get_max_accel_sport_plus(v_ego)
|
|
else:
|
|
self.max_accel = get_max_accel_sport(v_ego)
|
|
else:
|
|
if frogpilot_toggles.acceleration_profile == 1:
|
|
self.max_accel = get_max_accel_eco(v_ego)
|
|
elif frogpilot_toggles.acceleration_profile == 2:
|
|
self.max_accel = get_max_accel_sport(v_ego)
|
|
elif frogpilot_toggles.acceleration_profile == 3:
|
|
self.max_accel = get_max_accel_sport_plus(v_ego)
|
|
elif controlsState.experimentalMode:
|
|
self.max_accel = ACCEL_MAX
|
|
else:
|
|
self.max_accel = get_max_accel(v_ego)
|
|
|
|
if not self.tracking_lead:
|
|
self.max_accel = min(self.max_accel, self.max_accel * (self.v_cruise / CITY_SPEED_LIMIT))
|
|
|
|
if controlsState.experimentalMode:
|
|
self.min_accel = ACCEL_MIN
|
|
elif min(self.mtsc_target, self.vtsc_target) < v_cruise:
|
|
self.min_accel = A_CRUISE_MIN
|
|
elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear):
|
|
if eco_gear:
|
|
self.min_accel = A_CRUISE_MIN_ECO
|
|
else:
|
|
self.min_accel = A_CRUISE_MIN_SPORT
|
|
else:
|
|
if frogpilot_toggles.deceleration_profile == 1:
|
|
self.min_accel = A_CRUISE_MIN_ECO
|
|
elif frogpilot_toggles.deceleration_profile == 2:
|
|
self.min_accel = A_CRUISE_MIN_SPORT
|
|
else:
|
|
self.min_accel = A_CRUISE_MIN
|
|
|
|
def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
|
|
if frogpilotCarState.trafficModeActive:
|
|
self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration)
|
|
self.base_danger_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger)
|
|
self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed)
|
|
self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_t_follow)
|
|
else:
|
|
self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
|
|
frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed,
|
|
frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed,
|
|
frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed,
|
|
frogpilot_toggles.custom_personalities, controlsState.personality
|
|
)
|
|
|
|
self.t_follow = get_T_FOLLOW(
|
|
frogpilot_toggles.aggressive_follow,
|
|
frogpilot_toggles.standard_follow,
|
|
frogpilot_toggles.relaxed_follow,
|
|
frogpilot_toggles.custom_personalities, controlsState.personality
|
|
)
|
|
|
|
if self.tracking_lead:
|
|
self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles)
|
|
else:
|
|
self.acceleration_jerk = self.base_acceleration_jerk
|
|
self.danger_jerk = self.base_danger_jerk
|
|
self.speed_jerk = self.base_speed_jerk
|
|
|
|
def set_lead_status(self, lead_distance, stopping_distance, v_ego):
|
|
following_lead = self.lead_one.status and 1 < lead_distance < self.model_length + stopping_distance
|
|
following_lead &= v_ego > CRUISING_SPEED or self.tracking_lead
|
|
|
|
self.tracking_lead_mac.add_data(following_lead)
|
|
self.tracking_lead = self.tracking_lead_mac.get_moving_average() >= PROBABILITY
|
|
|
|
def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
|
|
# Offset by FrogAi for FrogPilot for a more natural approach to a faster lead
|
|
if frogpilot_toggles.aggressive_acceleration and v_lead > v_ego:
|
|
distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
|
|
standstill_offset = max(stopping_distance - v_ego, 0) * max(v_lead - v_ego, 0)
|
|
acceleration_offset = clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor)
|
|
self.acceleration_jerk = self.base_acceleration_jerk / acceleration_offset
|
|
self.danger_jerk = self.base_danger_jerk / acceleration_offset
|
|
self.speed_jerk = self.base_speed_jerk / acceleration_offset
|
|
self.t_follow /= acceleration_offset
|
|
|
|
# Offset by FrogAi for FrogPilot for a more natural approach to a slower lead
|
|
if (frogpilot_toggles.conditional_slower_lead or frogpilot_toggles.smoother_braking) and v_lead < v_ego:
|
|
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
|
|
far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - stopping_distance + (v_lead - CITY_SPEED_LIMIT), 0)
|
|
braking_offset = clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor)
|
|
if frogpilot_toggles.smoother_braking:
|
|
self.acceleration_jerk = self.base_acceleration_jerk * min(braking_offset, COMFORT_BRAKE / 2)
|
|
self.speed_jerk = self.base_speed_jerk * min(braking_offset, COMFORT_BRAKE * 2)
|
|
self.t_follow /= braking_offset
|
|
self.slower_lead = braking_offset - far_lead_offset > 1
|
|
|
|
def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles):
|
|
v_cruise_cluster = max(controlsState.vCruiseCluster, v_cruise) * CV.KPH_TO_MS
|
|
v_cruise_diff = v_cruise_cluster - v_cruise
|
|
|
|
v_ego_cluster = max(carState.vEgoCluster, v_ego)
|
|
v_ego_diff = v_ego_cluster - v_ego
|
|
|
|
# Pfeiferj's Map Turn Speed Controller
|
|
if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and controlsState.enabled:
|
|
mtsc_active = self.mtsc_target < v_cruise
|
|
self.mtsc_target = clip(self.mtsc.target_speed(v_ego, carState.aEgo), CRUISING_SPEED, v_cruise)
|
|
|
|
if frogpilot_toggles.mtsc_curvature_check and self.road_curvature < 1.0 and not mtsc_active:
|
|
self.mtsc_target = v_cruise
|
|
if self.mtsc_target == CRUISING_SPEED:
|
|
self.mtsc_target = v_cruise
|
|
else:
|
|
self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
|
|
|
|
# Pfeiferj's Speed Limit Controller
|
|
if frogpilot_toggles.speed_limit_controller:
|
|
SpeedLimitController.update(frogpilotCarState.dashboardSpeedLimit, controlsState.enabled, frogpilotNavigation.navigationSpeedLimit, v_cruise, v_ego, frogpilot_toggles)
|
|
unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
|
|
|
|
if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0:
|
|
if self.params_memory.get_bool("SLCConfirmed"):
|
|
self.slc_target = unconfirmed_slc_target
|
|
self.params_memory.put_bool("SLCConfirmed", False)
|
|
else:
|
|
self.slc_target = unconfirmed_slc_target
|
|
|
|
self.override_slc = self.overridden_speed > self.slc_target
|
|
self.override_slc |= carState.gasPressed and v_ego > self.slc_target
|
|
self.override_slc &= controlsState.enabled
|
|
|
|
if self.override_slc:
|
|
if frogpilot_toggles.speed_limit_controller_override_manual:
|
|
if carState.gasPressed:
|
|
self.overridden_speed = v_ego + v_ego_diff
|
|
self.overridden_speed = clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
|
|
elif frogpilot_toggles.speed_limit_controller_override_set_speed:
|
|
self.overridden_speed = v_cruise + v_cruise_diff
|
|
else:
|
|
self.overridden_speed = 0
|
|
else:
|
|
self.slc_target = 0
|
|
|
|
# Pfeiferj's Vision Turn Controller
|
|
if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and controlsState.enabled:
|
|
adjusted_road_curvature = self.road_curvature * frogpilot_toggles.curve_sensitivity
|
|
adjusted_target_lat_a = TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness
|
|
|
|
self.vtsc_target = (adjusted_target_lat_a / adjusted_road_curvature)**0.5
|
|
self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise)
|
|
else:
|
|
self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
|
|
|
|
if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled:
|
|
self.forcing_stop = True
|
|
self.v_cruise = -1
|
|
|
|
elif frogpilot_toggles.force_stops and self.cem.stop_light_detected and not self.override_force_stop and controlsState.enabled:
|
|
if self.tracked_model_length == 0:
|
|
self.tracked_model_length = self.model_length
|
|
|
|
self.forcing_stop = True
|
|
self.tracked_model_length -= v_ego * DT_MDL
|
|
self.v_cruise = min((self.tracked_model_length / PLANNER_TIME) - 1, v_cruise)
|
|
|
|
else:
|
|
if not self.cem.stop_light_detected:
|
|
self.override_force_stop = False
|
|
|
|
self.forcing_stop = False
|
|
self.tracked_model_length = 0
|
|
|
|
targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target]
|
|
self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
|
|
|
|
def publish(self, sm, pm, frogpilot_toggles):
|
|
frogpilot_plan_send = messaging.new_message('frogpilotPlan')
|
|
frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
|
frogpilotPlan = frogpilot_plan_send.frogpilotPlan
|
|
|
|
frogpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.acceleration_jerk)
|
|
frogpilotPlan.accelerationJerkStock = float(A_CHANGE_COST * self.base_acceleration_jerk)
|
|
frogpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.danger_jerk)
|
|
frogpilotPlan.speedJerk = float(J_EGO_COST * self.speed_jerk)
|
|
frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk)
|
|
frogpilotPlan.tFollow = float(self.t_follow)
|
|
|
|
frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH))
|
|
frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target)
|
|
|
|
frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode
|
|
|
|
frogpilotPlan.forcingStop = self.forcing_stop
|
|
|
|
frogpilotPlan.greenLight = not self.model_stopped
|
|
frogpilotPlan.redLight = self.cem.stop_light_detected
|
|
|
|
frogpilotPlan.laneWidthLeft = self.lane_width_left
|
|
frogpilotPlan.laneWidthRight = self.lane_width_right
|
|
|
|
frogpilotPlan.leadDeparting = self.lead_departing
|
|
|
|
frogpilotPlan.maxAcceleration = float(self.max_accel)
|
|
frogpilotPlan.minAcceleration = float(self.min_accel)
|
|
|
|
frogpilotPlan.slcOverridden = bool(self.override_slc)
|
|
frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
|
|
frogpilotPlan.slcSpeedLimit = self.slc_target
|
|
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
|
|
frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
|
|
|
|
frogpilotPlan.takingCurveQuickly = self.taking_curve_quickly
|
|
|
|
frogpilotPlan.vCruise = self.v_cruise
|
|
|
|
pm.send('frogpilotPlan', frogpilot_plan_send)
|