diff --git a/cereal b/cereal index 724d1d22ac877f..430535068ac3bb 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 29358cb7b65bf9..33336676f2aaba 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -146,6 +146,7 @@ def __init__(self, CI=None): 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 @@ def publish_logs(self, CS, start_time, CC, lac_log): 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 @@ def publish_logs(self, CS, start_time, CC, lac_log): 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 @@ def step(self): 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 2b1fd0111256ed..6cc6e80d3dab57 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 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): 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 @@ def parse_model(model_msg, model_error): 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 @@ def update(self, sm): 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 @@ def publish(self, sm, pm): 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 76a2222e85d6f8..c46d03ad1e9897 100755 --- a/selfdrive/controls/tests/test_cruise_speed.py +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -5,7 +5,6 @@ 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 @@ 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 3b316327218d60..f58e6383c42c27 100755 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -3,14 +3,13 @@ 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 000225ab77fea6..6c8495cc3b5567 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -19,6 +19,7 @@ def __init__(self, title, duration, **kwargs): 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 @@ def evaluate(self): 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 bb935fdc8e0f77..3fb8b6bab04f53 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 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, 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 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): 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 91e32036b5d791..2c6096e756f59f 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4f02bcfbf45697c5e6ba0a032797f6b2f37e16d3 +5eedd32ba97f9109c64059afc8e0fb827567f2ed