use long plan in lat plan

pull/27343/head
Bruce Wayne 3 years ago
parent 64744758f0
commit 451abc8d3b
  1. 5
      selfdrive/controls/lib/lateral_planner.py
  2. 4
      selfdrive/controls/plannerd.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

@ -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):

Loading…
Cancel
Save