From 451abc8d3b4f57420d95ecd8d8a04dcbc485c154 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Tue, 14 Feb 2023 20:29:16 -0800 Subject: [PATCH] use long plan in lat plan --- selfdrive/controls/lib/lateral_planner.py | 5 ++--- selfdrive/controls/plannerd.py | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 6a92b7e790..338b936cc1 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -52,10 +52,9 @@ class LateralPlanner: def update(self, sm, v_plan): # clip speed , lateral planning is not possible at 0 speed self.v_ego = max(MIN_SPEED, sm['carState'].vEgo) - v_plan = self.v_ego * np.ones(len(T_IDXS)) + v_plan = np.clip(v_plan, MIN_SPEED, np.inf) self.v_plan = v_plan - plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS)))) - plan_odo = np.clip(plan_odo, MIN_SPEED, np.inf)[:LAT_MPC_N + 1] + plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS))))[:LAT_MPC_N + 1] measured_curvature = sm['controlsState'].curvature # Parse model predictions diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 93d0c80dac..aac7a78ccd 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -30,10 +30,10 @@ def plannerd_thread(sm=None, pm=None): sm.update() if sm.updated['modelV2']: - lateral_planner.update(sm) - lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) + lateral_planner.update(sm, longitudinal_planner.mpc.v_solution) + lateral_planner.publish(sm, pm) def main(sm=None, pm=None):