James
56 lines
2.6 KiB
Python
56 lines
2.6 KiB
Python
#!/usr/bin/env python3
|
|
import numpy as np
|
|
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_jerk_factor, get_T_FOLLOW
|
|
|
|
class FrogPilotFollowing:
|
|
def __init__(self, FrogPilotPlanner):
|
|
self.frogpilot_planner = FrogPilotPlanner
|
|
|
|
self.following_lead = False
|
|
|
|
self.acceleration_jerk = 0
|
|
self.danger_jerk = 0
|
|
self.speed_jerk = 0
|
|
self.t_follow = 0
|
|
|
|
def update(self, long_control_active, v_ego, sm, frogpilot_toggles):
|
|
if long_control_active:
|
|
if sm["carState"].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, sm["selfdriveState"].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, sm["selfdriveState"].personality
|
|
)
|
|
|
|
self.t_follow = get_T_FOLLOW(
|
|
frogpilot_toggles.aggressive_follow,
|
|
frogpilot_toggles.standard_follow,
|
|
frogpilot_toggles.relaxed_follow,
|
|
frogpilot_toggles.custom_personalities, sm["selfdriveState"].personality
|
|
)
|
|
else:
|
|
self.base_acceleration_jerk = 0
|
|
self.base_danger_jerk = 0
|
|
self.base_speed_jerk = 0
|
|
self.t_follow = 0
|
|
|
|
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 self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
|
|
|
|
if long_control_active and self.frogpilot_planner.tracking_lead:
|
|
self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead)
|
|
|
|
def update_follow_values(self, lead_distance, v_ego, v_lead):
|