From ef0b120a9ac589b22672b95adb57ecb4e4cee336 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Wed, 14 Jul 2021 11:51:48 -0700 Subject: [PATCH] add jerk to longitudinal plan (#21598) * add jerk output, so its like lateral * typo * add to packet * update cereal and ref --- cereal | 2 +- selfdrive/controls/lib/lead_mpc.py | 5 +++++ selfdrive/controls/lib/long_mpc.py | 5 +++++ selfdrive/controls/lib/longitudinal_planner.py | 2 ++ selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 14 insertions(+), 2 deletions(-) diff --git a/cereal b/cereal index 150e87dbfd..8847ab51ed 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 150e87dbfd3260421b7fc35d3ff7ca417a38eeb2 +Subproject commit 8847ab51ed4bf18186b18a616f6e4d870e72e910 diff --git a/selfdrive/controls/lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc.py index c7d1c37dc8..da5bbc25f5 100644 --- a/selfdrive/controls/lib/lead_mpc.py +++ b/selfdrive/controls/lib/lead_mpc.py @@ -25,6 +25,10 @@ class LeadMpc(): self.duration = 0 self.status = False + self.v_solution = np.zeros(CONTROL_N) + self.a_solution = np.zeros(CONTROL_N) + self.j_solution = np.zeros(CONTROL_N) + def reset_mpc(self): ffi, self.libmpc = libmpc_py.get_libmpc(self.lead_id) self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, @@ -85,6 +89,7 @@ class LeadMpc(): self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) self.v_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.v_ego) self.a_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.a_ego) + self.j_solution = interp(T_IDXS[:CONTROL_N], MPC_T[:-1], self.mpc_solution.j_ego) self.duration = int((sec_since_boot() - t) * 1e9) # Reset if NaN or goes through lead car diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 51914a96e2..6f0db25fcf 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -29,6 +29,10 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = 0 self.cur_state[0].a_ego = 0 + self.v_solution = [0.0 for i in range(len(T_IDXS))] + self.a_solution = [0.0 for i in range(len(T_IDXS))] + self.j_solution = [0.0 for i in range(len(T_IDXS)-1)] + def set_accel_limits(self, min_a, max_a): self.min_a = min_a self.max_a = max_a @@ -54,6 +58,7 @@ class LongitudinalMpc(): self.v_solution = list(self.mpc_solution.v_ego) self.a_solution = list(self.mpc_solution.a_ego) + self.j_solution = list(self.mpc_solution.j_ego) # Reset if NaN or goes through lead car nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 60d109ac3f..643cefd7a1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -109,6 +109,7 @@ class Planner(): self.longitudinalPlanSource = key self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N] self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N] + self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N] next_a = self.mpcs[key].a_solution[5] # determine fcw @@ -140,6 +141,7 @@ class Planner(): longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory] longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] + longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] longitudinalPlan.hasLead = self.mpcs['lead0'].status longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 44eb8c34ad..2002a59f22 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -97382f81d5d59213c25b5a3b553b3da2076742a2 \ No newline at end of file +f2bad42b2f22dc4f2843c5697f5444f0fb4af175 \ No newline at end of file