mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 22:53:44 +08:00
FrogAi
88 lines
4.7 KiB
Python
88 lines
4.7 KiB
Python
#!/usr/bin/env python3
|
|
from openpilot.common.numpy_fast import clip, interp
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
|
|
|
|
from openpilot.selfdrive.frogpilot.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
|
|
|
|
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
|
|
|
|
class FrogPilotFollowing:
|
|
def __init__(self, FrogPilotPlanner):
|
|
self.frogpilot_planner = FrogPilotPlanner
|
|
|
|
self.following_lead = False
|
|
self.slower_lead = False
|
|
|
|
self.acceleration_jerk = 0
|
|
self.base_acceleration_jerk = 0
|
|
self.base_speed_jerk = 0
|
|
self.danger_jerk = 0
|
|
self.desired_follow_distance = 0
|
|
self.speed_jerk = 0
|
|
self.t_follow = 0
|
|
|
|
def update(self, aEgo, controlsState, frogpilotCarState, lead_distance, v_ego, v_lead, frogpilot_toggles):
|
|
if frogpilotCarState.trafficModeActive:
|
|
if aEgo >= 0:
|
|
self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration)
|
|
self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed)
|
|
else:
|
|
self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_deceleration)
|
|
self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed_decrease)
|
|
|
|
self.base_danger_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger)
|
|
self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_follow)
|
|
else:
|
|
if aEgo >= 0:
|
|
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
|
|
)
|
|
else:
|
|
self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
|
|
frogpilot_toggles.aggressive_jerk_deceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed_decrease,
|
|
frogpilot_toggles.standard_jerk_deceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed_decrease,
|
|
frogpilot_toggles.relaxed_jerk_deceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed_decrease,
|
|
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
|
|
)
|
|
|
|
self.acceleration_jerk = self.base_acceleration_jerk
|
|
self.danger_jerk = self.base_danger_jerk
|
|
self.speed_jerk = self.base_speed_jerk
|
|
|
|
self.following_lead = self.frogpilot_planner.tracking_lead and lead_distance < (self.t_follow + 1) * v_ego
|
|
|
|
if self.frogpilot_planner.tracking_lead:
|
|
self.update_follow_values(lead_distance, v_ego, v_lead, frogpilot_toggles)
|
|
self.desired_follow_distance = int(desired_follow_distance(v_ego, v_lead, self.t_follow))
|
|
else:
|
|
self.desired_follow_distance = 0
|
|
|
|
def update_follow_values(self, lead_distance, v_ego, v_lead, frogpilot_toggles):
|
|
# Offset by FrogAi for FrogPilot for a more natural approach to a faster lead
|
|
if frogpilot_toggles.human_following and v_lead > v_ego:
|
|
distance_factor = max(lead_distance - (v_ego * self.t_follow), 1)
|
|
standstill_offset = max(STOP_DISTANCE - v_ego, 1)
|
|
acceleration_offset = clip((v_lead - v_ego) * standstill_offset - COMFORT_BRAKE, 1, distance_factor)
|
|
self.acceleration_jerk /= acceleration_offset
|
|
self.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.human_following) and v_lead < v_ego > CRUISING_SPEED:
|
|
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
|
|
far_lead_offset = max(v_lead - CITY_SPEED_LIMIT, 1)
|
|
braking_offset = clip(min(v_ego - v_lead, v_lead) * far_lead_offset - COMFORT_BRAKE, 1, distance_factor)
|
|
if frogpilot_toggles.human_following:
|
|
self.t_follow /= braking_offset
|
|
self.slower_lead = braking_offset / far_lead_offset > 1
|