From 32c4af5270df644eb006ec49e8c7ab5739e37fef Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 2 Sep 2022 00:24:53 -0700 Subject: [PATCH] Add e2e long toggle (#25638) * Add toggle * Misc fixes * Update translations * pre alpha not great --- selfdrive/common/params.cc | 1 + .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 ++++------ selfdrive/controls/lib/longitudinal_planner.py | 9 +++++++-- selfdrive/ui/qt/offroad/settings.cc | 6 ++++++ 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 2f9c0c8aff158e..3c5c233bb168fc 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -95,6 +95,7 @@ std::unordered_map keys = { {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"DisablePowerDown", PERSISTENT}, + {"EndToEndLong", PERSISTENT}, {"DisableRadar_Allow", PERSISTENT}, {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB {"DisableUpdates", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index eba0aa11256296..1631d4846c2475 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -248,13 +248,13 @@ def set_weights(self, prev_accel_constraint=True): cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': - cost_weights = [0., 1.0, 0.0, 0.0, 0.0, 1.0] - constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] + cost_weights = [0., 0.2, 0.25, 1.0, 0.0, 1.0] + constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 5.0] elif self.mode == 'e2e': cost_weights = [0., 0.2, 0.25, 1., 0.0, .1] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0] else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized') + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set') self.set_cost_weights(cost_weights, constraint_cost_weights) def set_cur_state(self, v, a): @@ -347,7 +347,7 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): self.source = 'e2e' else: - raise NotImplementedError(f'Planner mode {self.mode} not recognized') + raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update') self.yref[:,1] = x self.yref[:,2] = v @@ -357,8 +357,6 @@ def update(self, carstate, radarstate, v_cruise, x, v, a, j): self.solver.set(i, "yref", self.yref[i]) self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) - self.source = SOURCES[np.argmin(x_obstacles[0])] self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.prev_a) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 63887111689bce..f5cd275c43648e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -6,6 +6,7 @@ import cereal.messaging as messaging from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter +from common.params import Params from common.realtime import DT_MDL from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longcontrol import LongCtrlState @@ -47,7 +48,10 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class Planner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP - self.mpc = LongitudinalMpc() + params = Params() + # TODO read param in the loop for live toggling + mode = 'blended' if params.get_bool('EndToEndLong') else 'acc' + self.mpc = LongitudinalMpc(mode=mode) self.fcw = False @@ -122,7 +126,8 @@ def update(self, sm): self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = self.mpc.crash_cnt > 5 + # TODO write fcw in e2e_long mode + self.fcw = self.mpc.mode == 'acc' and self.mpc.crash_cnt > 5 if self.fcw: cloudlog.info("FCW triggered") diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 99505c9d12e95c..742456b2c54b3f 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -59,6 +59,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "Upload data from the driver facing camera and help improve the driver monitoring algorithm.", "../assets/offroad/icon_monitoring.png", }, + { + "EndToEndLong", + tr("🌮 End-to-end longitudinal (extremely alpha) 🌮"), + tr("Let the driving model control the gas and brakes, openpilot will drive as it thinks a human would. Super experimental."), + "../assets/offroad/icon_road.png", + }, { "DisengageOnAccelerator", "Disengage On Accelerator Pedal",