MPC path in UI (#27380)

* 10s lat

* Full length MPC

* redfine N

* Leave controls the same for now

* Updates

* use long plan in lat plan

* interp plan

* simplergit add selfdrive/controls/plannerd.py selfdrive/controls/

* expand to 10s

* revert this

* fix linter

* vizualize

* fix long test

* typo

* cleanup

* compiles

* unused

* unused

* bump cereal

* bump cereal

* use model if no uiplanm

* update replay

* update ref commit

* bump cereal to master

cereal

@@ -1 +1 @@
-Subproject commit 162a26ca2d7e5bc9a42bb5ea11e98194f722027b
+Subproject commit b88523f05ac958f87a8f6d57c3f4bb20da55f216

selfdrive/controls/lib/longitudinal_planner.py

@@ -121,7 +121,8 @@ 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)
 
-    self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
+    self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
+    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.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 

selfdrive/controls/plannerd.py

@@ -1,12 +1,28 @@
 #!/usr/bin/env python3
+import numpy as np
 from cereal import car
 from common.params import Params
 from common.realtime import Priority, config_realtime_process
 from system.swaglog import cloudlog
+from selfdrive.modeld.constants import T_IDXS
 from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
 from selfdrive.controls.lib.lateral_planner import LateralPlanner
 import cereal.messaging as messaging
 
+def cumtrapz(x, t):
+  return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
+
+def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
+  plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS)
+  model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS)
+
+  ui_send = messaging.new_message('uiPlan')
+  ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
+  uiPlan = ui_send.uiPlan
+  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.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
+  pm.send('uiPlan', ui_send)
 
 def plannerd_thread(sm=None, pm=None):
   config_realtime_process(5, Priority.CTRL_LOW)
@@ -24,7 +40,7 @@ def plannerd_thread(sm=None, pm=None):
                              poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
 
   if pm is None:
-    pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan'])
+    pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan', 'uiPlan'])
 
   while True:
     sm.update()
@@ -34,7 +50,7 @@ def plannerd_thread(sm=None, pm=None):
       lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
-
+      publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
 
 def main(sm=None, pm=None):
   plannerd_thread(sm, pm)

selfdrive/modeld/models/driving.cc

@@ -200,7 +200,7 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMet
 }
 
 template<size_t size>
-void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
                const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
   xyzt.setT(to_kj_array_ptr(t));
   xyzt.setX(to_kj_array_ptr(x));
@@ -209,7 +209,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<flo
 }
 
 template<size_t size>
-void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
                const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
                const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
   fill_xyzt(xyzt, t, x, y, z);

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -99,13 +99,13 @@ class Plant:
     # Simulate model predicting slightly faster speed
     # this is to ensure lead policy is effective when model
     # does not predict slowdown in e2e mode
-    position = log.ModelDataV2.XYZTData.new_message()
+    position = log.XYZTData.new_message()
     position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)]
     model.modelV2.position = position
-    velocity = log.ModelDataV2.XYZTData.new_message()
+    velocity = log.XYZTData.new_message()
     velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)]
     model.modelV2.velocity = velocity
-    acceleration = log.ModelDataV2.XYZTData.new_message()
+    acceleration = log.XYZTData.new_message()
     acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)]
     model.modelV2.acceleration = acceleration
 

selfdrive/test/process_replay/process_replay.py

@@ -292,7 +292,7 @@ CONFIGS = [
   ProcessConfig(
     proc_name="plannerd",
     pub_sub={
-      "modelV2": ["lateralPlan", "longitudinalPlan"],
+      "modelV2": ["lateralPlan", "longitudinalPlan", "uiPlan"],
       "carControl": [], "carState": [], "controlsState": [], "radarState": [],
     },
     ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"],

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-086896986a3fcdb1a03d9afd00a9abc928f9ef25
+70753c5f491de7432ff22f4ef560820ce9919b2e

selfdrive/ui/qt/maps/map_helpers.cc

@@ -31,7 +31,7 @@ QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
 QMapbox::CoordinatesCollections model_to_collection(
   const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
   const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
-  const cereal::ModelDataV2::XYZTData::Reader &line){
+  const cereal::XYZTData::Reader &line){
 
   Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]);
   Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]);

