From e3e0c933983b64b13cfce5f4e974f7d3458502f1 Mon Sep 17 00:00:00 2001 From: Cameron Clough Date: Sun, 23 Jan 2022 01:44:40 +0000 Subject: [PATCH] controls: add curvature based steer control type --- selfdrive/car/tests/test_car_interfaces.py | 2 +- selfdrive/controls/controlsd.py | 17 +++++++-- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 4 +- .../controls/lib/latcontrol_curvature.py | 38 +++++++++++++++++++ selfdrive/controls/lib/latcontrol_indi.py | 4 +- selfdrive/controls/lib/latcontrol_lqr.py | 4 +- selfdrive/controls/lib/latcontrol_pid.py | 4 +- 8 files changed, 61 insertions(+), 14 deletions(-) create mode 100644 selfdrive/controls/lib/latcontrol_curvature.py diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 2f4984c3d..f2eae9bcf 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -34,7 +34,7 @@ class TestCarInterfaces(unittest.TestCase): self.assertGreater(car_params.mass, 1) 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() if tuning == 'pid': self.assertTrue(len(car_params.lateralTuning.pid.kpV)) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 936753030..8fc7cb073 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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_lqr import LatControlLQR 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.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -129,6 +130,8 @@ class Controls: if self.CP.steerControlType == car.CarParams.SteerControlType.angle: 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': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': @@ -516,9 +519,11 @@ class Controls: lat_plan.psis, lat_plan.curvatures, lat_plan.curvatureRates) - actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM, - params, self.last_actuators, desired_curvature, - desired_curvature_rate) + actuators.steer, actuators.steeringAngleDeg, \ + actuators.curvature, actuators.curvatureRate, \ + 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: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0: @@ -528,7 +533,9 @@ class Controls: if CC.latActive: steer = clip(self.sm['testJoystick'].axes[1], -1, 1) # 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.steeringAngleDeg = CS.steeringAngleDeg @@ -680,6 +687,8 @@ class Controls: controlsState.lateralControlState.debugState = lac_log elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log + elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature: + controlsState.lateralControlState.curvatureState = lac_log elif lat_tuning == 'pid': controlsState.lateralControlState.pidState = lac_log elif lat_tuning == 'lqr': diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index eb16aca2e..f5b3b47dd 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -13,7 +13,7 @@ class LatControl(ABC): self.sat_count = 0. @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 def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index c93535631..0e22cc7aa 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees 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() 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.steeringAngleDeg = float(CS.steeringAngleDeg) 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 diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py new file mode 100644 index 000000000..4cbf6a4b1 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index dc1b31bad..12e22c8c2 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -65,7 +65,7 @@ class LatControlINDI(LatControl): self.steer_filter.x = 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 # Update Kalman filter 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.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 diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 5c273a45b..dc6861383 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -31,7 +31,7 @@ class LatControlLQR(LatControl): super().reset() 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() steers_max = get_steer_max(CP, CS.vEgo) @@ -83,4 +83,4 @@ class LatControlLQR(LatControl): lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output 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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index f5ff5a95e..f5c3a03ba 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -18,7 +18,7 @@ class LatControlPID(LatControl): super().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.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -51,4 +51,4 @@ class LatControlPID(LatControl): pid_log.output = output_steer 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