dbe15596a1
Enable the ability to activate 'Traffic Mode' by holding down the 'distance' button for 2.5 seconds. When 'Traffic Mode' is active the onroad UI will turn red and openpilot will drive catered towards stop and go traffic.
167 lines
7.4 KiB
Python
Executable File
167 lines
7.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import math
|
|
import numpy as np
|
|
from openpilot.common.numpy_fast import clip, interp
|
|
|
|
import cereal.messaging as messaging
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from openpilot.common.filter_simple import FirstOrderFilter
|
|
from openpilot.common.realtime import DT_MDL
|
|
from openpilot.selfdrive.modeld.constants import ModelConstants
|
|
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
|
|
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
|
|
from openpilot.common.swaglog import cloudlog
|
|
|
|
LON_MPC_STEP = 0.2 # first step is 0.2s
|
|
A_CRUISE_MIN = -1.2
|
|
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
|
|
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
|
|
|
|
# Lookup table for turns
|
|
_A_TOTAL_MAX_V = [1.7, 3.2]
|
|
_A_TOTAL_MAX_BP = [20., 40.]
|
|
|
|
|
|
def get_max_accel(v_ego):
|
|
return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
|
|
|
|
|
|
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
|
|
"""
|
|
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
|
this should avoid accelerating when losing the target in turns
|
|
"""
|
|
|
|
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
|
|
# The lookup table for turns should also be updated if we do this
|
|
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
|
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
|
|
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
|
|
|
|
return [a_target[0], min(a_target[1], a_x_allowed)]
|
|
|
|
|
|
class LongitudinalPlanner:
|
|
def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
|
|
self.CP = CP
|
|
self.mpc = LongitudinalMpc()
|
|
self.fcw = False
|
|
self.dt = dt
|
|
|
|
self.a_desired = init_a
|
|
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
|
|
self.v_model_error = 0.0
|
|
|
|
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
|
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
|
self.j_desired_trajectory = np.zeros(CONTROL_N)
|
|
self.solverExecutionTime = 0.0
|
|
|
|
@staticmethod
|
|
def parse_model(model_msg, model_error, v_ego, taco_tune):
|
|
if (len(model_msg.position.x) == 33 and
|
|
len(model_msg.velocity.x) == 33 and
|
|
len(model_msg.acceleration.x) == 33):
|
|
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
|
|
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
|
|
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
|
|
j = np.zeros(len(T_IDXS_MPC))
|
|
else:
|
|
x = np.zeros(len(T_IDXS_MPC))
|
|
v = np.zeros(len(T_IDXS_MPC))
|
|
a = np.zeros(len(T_IDXS_MPC))
|
|
j = np.zeros(len(T_IDXS_MPC))
|
|
|
|
if taco_tune:
|
|
max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0])
|
|
curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0)
|
|
max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0
|
|
v = np.minimum(max_v, v)
|
|
|
|
return x, v, a, j
|
|
|
|
def update(self, sm, frogpilot_toggles):
|
|
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
|
|
|
|
v_ego = sm['carState'].vEgo
|
|
v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
|
|
v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
|
|
|
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
|
|
force_slow_decel = sm['controlsState'].forceDecel
|
|
|
|
# Reset current state when not engaged, or user is controlling the speed
|
|
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled
|
|
|
|
# No change cost when user is controlling the speed, or when standstill
|
|
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
|
|
|
accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
|
|
if self.mpc.mode == 'acc':
|
|
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
|
|
else:
|
|
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
|
|
|
|
if reset_state:
|
|
self.v_desired_filter.x = v_ego
|
|
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
|
|
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
|
|
|
|
# Prevent divergence, smooth in current v_ego
|
|
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
|
# Compute model v_ego error
|
|
self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
|
|
|
|
if force_slow_decel:
|
|
v_cruise = 0.0
|
|
# clip limits, cannot init MPC outside of bounds
|
|
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
|
|
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
|
|
|
|
self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality)
|
|
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
|
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
|
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune)
|
|
self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
|
|
sm['frogpilotCarControl'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality)
|
|
|
|
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
|
|
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
|
|
self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
|
|
self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
|
|
self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
|
|
|
|
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
|
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
|
if self.fcw:
|
|
cloudlog.info("FCW triggered")
|
|
|
|
# Interpolate 0.05 seconds and save as starting point for next iteration
|
|
a_prev = self.a_desired
|
|
self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
|
|
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
|
|
|
|
def publish(self, sm, pm):
|
|
plan_send = messaging.new_message('longitudinalPlan')
|
|
|
|
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
|
|
|
longitudinalPlan = plan_send.longitudinalPlan
|
|
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
|
|
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
|
|
|
|
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
|
|
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
|
|
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
|
|
|
|
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
|
|
longitudinalPlan.longitudinalPlanSource = self.mpc.source
|
|
longitudinalPlan.fcw = self.fcw
|
|
|
|
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
|
|
|
|
pm.send('longitudinalPlan', plan_send)
|