@ -1,7 +1,7 @@
#!/usr/bin/env python3
import os
import numpy as np
from cereal import log
from common . realtime import sec_since_boot
from common . numpy_fast import clip
from system . swaglog import cloudlog
@ -54,18 +54,38 @@ FCW_IDXS = T_IDXS < 5.0
T_DIFFS = np . diff ( T_IDXS , prepend = [ 0. ] )
MIN_ACCEL = - 3.5
MAX_ACCEL = 2.0
T_FOLLOW = 1.45
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
def get_jerk_factor ( personality = log . LongitudinalPersonality . standard ) :
if personality == log . LongitudinalPersonality . relaxed :
return 1.0
elif personality == log . LongitudinalPersonality . standard :
return 1.0
elif personality == log . LongitudinalPersonality . aggressive :
return 0.5
else :
raise NotImplementedError ( " Longitudinal personality not supported " )
def get_T_FOLLOW ( personality = log . LongitudinalPersonality . standard ) :
if personality == log . LongitudinalPersonality . relaxed :
return 1.75
elif personality == log . LongitudinalPersonality . standard :
return 1.45
elif personality == log . LongitudinalPersonality . aggressive :
return 1.25
else :
raise NotImplementedError ( " Longitudinal personality not supported " )
def get_stopped_equivalence_factor ( v_lead ) :
return ( v_lead * * 2 ) / ( 2 * COMFORT_BRAKE )
def get_safe_obstacle_distance ( v_ego , t_follow = T_FOLLOW ) :
def get_safe_obstacle_distance ( v_ego , t_follow ) :
return ( v_ego * * 2 ) / ( 2 * COMFORT_BRAKE ) + t_follow * v_ego + STOP_DISTANCE
def desired_follow_distance ( v_ego , v_lead ) :
return get_safe_obstacle_distance ( v_ego ) - get_stopped_equivalence_factor ( v_lead )
def desired_follow_distance ( v_ego , v_lead , t_follow = get_T_FOLLOW ( ) ) :
return get_safe_obstacle_distance ( v_ego , t_follow ) - get_stopped_equivalence_factor ( v_lead )
def gen_long_model ( ) :
@ -161,7 +181,8 @@ def gen_long_ocp():
x0 = np . zeros ( X_DIM )
ocp . constraints . x0 = x0
ocp . parameter_values = np . array ( [ - 1.2 , 1.2 , 0.0 , 0.0 , T_FOLLOW , LEAD_DANGER_FACTOR ] )
ocp . parameter_values = np . array ( [ - 1.2 , 1.2 , 0.0 , 0.0 , get_T_FOLLOW ( ) , LEAD_DANGER_FACTOR ] )
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np . zeros ( CONSTR_DIM )
@ -249,10 +270,11 @@ class LongitudinalMpc:
for i in range ( N ) :
self . solver . cost_set ( i , ' Zl ' , Zl )
def set_weights ( self , prev_accel_constraint = True ) :
def set_weights ( self , prev_accel_constraint = True , personality = log . LongitudinalPersonality . standard ) :
jerk_factor = get_jerk_factor ( personality )
if self . mode == ' acc ' :
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [ X_EGO_OBSTACLE_COST , X_EGO_COST , V_EGO_COST , A_EGO_COST , a_change_cost , J_EGO_COST ]
cost_weights = [ X_EGO_OBSTACLE_COST , X_EGO_COST , V_EGO_COST , A_EGO_COST , jerk_factor * a_change_cost , jerk_factor * J_EGO_COST ]
constraint_cost_weights = [ LIMIT_COST , LIMIT_COST , LIMIT_COST , DANGER_ZONE_COST ]
elif self . mode == ' blended ' :
a_change_cost = 40.0 if prev_accel_constraint else 0
@ -307,7 +329,8 @@ class LongitudinalMpc:
self . cruise_min_a = min_a
self . max_a = max_a
def update ( self , radarstate , v_cruise , x , v , a , j ) :
def update ( self , radarstate , v_cruise , x , v , a , j , personality = log . LongitudinalPersonality . standard ) :
t_follow = get_T_FOLLOW ( personality )
v_ego = self . x0 [ 1 ]
self . status = radarstate . leadOne . status or radarstate . leadTwo . status
@ -334,7 +357,7 @@ class LongitudinalMpc:
v_cruise_clipped = np . clip ( v_cruise * np . ones ( N + 1 ) ,
v_lower ,
v_upper )
cruise_obstacle = np . cumsum ( T_DIFFS * v_cruise_clipped ) + get_safe_obstacle_distance ( v_cruise_clipped )
cruise_obstacle = np . cumsum ( T_DIFFS * v_cruise_clipped ) + get_safe_obstacle_distance ( v_cruise_clipped , get_T_FOLLOW ( ) )
x_obstacles = np . column_stack ( [ lead_0_obstacle , lead_1_obstacle , cruise_obstacle ] )
self . source = SOURCES [ np . argmin ( x_obstacles [ 0 ] ) ]
@ -368,7 +391,7 @@ class LongitudinalMpc:
self . params [ : , 2 ] = np . min ( x_obstacles , axis = 1 )
self . params [ : , 3 ] = np . copy ( self . prev_a )
self . params [ : , 4 ] = T_FOLLOW
self . params [ : , 4 ] = t_follow
self . run ( )
if ( np . any ( lead_xv_0 [ FCW_IDXS , 0 ] - self . x_sol [ FCW_IDXS , 0 ] < CRASH_DISTANCE ) and
@ -380,9 +403,9 @@ class LongitudinalMpc:
# Check if it got within lead comfort range
# TODO This should be done cleaner
if self . mode == ' blended ' :
if any ( ( lead_0_obstacle - get_safe_obstacle_distance ( self . x_sol [ : , 1 ] , T_FOLLOW ) ) - self . x_sol [ : , 0 ] < 0.0 ) :
if any ( ( lead_0_obstacle - get_safe_obstacle_distance ( self . x_sol [ : , 1 ] , t_follow ) ) - self . x_sol [ : , 0 ] < 0.0 ) :
self . source = ' lead0 '
if any ( ( lead_1_obstacle - get_safe_obstacle_distance ( self . x_sol [ : , 1 ] , T_FOLLOW ) ) - self . x_sol [ : , 0 ] < 0.0 ) and \
if any ( ( lead_1_obstacle - get_safe_obstacle_distance ( self . x_sol [ : , 1 ] , t_follow ) ) - self . x_sol [ : , 0 ] < 0.0 ) and \
( lead_1_obstacle [ 0 ] - lead_0_obstacle [ 0 ] ) :
self . source = ' lead1 '