controls: add curvature based steer control type
parent
8af20af66d
commit
e3e0c93398
|
@ -34,7 +34,7 @@ class TestCarInterfaces(unittest.TestCase):
|
||||||
self.assertGreater(car_params.mass, 1)
|
self.assertGreater(car_params.mass, 1)
|
||||||
self.assertGreater(car_params.steerRateCost, 1e-3)
|
self.assertGreater(car_params.steerRateCost, 1e-3)
|
||||||
|
|
||||||
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
|
if car_params.steerControlType not in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvature):
|
||||||
tuning = car_params.lateralTuning.which()
|
tuning = car_params.lateralTuning.which()
|
||||||
if tuning == 'pid':
|
if tuning == 'pid':
|
||||||
self.assertTrue(len(car_params.lateralTuning.pid.kpV))
|
self.assertTrue(len(car_params.lateralTuning.pid.kpV))
|
||||||
|
|
|
@ -21,6 +21,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||||
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
||||||
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||||
|
from selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
|
||||||
from selfdrive.controls.lib.events import Events, ET
|
from selfdrive.controls.lib.events import Events, ET
|
||||||
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
||||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||||
|
@ -129,6 +130,8 @@ class Controls:
|
||||||
|
|
||||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||||
self.LaC = LatControlAngle(self.CP, self.CI)
|
self.LaC = LatControlAngle(self.CP, self.CI)
|
||||||
|
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||||
|
self.LaC = LatControlCurvature(self.CP, self.CI)
|
||||||
elif self.CP.lateralTuning.which() == 'pid':
|
elif self.CP.lateralTuning.which() == 'pid':
|
||||||
self.LaC = LatControlPID(self.CP, self.CI)
|
self.LaC = LatControlPID(self.CP, self.CI)
|
||||||
elif self.CP.lateralTuning.which() == 'indi':
|
elif self.CP.lateralTuning.which() == 'indi':
|
||||||
|
@ -516,9 +519,11 @@ class Controls:
|
||||||
lat_plan.psis,
|
lat_plan.psis,
|
||||||
lat_plan.curvatures,
|
lat_plan.curvatures,
|
||||||
lat_plan.curvatureRates)
|
lat_plan.curvatureRates)
|
||||||
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
|
actuators.steer, actuators.steeringAngleDeg, \
|
||||||
params, self.last_actuators, desired_curvature,
|
actuators.curvature, actuators.curvatureRate, \
|
||||||
desired_curvature_rate)
|
actuators.pathAngle, actuators.pathDeviation, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
|
||||||
|
params, self.last_actuators, lat_plan,
|
||||||
|
desired_curvature, desired_curvature_rate)
|
||||||
else:
|
else:
|
||||||
lac_log = log.ControlsState.LateralDebugState.new_message()
|
lac_log = log.ControlsState.LateralDebugState.new_message()
|
||||||
if self.sm.rcv_frame['testJoystick'] > 0:
|
if self.sm.rcv_frame['testJoystick'] > 0:
|
||||||
|
@ -528,7 +533,9 @@ class Controls:
|
||||||
if CC.latActive:
|
if CC.latActive:
|
||||||
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
|
steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
|
||||||
# max angle is 45 for angle-based cars
|
# max angle is 45 for angle-based cars
|
||||||
actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
|
steer_angle = steer * 45.
|
||||||
|
curvature = self.VM.calc_curvature(math.radians(steer_angle), CS.vEgo, params.roll)
|
||||||
|
actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer_angle, curvature
|
||||||
|
|
||||||
lac_log.active = self.active
|
lac_log.active = self.active
|
||||||
lac_log.steeringAngleDeg = CS.steeringAngleDeg
|
lac_log.steeringAngleDeg = CS.steeringAngleDeg
|
||||||
|
@ -680,6 +687,8 @@ class Controls:
|
||||||
controlsState.lateralControlState.debugState = lac_log
|
controlsState.lateralControlState.debugState = lac_log
|
||||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||||
controlsState.lateralControlState.angleState = lac_log
|
controlsState.lateralControlState.angleState = lac_log
|
||||||
|
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature:
|
||||||
|
controlsState.lateralControlState.curvatureState = lac_log
|
||||||
elif lat_tuning == 'pid':
|
elif lat_tuning == 'pid':
|
||||||
controlsState.lateralControlState.pidState = lac_log
|
controlsState.lateralControlState.pidState = lac_log
|
||||||
elif lat_tuning == 'lqr':
|
elif lat_tuning == 'lqr':
|
||||||
|
|
|
@ -13,7 +13,7 @@ class LatControl(ABC):
|
||||||
self.sat_count = 0.
|
self.sat_count = 0.
|
||||||
|
|
||||||
@abstractmethod
|
@abstractmethod
|
||||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
|
|
|
@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||||
|
|
||||||
|
|
||||||
class LatControlAngle(LatControl):
|
class LatControlAngle(LatControl):
|
||||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||||
|
|
||||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||||
|
@ -22,4 +22,4 @@ class LatControlAngle(LatControl):
|
||||||
angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
|
angle_log.saturated = self._check_saturation(angle_control_saturated, CS)
|
||||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||||
return 0, float(angle_steers_des), angle_log
|
return 0, float(angle_steers_des), 0, 0, 0, 0, angle_log
|
||||||
|
|
|
@ -0,0 +1,38 @@
|
||||||
|
import math
|
||||||
|
|
||||||
|
from cereal import log
|
||||||
|
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||||
|
|
||||||
|
|
||||||
|
class LatControlCurvature(LatControl):
|
||||||
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
|
curvature_log = log.ControlsState.LateralCurvatureState.new_message()
|
||||||
|
|
||||||
|
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||||
|
curvature_log.active = False
|
||||||
|
curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg), CS.vEgo, params.roll)
|
||||||
|
curvature_rate = 0
|
||||||
|
path_angle = 0
|
||||||
|
path_deviation = 0
|
||||||
|
else:
|
||||||
|
curvature_log.active = True
|
||||||
|
|
||||||
|
curvatures = lat_plan.curvatures
|
||||||
|
path_points = lat_plan.dPathPoints
|
||||||
|
|
||||||
|
# "road/lane curvature" is different from the immediate manoeuvre/path curvature
|
||||||
|
# later curvature values are probably closer to this "road curvature" value
|
||||||
|
curvature = -curvatures[6]
|
||||||
|
path_deviation = -path_points[0] if len(path_points) > 0 else 0
|
||||||
|
|
||||||
|
# TODO
|
||||||
|
curvature_rate = 0
|
||||||
|
path_angle = 0
|
||||||
|
|
||||||
|
# TODO: calculate saturated, like latcontrol_angle
|
||||||
|
curvature_log.saturated = False
|
||||||
|
curvature_log.curvature = curvature
|
||||||
|
curvature_log.curvatureRate = curvature_rate
|
||||||
|
curvature_log.pathAngle = path_angle
|
||||||
|
curvature_log.pathDeviation = path_deviation
|
||||||
|
return 0, 0, curvature, curvature_rate, path_angle, path_deviation, curvature_log
|
|
@ -65,7 +65,7 @@ class LatControlINDI(LatControl):
|
||||||
self.steer_filter.x = 0.
|
self.steer_filter.x = 0.
|
||||||
self.speed = 0.
|
self.speed = 0.
|
||||||
|
|
||||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
self.speed = CS.vEgo
|
self.speed = CS.vEgo
|
||||||
# Update Kalman filter
|
# Update Kalman filter
|
||||||
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
|
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
|
||||||
|
@ -119,4 +119,4 @@ class LatControlINDI(LatControl):
|
||||||
indi_log.output = float(output_steer)
|
indi_log.output = float(output_steer)
|
||||||
indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
||||||
|
|
||||||
return float(output_steer), float(steers_des), indi_log
|
return float(output_steer), float(steers_des), 0, 0, 0, 0, indi_log
|
||||||
|
|
|
@ -31,7 +31,7 @@ class LatControlLQR(LatControl):
|
||||||
super().reset()
|
super().reset()
|
||||||
self.i_lqr = 0.0
|
self.i_lqr = 0.0
|
||||||
|
|
||||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
||||||
|
|
||||||
steers_max = get_steer_max(CP, CS.vEgo)
|
steers_max = get_steer_max(CP, CS.vEgo)
|
||||||
|
@ -83,4 +83,4 @@ class LatControlLQR(LatControl):
|
||||||
lqr_log.output = output_steer
|
lqr_log.output = output_steer
|
||||||
lqr_log.lqrOutput = lqr_output
|
lqr_log.lqrOutput = lqr_output
|
||||||
lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
||||||
return output_steer, desired_angle, lqr_log
|
return output_steer, desired_angle, 0, 0, 0, 0, lqr_log
|
||||||
|
|
|
@ -18,7 +18,7 @@ class LatControlPID(LatControl):
|
||||||
super().reset()
|
super().reset()
|
||||||
self.pid.reset()
|
self.pid.reset()
|
||||||
|
|
||||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
def update(self, active, CS, CP, VM, params, last_actuators, lat_plan, desired_curvature, desired_curvature_rate):
|
||||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||||
|
@ -51,4 +51,4 @@ class LatControlPID(LatControl):
|
||||||
pid_log.output = output_steer
|
pid_log.output = output_steer
|
||||||
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
|
||||||
|
|
||||||
return output_steer, angle_steers_des, pid_log
|
return output_steer, angle_steers_des, 0, 0, 0, 0, pid_log
|
||||||
|
|
Loading…
Reference in New Issue