selfdrive/ui/qt/maps/map_helpers.h

@@ -20,7 +20,7 @@ QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in);
 QMapbox::CoordinatesCollections model_to_collection(
   const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
   const cereal::LiveLocationKalman::Measurement::Reader &positionECEF,
-  const cereal::ModelDataV2::XYZTData::Reader &line);
+  const cereal::XYZTData::Reader &line);
 QMapbox::CoordinatesCollections coordinate_to_collection(const QMapbox::Coordinate &c);
 QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List<cereal::NavRoute::Coordinate>::Reader &coordinate_list);
 QMapbox::CoordinatesCollections coordinate_list_to_collection(const QList<QGeoCoordinate> &coordinate_list);

selfdrive/ui/qt/onroad.cc

@@ -674,7 +674,7 @@ void AnnotatedCameraWidget::paintGL() {
 
   if (s->worldObjectsVisible()) {
     if (sm.rcv_frame("modelV2") > s->scene.started_frame) {
-      update_model(s, sm["modelV2"].getModelV2());
+      update_model(s, sm["modelV2"].getModelV2(), sm["uiPlan"].getUiPlan());
       if (sm.rcv_frame("radarState") > s->scene.started_frame) {
         update_leads(s, radar_state, sm["modelV2"].getModelV2().getPosition());
       }

selfdrive/ui/ui.cc

@@ -35,7 +35,7 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y,
   return false;
 }
 
-int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) {
+int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) {
   const auto line_x = line.getX();
   int max_idx = 0;
   for (int i = 1; i < TRAJECTORY_SIZE && line_x[i] <= path_height; ++i) {
@@ -44,7 +44,7 @@ int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const
   return max_idx;
 }
 
-void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line) {
+void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) {
   for (int i = 0; i < 2; ++i) {
     auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
     if (lead_data.getStatus()) {
@@ -54,7 +54,7 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con
   }
 }
 
-void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
+void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line,
                       float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) {
   const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
   QPolygonF left_points, right_points;
@@ -79,10 +79,15 @@ void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Rea
   *pvd = left_points + right_points;
 }
 
-void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
+void update_model(UIState *s, 
+                  const cereal::ModelDataV2::Reader &model,
+                  const cereal::UiPlan::Reader &plan) {
   UIScene &scene = s->scene;
-  auto model_position = model.getPosition();
-  float max_distance = std::clamp(model_position.getX()[TRAJECTORY_SIZE - 1],
+  auto plan_position = plan.getPosition();
+  if (plan_position.getX().size() < TRAJECTORY_SIZE){
+    plan_position = model.getPosition();
+  }
+  float max_distance = std::clamp(plan_position.getX()[TRAJECTORY_SIZE - 1],
                                   MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
 
   // update lane lines
@@ -108,8 +113,8 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
     const float lead_d = lead_one.getDRel() * 2.;
     max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
   }
-  max_idx = get_path_length_idx(model_position, max_distance);
-  update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false);
+  max_idx = get_path_length_idx(plan_position, max_distance);
+  update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false);
 }
 
 void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd) {
@@ -248,6 +253,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
     "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "gnssMeasurements",
+    "uiPlan",
   });
 
   Params params;

selfdrive/ui/ui.h

@@ -198,9 +198,11 @@ public slots:
 };
 
 void ui_update_params(UIState *s);
-int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height);
-void update_model(UIState *s, const cereal::ModelDataV2::Reader &model);
+int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height);
+void update_model(UIState *s,
+                  const cereal::ModelDataV2::Reader &model,
+                  const cereal::UiPlan::Reader &plan);
 void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd);
-void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::ModelDataV2::XYZTData::Reader &line);
-void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line,
+void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
+void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line,
                       float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert);