zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

remove CarrotLatControl

Browse Source
This commit is contained in:
ajouatom 2025-02-08 17:41:36 +09:00
parent 1bb8fcdb4f
commit bc40451c6b
10 changed files with 21 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
common/params.cc
View File

@ -272,7 +272,6 @@ std::unordered_map<std::string, uint32_t> keys = {
{"MyDrivingModeAuto", PERSISTENT},
{"TrafficLightDetectMode", PERSISTENT},
{"SteerActuatorDelay", PERSISTENT},
{"ModelActuatorDelay", PERSISTENT},
{"CruiseOnDist", PERSISTENT},
{"CruiseMaxVals1", PERSISTENT},
{"CruiseMaxVals2", PERSISTENT},
@ -310,7 +309,6 @@ std::unordered_map<std::string, uint32_t> keys = {
{"MaxAngleFrames", PERSISTENT},
{"SoftHoldMode", PERSISTENT},
{"CarrotLatControl", PERSISTENT},
{"CarrotLatFilter", PERSISTENT},
{"LatMpcPathCost", PERSISTENT },
{"LatMpcMotionCost", PERSISTENT },
{"LatMpcMotionCost2", PERSISTENT },

26
selfdrive/carrot_settings.json
View File

@ -66,19 +66,6 @@
"default": 1,
"unit": 1
},
{
"group": "조향튜닝",
"name": "CarrotLatFilter",
"title": "*당근조향제어필터(5)",
"descr": "필터링개수",
"egroup": "LAT",
"etitle": "*Carrot lateral filter(5)",
"edescr": "",
"min": 2,
"max": 30,
"default": 5,
"unit": 1
},
{
"group": "조향튜닝",
"name": "LatMpcPathCost",
@ -222,19 +209,6 @@
"default": 40,
"unit": 1
},
{
"group": "조향튜닝",
"name": "ModelActuatorDelay",
"title": "*ModelActuatorDelay(80)x0.01",
"descr": "모델지연값, 커브진입시점로 조절",
"egroup": "LAT",
"etitle": "*ModelActuatorDelay(80)",
"edescr": "",
"min": 1,
"max": 300,
"default": 80,
"unit": 1
},
{
"group": "크루즈",
"name": "CruiseOnDist",

37
selfdrive/controls/controlsd.py
View File

@ -11,7 +11,7 @@ from openpilot.common.swaglog import cloudlog
import numpy as np
from opendbc.car.car_helpers import get_car_interface
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lag_adjusted_curvature, get_lag_adjusted_curvature1
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lag_adjusted_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@ -126,39 +126,20 @@ class Controls:
self.lanefull_mode_enabled = (lat_plan.useLaneLines and self.params.get_int("UseLaneLineSpeedApply") > 0 and
curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
carrot_lat_control = self.params.get_int("CarrotLatControl")
if carrot_lat_control > 0:
model_delay = self.params.get_float("ModelActuatorDelay") * 0.01
steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
t_since_plan = (self.sm.frame - self.sm.recv_frame['lateralPlan']) * DT_CTRL
if carrot_lat_control == 1:
if len(lat_plan.curvatures) != CONTROL_N:
self.desired_curvature_next = self.desired_curvature = desired_curvature_ff = 0.0
else:
curvature = np.interp(model_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)
desired_curvature_ff = np.interp(model_delay + steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, curvature)
elif carrot_lat_control == 2:
desired_curvature = get_lag_adjusted_curvature1(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay)
desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
else:
lat_filter = carrot_lat_control
desired_curvature_now, desired_curvature_ff = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, self.desired_curvature, model_delay, steer_actuator_delay, t_since_plan, lat_filter)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature_now)
steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
if self.params.get_bool("CarrotLatControl"):
desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
else:
steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
if self.lanefull_mode_enabled:
desired_curvature = get_lag_adjusted_curvature1(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay)
desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature)
else:
desired_curvature_ff = self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
actuators.curvature = float(self.desired_curvature)
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, desired_curvature_ff, self.desired_curvature,
self.steer_limited, self.desired_curvature,
self.sm['liveLocationKalman']) # TODO what if not available
actuators.steer = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)

47
selfdrive/controls/lib/drive_helpers.py
View File

@ -23,52 +23,7 @@ def apply_deadzone(error, deadzone):
error = 0.
return error
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, desired_curvature_last, model_delay, steer_actuator_delay, t_since_plan, lat_filter):
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
v_ego = max(MIN_SPEED, v_ego)
delay = max(0.01, steer_actuator_delay)
current_curvature_desired = desired_curvature_last
window_size = lat_filter
if window_size > 0:
#def moving_average(data, window_size):
# kernel = np.ones(window_size) / window_size
# return np.convolve(data, kernel, mode='same')
def moving_average(data, window_size):
data = np.array(data, dtype=float)
kernel = np.ones(window_size) / window_size
smoothed = np.convolve(data, kernel, mode='same')
half_window = window_size // 2
smoothed[:half_window] = (
np.cumsum(data[:window_size])[:half_window] / np.arange(1, half_window + 1)
)
smoothed[-half_window:] = (
np.cumsum(data[-window_size:][::-1])[:half_window][::-1] / np.arange(1, half_window + 1)
)
return smoothed.tolist()
curvatures = moving_average(curvatures, window_size)
desired_curvature = np.interp(model_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
desired_curvature_ff = np.interp(model_delay + steer_actuator_delay + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
safe_desired_curvature = np.clip(desired_curvature,
current_curvature_desired - max_curvature_rate * DT_MDL,
current_curvature_desired + max_curvature_rate * DT_MDL)
#safe_desired_curvature_ff = clip(desired_curvature_ff,
# current_curvature_desired - max_curvature_rate * DT_MDL,
# current_curvature_desired + max_curvature_rate * DT_MDL)
return safe_desired_curvature, desired_curvature_ff
def get_lag_adjusted_curvature1(CP, v_ego, psis, curvatures, steer_actuator_delay):
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay):
if len(psis) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N

2
selfdrive/controls/lib/latcontrol.py
View File

@ -17,7 +17,7 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk):
def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
pass
def reset(self):

