From f3414250a3e783fce7bf7af171fd2ac8ec1f1a5d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Mar 2024 00:57:57 -0700 Subject: [PATCH] Move personality to controlsState (#31855) * start at param * start by sending personality * change to personality * POC: button changes personality * what's wrong with this? * fix * not really possible but fuzzy test catches this * there's always a typo * dang, we're dropping messages * clean up * no comment * bump * rename * not all cars yet * works but at what cost * clean up * inside settings * write param so we save the distance button changes * setChecked activates buttonToggled and already writes param! * don't need this, we update from longitudinalPlan on changes * some clean up * more * ui * allow some time for ui to receive and write param * plannerd: only track changes in case no ui * Revert "plannerd: only track changes in case no ui" This reverts commit 2b081aa6ceb92c67a621b74592b2292756d29871. * write in plannerd as well, I assume this is atomic? * don't write when setting checked (only user clicks) * better nane * more * Update selfdrive/controls/lib/longitudinal_planner.py Co-authored-by: Cameron Clough * doesn't write param now * ParamWatcher is nice * no debug * Update translations * fix * odd drain sock proc replay behavior * vanish * Revert "odd drain sock proc replay behavior" This reverts commit 29b70b39413e1852bb512155af6b6a94a5bd9454. * add GM * only if OP long * move personality to controlsState, since eventually it won't be exclusive to long planner more bump * diff without translations * fix * put nonblocking * CS should start at up to date personality always (no ui flicker) * update toggle on cereal message change * fix * fix that * ubmp * mypy doesn't know this is an int :( * update translations * fix the tests * revert ui * not here * migrate controlsState * Revert "migrate controlsState" - i see no reason we need to test with any specific personality This reverts commit 6063508f2df1a5623f113cda34dcd59a1f4b2ac9. * Update ref_commit --------- Co-authored-by: Cameron Clough old-commit-hash: 29e55f99a54d95215aa79ecf94a22363f82913a6 --- cereal | 2 +- selfdrive/controls/controlsd.py | 11 +++++++++- .../controls/lib/longitudinal_planner.py | 20 ++----------------- selfdrive/controls/tests/test_cruise_speed.py | 8 +++----- .../controls/tests/test_following_distance.py | 8 +++----- .../test/longitudinal_maneuvers/maneuver.py | 2 ++ .../test/longitudinal_maneuvers/plant.py | 4 +++- selfdrive/test/process_replay/ref_commit | 2 +- 8 files changed, 25 insertions(+), 32 deletions(-) diff --git a/cereal b/cereal index 724d1d22ac..430535068a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 724d1d22ac877ff75058f0d62860cf51c27f3546 +Subproject commit 430535068ac3bb94d3e117a3cfbc348ef37eb72d diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 29358cb7b6..33336676f2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -146,6 +146,7 @@ class Controls: self.steer_limited = False self.desired_curvature = 0.0 self.experimental_mode = False + self.personality = self.read_personality_param() self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False @@ -680,7 +681,7 @@ class Controls: hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead - hudControl.leadDistanceBars = self.sm['longitudinalPlan'].personality.raw + 1 + hudControl.leadDistanceBars = self.personality + 1 hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True @@ -769,6 +770,7 @@ class Controls: controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter controlsState.experimentalMode = self.experimental_mode + controlsState.personality = self.personality lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: @@ -821,10 +823,17 @@ class Controls: self.CS_prev = CS + def read_personality_param(self): + try: + return int(self.params.get('LongitudinalPersonality')) + except (ValueError, TypeError): + return log.LongitudinalPersonality.standard + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2b1fd01112..6cc6e80d3d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,8 +2,6 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params -from cereal import log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -61,16 +59,6 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - self.params = Params() - self.param_read_counter = 0 - self.personality = log.LongitudinalPersonality.standard - self.read_param() - - def read_param(self): - try: - self.personality = int(self.params.get('LongitudinalPersonality')) - except (ValueError, TypeError): - self.personality = log.LongitudinalPersonality.standard @staticmethod def parse_model(model_msg, model_error): @@ -89,9 +77,6 @@ class LongitudinalPlanner: return x, v, a, j def update(self, sm): - if self.param_read_counter % 50 == 0: - self.read_param() - self.param_read_counter += 1 self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -130,11 +115,11 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=self.personality) + self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) @@ -170,6 +155,5 @@ class LongitudinalPlanner: longitudinalPlan.fcw = self.fcw longitudinalPlan.solverExecutionTime = self.mpc.solve_time - longitudinalPlan.personality = self.personality pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py index 76a2222e85..c46d03ad1e 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -5,7 +5,6 @@ import unittest from parameterized import parameterized_class from cereal import log -from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT from cereal import car from openpilot.common.conversions import Conversions as CV @@ -15,7 +14,7 @@ ButtonEvent = car.CarState.ButtonEvent ButtonType = car.CarState.ButtonEvent.Type -def run_cruise_simulation(cruise, e2e, t_end=20.): +def run_cruise_simulation(cruise, e2e, personality, t_end=20.): man = Maneuver( '', duration=t_end, @@ -26,6 +25,7 @@ def run_cruise_simulation(cruise, e2e, t_end=20.): prob_lead_values=[0.0], breakpoints=[0.], e2e=e2e, + personality=personality, ) valid, output = man.evaluate() assert valid @@ -38,12 +38,10 @@ def run_cruise_simulation(cruise, e2e, t_end=20.): [5,35])) # speed class TestCruiseSpeed(unittest.TestCase): def test_cruise_speed(self): - params = Params() - params.put("LongitudinalPersonality", str(self.personality)) print(f'Testing {self.speed} m/s') cruise_speed = float(self.speed) - simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e) + simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s') diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 3b31632721..f58e6383c4 100755 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -3,14 +3,13 @@ import unittest import itertools from parameterized import parameterized_class -from openpilot.common.params import Params from cereal import log from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver -def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): +def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0): man = Maneuver( '', duration=t_end, @@ -20,6 +19,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): speed_lead_values=[v_lead], breakpoints=[0.], e2e=e2e, + personality=personality, ) valid, output = man.evaluate() assert valid @@ -34,10 +34,8 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False): [0,10,35])) # speed class TestFollowingDistance(unittest.TestCase): def test_following_distance(self): - params = Params() - params.put("LongitudinalPersonality", str(self.personality)) v_lead = float(self.speed) - simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e) + simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality) correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) err_ratio = 0.2 if self.e2e else 0.1 self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5)) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 000225ab77..6c8495cc3b 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -19,6 +19,7 @@ class Maneuver: self.ensure_start = kwargs.get("ensure_start", False) self.enabled = kwargs.get("enabled", True) self.e2e = kwargs.get("e2e", False) + self.personality = kwargs.get("personality", 0) self.force_decel = kwargs.get("force_decel", False) self.duration = duration @@ -33,6 +34,7 @@ class Maneuver: only_lead2=self.only_lead2, only_radar=self.only_radar, e2e=self.e2e, + personality=self.personality, force_decel=self.force_decel, ) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index bb935fdc8e..3fb8b6bab0 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -15,7 +15,7 @@ class Plant: messaging_initialized = False def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, - enabled=True, only_lead2=False, only_radar=False, e2e=False, force_decel=False): + enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): self.rate = 1. / DT_MDL if not Plant.messaging_initialized: @@ -39,6 +39,7 @@ class Plant: self.only_lead2 = only_lead2 self.only_radar = only_radar self.e2e = e2e + self.personality = personality self.force_decel = force_decel self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) @@ -112,6 +113,7 @@ class Plant: control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off control.controlsState.vCruise = float(v_cruise * 3.6) control.controlsState.experimentalMode = self.e2e + control.controlsState.personality = self.personality control.controlsState.forceDecel = self.force_decel car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 91e32036b5..2c6096e756 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4f02bcfbf45697c5e6ba0a032797f6b2f37e16d3 +5eedd32ba97f9109c64059afc8e0fb827567f2ed