uiPlan: add planned acceleration field (#27484)

* add accel

* bump cereal

* Update ref_commit

---------

Co-authored-by: Bruce Wayne <harald.the.engineer@gmail.com>
old-commit-hash: 3ab8645d3a
beeps
Shane Smiskol 2 years ago committed by GitHub
parent 042ff4e58f
commit 827c5678f0
  1. 2
      cereal
  2. 3
      selfdrive/controls/lib/longitudinal_planner.py
  3. 1
      selfdrive/controls/plannerd.py
  4. 2
      selfdrive/test/process_replay/ref_commit

@ -1 +1 @@
Subproject commit b88523f05ac958f87a8f6d57c3f4bb20da55f216 Subproject commit 42f84fd85d06c0fc85371daa2b4820fca49d763e

@ -122,8 +122,9 @@ class LongitudinalPlanner:
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j)
self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone # TODO counter is only needed because radar is glitchy, remove once radar is gone

@ -22,6 +22,7 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist() uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist() uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
pm.send('uiPlan', ui_send) pm.send('uiPlan', ui_send)
def plannerd_thread(sm=None, pm=None): def plannerd_thread(sm=None, pm=None):

@ -1 +1 @@
dfa8e947c4ef76a9d89974a434e94a078e1ccc6a f9c7e05b836c4bff364978752e82d64b90f9d6e6

Loading…
Cancel
Save