You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
64 lines
2.4 KiB
64 lines
2.4 KiB
import numpy as np
|
|
from cereal import log
|
|
from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
|
from openpilot.common.realtime import DT_CTRL, DT_MDL
|
|
|
|
MIN_SPEED = 1.0
|
|
CONTROL_N = 17
|
|
CAR_ROTATION_RADIUS = 0.0
|
|
# This is a turn radius smaller than most cars can achieve
|
|
MAX_CURVATURE = 0.2
|
|
MAX_VEL_ERR = 5.0 # m/s
|
|
|
|
# EU guidelines
|
|
MAX_LATERAL_JERK = 5.0 # m/s^3
|
|
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
|
|
|
|
|
|
def clamp(val, min_val, max_val):
|
|
clamped_val = float(np.clip(val, min_val, max_val))
|
|
return clamped_val, clamped_val != val
|
|
|
|
def smooth_value(val, prev_val, tau):
|
|
alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1
|
|
return alpha * val + (1 - alpha) * prev_val
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll):
|
|
# This function respects ISO lateral jerk and acceleration limits + a max curvature
|
|
v_ego = max(v_ego, MIN_SPEED)
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
|
|
new_curvature = np.clip(new_curvature,
|
|
prev_curvature - max_curvature_rate * DT_CTRL,
|
|
prev_curvature + max_curvature_rate * DT_CTRL)
|
|
|
|
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
|
max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
|
min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
|
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
|
|
|
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
|
return float(new_curvature), limited_accel or limited_max_curv
|
|
|
|
|
|
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
|
|
# ToDo: Try relative error, and absolute speed
|
|
if len(modelV2.temporalPose.trans):
|
|
vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
|
|
return float(vel_err)
|
|
return 0.0
|
|
|
|
|
|
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
|
|
if len(speeds) == len(t_idxs):
|
|
v_now = speeds[0]
|
|
a_now = accels[0]
|
|
v_target = np.interp(action_t, t_idxs, speeds)
|
|
a_target = 2 * (v_target - v_now) / (action_t) - a_now
|
|
v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
|
|
else:
|
|
v_target = 0.0
|
|
v_target_1sec = 0.0
|
|
a_target = 0.0
|
|
should_stop = (v_target < vEgoStopping and
|
|
v_target_1sec < vEgoStopping)
|
|
return a_target, should_stop
|
|
|