From b4a0c211f01b57a430418e7f701e8e09ab284e93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Thu, 20 Jun 2024 16:33:06 -0700 Subject: [PATCH] Long planner: small cleanup (#32804) old-commit-hash: 442e9f4ae492901a5698b3188741c97c053026c4 --- selfdrive/controls/lib/longitudinal_planner.py | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8772fadb47..00b4e243b4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,11 +44,10 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -def get_accel_from_plan(CP, long_plan): - speeds = long_plan.speeds +def get_accel_from_plan(CP, speeds, accels): if len(speeds) == CONTROL_N: v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds) - a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, long_plan.accels) + a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels) v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds) a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now @@ -141,11 +140,9 @@ class LongitudinalPlanner: 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['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) - self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] - self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] - self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) + self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) + self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, 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 > 2 and not sm['carState'].standstill @@ -154,7 +151,7 @@ class LongitudinalPlanner: # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) + self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): @@ -179,7 +176,7 @@ class LongitudinalPlanner: longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw - a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan) + a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels) longitudinalPlan.aTarget = a_target longitudinalPlan.shouldStop = should_stop longitudinalPlan.allowBrake = True