2
selfdrive/controls/lib/latcontrol_angle.py
View File

@ -11,7 +11,7 @@ class LatControlAngle(LatControl):
super().__init__(CP, CI)
self.sat_check_min_speed = 5.
def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk):
def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
if not active:

2
selfdrive/controls/lib/latcontrol_pid.py
View File

@ -17,7 +17,7 @@ class LatControlPID(LatControl):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk):
def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)

5
selfdrive/controls/lib/latcontrol_torque.py
View File

@ -52,7 +52,7 @@ class LatControlTorque(LatControl):
self.torque_params.latAccelOffset = latAccelOffset
self.torque_params.friction = friction
def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_now, llk):
def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk):
self.frame += 1
if self.frame % 10 == 0:
lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
@ -96,8 +96,7 @@ class LatControlTorque(LatControl):
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
desired_lateral_accel_now = desired_curvature_now * CS.vEgo ** 2
setpoint = desired_lateral_accel_now + low_speed_factor * desired_curvature_now
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,

36
selfdrive/controls/lib/lateral_planner.py
View File

@ -73,13 +73,6 @@ class LateralPlanner:
self.reset_mpc(np.zeros(4))
self.curve_speed = 0
self.prev_path_xyz = None
self.carrot_lat_control = 0
self.carrot_lat_filter = 5
self.path_history = deque(maxlen=self.carrot_lat_filter)
self.lateralMotionCost = LATERAL_MOTION_COST
self.lateralMotionCost2 = LATERAL_MOTION_COST
def reset_mpc(self, x0=None):
if x0 is None:
@ -100,11 +93,6 @@ class LateralPlanner:
LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01
LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01
STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost")
self.carrot_lat_control = self.params.get_int("CarrotLatControl")
carrot_lat_filter = self.params.get_int("CarrotLatFilter")
if carrot_lat_filter != self.carrot_lat_filter:
self.carrot_lat_filter = carrot_lat_filter
self.path_history = deque(maxlen=self.carrot_lat_filter)
# clip speed , lateral planning is not possible at 0 speed
measured_curvature = sm['controlsState'].curvature
@ -150,12 +138,15 @@ class LateralPlanner:
self.LP.lane_change_multiplier = 1.0
lateral_motion_cost = self.lateralMotionCost
path_cost = PATH_COST
atc_type = sm['carrotMan'].atcType
if atc_activate:
if atc_type == "turn left" and md.meta.desireState[1] > 0.1:
if atc_type == "turn left" and (md.orientationRate.z[-1] > 0.1 or md.meta.desireState[1] > 0.1):
lateral_motion_cost = self.lateralMotionCost2
elif atc_type == "turn right" and md.meta.desireState[2] > 0.1:
#path_cost *= 2
elif atc_type == "turn right" and (md.orientationRate.z[-1] < -0.1 or md.meta.desireState[2] > 0.1):
lateral_motion_cost = self.lateralMotionCost2
#path_cost *= 2
# lanelines calculation?
self.LP.lanefull_mode = self.useLaneLineMode
@ -170,22 +161,7 @@ class LateralPlanner:
self.path_xyz[:, 1] += self.pathOffset
"""
# Smooth path
self.alpha = 0.2
if self.prev_path_xyz is None:
self.prev_path_xyz = self.path_xyz.copy()
self.path_xyz = self.alpha * self.path_xyz + (1 - self.alpha) * self.prev_path_xyz
self.prev_path_xyz = self.path_xyz
"""
if self.carrot_lat_control in [1,2]:
if False: #self.plan_a[0] < -1.0:
self.path_history.clear()
self.path_history.append(self.path_xyz)
self.path_xyz = np.mean(np.array(self.path_history), axis=0)
self.lat_mpc.set_weights(PATH_COST, lateral_motion_cost,
self.lat_mpc.set_weights(path_cost, lateral_motion_cost,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)

2
system/manager/manager.py
View File

@ -124,7 +124,6 @@ def get_default_params():
("LaneChangeNeedTorque", "0"),
("MaxAngleFrames", "89"),
("CarrotLatControl", "0"),
("CarrotLatFilter", "5"),
("DampingFactor", "0"),
("LateralTorqueCustom", "0"),
("LateralTorqueAccelFactor", "2500"),
@ -141,7 +140,6 @@ def get_default_params():
("CustomSteerDeltaDown", "0"),
("SpeedFromPCM", "2"),
("SteerActuatorDelay", "30"),
("ModelActuatorDelay", "20"),
("MaxTimeOffroadMin", "60"),
("DisableDM", "0"),
("RecordRoadCam", "0"),