From 47cc314b14baba530920e78ad1c31935acc71b8b Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Mon, 30 Sep 2024 18:13:51 -0700 Subject: [PATCH] Gate acceleration on model gas press predictions (#33643) * no ui squash * already calibrated * Already calibrated * Add longitudinal maneuver tests * Fix linter errors * Added get_coast_accel function --------- Co-authored-by: Bruce Wayne --- .../controls/lib/longitudinal_planner.py | 27 +++++++++++-- .../test/longitudinal_maneuvers/maneuver.py | 16 ++++++-- .../test/longitudinal_maneuvers/plant.py | 12 ++++-- .../test_longitudinal.py | 38 +++++++++++++++++++ 4 files changed, 83 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0b65119383..54838924be 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -21,6 +21,8 @@ A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +ALLOW_THROTTLE_THRESHOLD = 0.5 +ACCEL_LIMIT_MARGIN = 0.05 # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -30,6 +32,9 @@ _A_TOTAL_MAX_BP = [20., 40.] def get_max_accel(v_ego): return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) +def get_coast_accel(pitch): + return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py + def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ @@ -69,6 +74,7 @@ class LongitudinalPlanner: self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt + self.allow_throttle = True self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) @@ -93,11 +99,20 @@ class LongitudinalPlanner: v = np.zeros(len(T_IDXS_MPC)) a = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC)) - return x, v, a, j + if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: + throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] + else: + throttle_prob = 1.0 + return x, v, a, j, throttle_prob def update(self, sm): self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + if len(sm['carControl'].orientationNED) == 3: + accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) + else: + accel_coast = ACCEL_MAX + v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS @@ -130,6 +145,13 @@ class LongitudinalPlanner: self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) + x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) + self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD + + if not self.allow_throttle and v_ego > 5.0: # Don't clip at low speeds since throttle_prob doesn't account for creep + # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin + clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) if force_slow_decel: v_cruise = 0.0 @@ -140,7 +162,6 @@ class LongitudinalPlanner: self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].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=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -179,6 +200,6 @@ class LongitudinalPlanner: longitudinalPlan.aTarget = a_target longitudinalPlan.shouldStop = should_stop longitudinalPlan.allowBrake = True - longitudinalPlan.allowThrottle = True + longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py index 6c8495cc3b..301f99dd56 100644 --- a/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -12,11 +12,14 @@ class Maneuver: self.breakpoints = kwargs.get("breakpoints", [0.0, duration]) self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))]) self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))]) + self.prob_throttle_values = kwargs.get("prob_throttle_values", [1.0 for i in range(len(self.breakpoints))]) self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))]) + self.pitch_values = kwargs.get("pitch_values", [0.0 for i in range(len(self.breakpoints))]) self.only_lead2 = kwargs.get("only_lead2", False) self.only_radar = kwargs.get("only_radar", False) self.ensure_start = kwargs.get("ensure_start", False) + self.ensure_slowdown = kwargs.get("ensure_slowdown", False) self.enabled = kwargs.get("enabled", True) self.e2e = kwargs.get("e2e", False) self.personality = kwargs.get("personality", 0) @@ -42,9 +45,11 @@ class Maneuver: logs = [] while plant.current_time < self.duration: speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) - prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) + prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) - log = plant.step(speed_lead, prob, cruise) + pitch = np.interp(plant.current_time, self.breakpoints, self.pitch_values) + prob_throttle = np.interp(plant.current_time, self.breakpoints, self.prob_throttle_values) + log = plant.step(speed_lead, prob_lead, cruise, pitch, prob_throttle) d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. @@ -57,13 +62,18 @@ class Maneuver: speed_lead, log['acceleration']])) - if d_rel < .4 and (self.only_radar or prob > 0.5): + if d_rel < .4 and (self.only_radar or prob_lead > 0.5): print("Crashed!!!!") valid = False if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: print('LongitudinalPlanner not starting!') valid = False + + if self.ensure_slowdown and log['speed'] > 5.5: + print('LongitudinalPlanner not slowing down!') + valid = False + if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04: print('Not stopping with force decel') valid = False diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 21f2b3b57f..3c8c09d6be 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -57,13 +57,14 @@ class Plant: def current_time(self): return float(self.rk.frame) / self.rate - def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): + def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle=1.0): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') ss = messaging.new_message('selfdriveState') car_state = messaging.new_message('carState') + car_control = messaging.new_message('carControl') model = messaging.new_message('modelV2') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead @@ -73,14 +74,14 @@ class Plant: v_rel = v_lead - self.speed if self.only_radar: status = True - elif prob > .5: + elif prob_lead > .5: status = True else: status = False else: d_rel = 200. v_rel = 0. - prob = 0.0 + prob_lead = 0.0 status = False lead = log.RadarState.LeadData.new_message() @@ -94,7 +95,7 @@ class Plant: # TODO use real radard logic for this lead.aLeadTau = float(_LEAD_ACCEL_TAU) lead.status = status - lead.modelProb = float(prob) + lead.modelProb = float(prob_lead) if not self.only_lead2: radar.radarState.leadOne = lead radar.radarState.leadTwo = lead @@ -112,6 +113,7 @@ class Plant: acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration + model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off ss.selfdriveState.experimentalMode = self.e2e @@ -120,10 +122,12 @@ class Plant: car_state.carState.vEgo = float(self.speed) car_state.carState.standstill = self.speed < 0.01 car_state.carState.vCruise = float(v_cruise * 3.6) + car_control.carControl.orientationNED = [0., float(pitch), 0.] # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, 'carState': car_state.carState, + 'carControl': car_control.carControl, 'controlsState': control.controlsState, 'selfdriveState': ss.selfdriveState, 'modelV2': model.modelV2} diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 62a95babeb..7eea44aae6 100644 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -82,6 +82,44 @@ def create_maneuvers(kwargs): breakpoints=[0.0, 2., 2.01], **kwargs, ), + Maneuver( + "approach stopped car at 20m/s, with prob_throttle_values and pitch = -0.1", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_throttle_values=[1., 0., 0.], + cruise_values=[20., 20., 20.], + pitch_values=[0., -0.1, -0.1], + breakpoints=[0.0, 2., 2.01], + **kwargs, + ), + Maneuver( + "approach stopped car at 20m/s, with prob_throttle_values and pitch = +0.1", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_throttle_values=[1., 0., 0.], + cruise_values=[20., 20., 20.], + pitch_values=[0., 0.1, 0.1], + breakpoints=[0.0, 2., 2.01], + **kwargs, + ), + Maneuver( + "slow to 5m/s with allow_throttle = False and pitch = +0.1", + duration=25., + initial_speed=20., + lead_relevancy=False, + prob_throttle_values=[1., 0., 0.], + cruise_values=[20., 20., 20.], + pitch_values=[0., 0.1, 0.1], + breakpoints=[0.0, 2., 2.01], + ensure_slowdown=True, + **kwargs, + ), Maneuver( "approach slower cut-in car at 20m/s", duration=20.,