From 06480657a2a44837d95a67bfe97a6dec88d367b9 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 10 Mar 2024 04:26:48 -0700 Subject: [PATCH] move personality to controlsState, since eventually it won't be exclusive to long planner more bump --- cereal | 2 +- selfdrive/controls/controlsd.py | 12 +++++++ .../controls/lib/longitudinal_planner.py | 35 ++----------------- selfdrive/controls/plannerd.py | 3 +- .../test/longitudinal_maneuvers/plant.py | 3 +- 5 files changed, 18 insertions(+), 37 deletions(-) diff --git a/cereal b/cereal index 93d3df3210..ae01891097 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 93d3df3210a8c4f4c7e6f45950f73dc8a2f09a2c +Subproject commit ae018910975883dfe70700fe161092ef62050fc2 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 29358cb7b6..e7f360218b 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 = log.LongitudinalPersonality.standard self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False @@ -650,6 +651,12 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) + # decrement personality on distance button press + if self.CP.openpilotLongitudinalControl: + if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): + self.personality = (self.personality - 1) % 3 + self.params.put('LongitudinalPersonality', str(self.personality)) + return CC, lac_log def publish_logs(self, CS, start_time, CC, lac_log): @@ -769,6 +776,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: @@ -825,6 +833,10 @@ class Controls: 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 + try: + self.personality = int(self.params.get('LongitudinalPersonality')) + except (ValueError, TypeError): + self.personality = log.LongitudinalPersonality.standard 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 014833eac6..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 car, log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -17,8 +15,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error from openpilot.common.swaglog import cloudlog -ButtonType = car.CarState.ButtonEvent.Type - LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] @@ -63,19 +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 - - def write_param(self): - self.params.put('LongitudinalPersonality', str(self.personality)) @staticmethod def parse_model(model_msg, model_error): @@ -93,18 +76,7 @@ class LongitudinalPlanner: j = np.zeros(len(T_IDXS_MPC)) return x, v, a, j - def update(self, sm, carState_sock): - if self.param_read_counter % 50 == 0: - self.read_param() - - # decrement personality on distance button press - if self.CP.openpilotLongitudinalControl: - for m in messaging.drain_sock(carState_sock, wait_for_one=False): - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in m.carState.buttonEvents): - self.personality = (self.personality - 1) % 3 - self.write_param() - - self.param_read_counter += 1 + def update(self, sm): self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -143,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) @@ -183,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/plannerd.py b/selfdrive/controls/plannerd.py index aaba814abc..eeeeda050e 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -30,12 +30,11 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'], poll='modelV2', ignore_avg_freq=['radarState']) - carState_sock = messaging.sub_sock('carState') while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm, carState_sock) + longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) publish_ui_plan(sm, pm, longitudinal_planner) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 7e9576fdec..bb935fdc8e 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -50,7 +50,6 @@ class Plant: from openpilot.selfdrive.car.honda.interface import CarInterface self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed) - self.carState_sock = messaging.sub_sock('carState') @property def current_time(self): @@ -122,7 +121,7 @@ class Plant: 'carState': car_state.carState, 'controlsState': control.controlsState, 'modelV2': model.modelV2} - self.planner.update(sm, self.carState_sock) + self.planner.update(sm) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired self.speeds = self.planner.v_desired_trajectory.tolist()