start with cruise source

cereal

@@ -1 +1 @@
-Subproject commit bbcc8eea73fdf3d0cbb0cc925d47379dd33ba0b8
+Subproject commit cdbc15c97214a75f80493279416d3b24e18e0a45

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -17,7 +17,7 @@ LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
 JSON_FILE = "acados_ocp_long.json"
 
-SOURCES = ['lead0', 'lead1', 'cruise']
+SOURCES = ['lead0', 'lead1', 'stop', 'cruise']
 
 X_DIM = 3
 U_DIM = 1
@@ -171,7 +171,7 @@ class LongitudinalMpc():
     self.accel_limit_arr = np.zeros((N+1, 2))
     self.accel_limit_arr[:,0] = -1.2
     self.accel_limit_arr[:,1] = 1.2
-    self.source = SOURCES[2]
+    self.source = SOURCES[3]
 
   def reset(self):
     self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
@@ -286,9 +286,10 @@ class LongitudinalMpc():
     self.cruise_min_a = min_a
     self.cruise_max_a = max_a
 
-  def update(self, carstate, radarstate, v_cruise):
+  def update(self, carstate, radarstate, model, v_cruise):
     v_ego = self.x0[1]
-    self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+    stopping = model.stopLine.prob > 0.5
+    self.status = radarstate.leadOne.status or radarstate.leadTwo.status or stopping
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
@@ -302,6 +303,10 @@ class LongitudinalMpc():
     # and then treat that as a stopped car/obstacle at this new distance.
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    if stopping:
+      stop_line_obstacle = (model.stopLine.x + 6.0) * np.ones(N+1)
+    else:
+      stop_line_obstacle = 400 * np.ones(N+1)
 
     # Fake an obstacle for cruise
     # TODO find cleaner way to write hacky fake cruise obstacle
@@ -312,7 +317,7 @@ class LongitudinalMpc():
                                cruise_upper_bound)
     cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped)
 
-    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, stop_line_obstacle, cruise_obstacle])
     self.source = SOURCES[np.argmin(x_obstacles[0])]
     self.params[:,2] = np.min(x_obstacles, axis=1)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -93,7 +93,7 @@ class Planner():
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired, self.a_desired)
-    self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+    self.mpc.update(sm['carState'], sm['radarState'], sm['modelV2'], v_cruise)
     self.v_desired_trajectory = self.mpc.v_solution[:CONTROL_N]
     self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
     self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]

selfdrive/modeld/models/driving.cc

@@ -30,6 +30,11 @@ constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
 constexpr int LEAD_MHP_SELECTION = 3;
 constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
 
+constexpr int STOP_LINE_MHP_N = 3;
+constexpr int STOP_LINE_PRED_DIM = 8;
+constexpr int STOP_LINE_MHP_SELECTION = 1;
+constexpr int STOP_LINE_MHP_GROUP_SIZE = (2*STOP_LINE_PRED_DIM + STOP_LINE_MHP_SELECTION);
+
 constexpr int POSE_SIZE = 12;
 
 constexpr int PLAN_IDX = 0;
@@ -38,7 +43,9 @@ constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
 constexpr int RE_IDX = LL_PROB_IDX + 8;
 constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
 constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
-constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
+constexpr int STOP_LINE_IDX = LEAD_PROB_IDX + 3;
+constexpr int STOP_LINE_PROB_IDX = STOP_LINE_IDX + STOP_LINE_MHP_N*STOP_LINE_MHP_GROUP_SIZE;
+constexpr int DESIRE_STATE_IDX = STOP_LINE_PROB_IDX + 1;
 constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
 constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
 constexpr int OUTPUT_SIZE =  POSE_IDX + POSE_SIZE;
@@ -117,6 +124,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
   net_outputs.road_edges = &s->output[RE_IDX];
   net_outputs.lead = &s->output[LEAD_IDX];
   net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
+  net_outputs.stop_line = &s->output[STOP_LINE_IDX];
+  net_outputs.stop_line_prob = &s->output[STOP_LINE_PROB_IDX];
   net_outputs.meta = &s->output[DESIRE_STATE_IDX];
   net_outputs.pose = &s->output[POSE_IDX];
   return net_outputs;
@@ -145,6 +154,10 @@ static const float *get_lead_data(const float *lead, int t_offset) {
   return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
 }
 
+static const float *get_stop_line_data(const float *stop_line) {
+  return get_best_data(stop_line, STOP_LINE_MHP_N, STOP_LINE_MHP_GROUP_SIZE, -1);
+}
+
 
 void fill_sigmoid(const float *input, float *output, int len, int stride) {
   for (int i=0; i<len; i++) {
@@ -186,6 +199,29 @@ void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *le
   lead.setAStd(a_stds_arr);
 }
 
+void fill_stop_line(cereal::ModelDataV2::StopLineData::Builder stop_line, const float *stop_line_data, const float *prob) {
+  const float *data = get_stop_line_data(stop_line_data);
+  stop_line.setProb(sigmoid(prob[0]));
+
+  stop_line.setX(data[0]);
+  stop_line.setY(data[1]);
+  stop_line.setZ(data[2]);
+  stop_line.setRoll(data[3]);
+  stop_line.setPitch(data[4]);
+  stop_line.setYaw(data[5]);
+  stop_line.setSpeedAtLine(data[6]);
+  stop_line.setSecondsUntilLine(data[7]);
+
+  stop_line.setXStd(data[STOP_LINE_PRED_DIM+0]);
+  stop_line.setYStd(data[STOP_LINE_PRED_DIM+1]);
+  stop_line.setZStd(data[STOP_LINE_PRED_DIM+2]);
+  stop_line.setRollStd(data[STOP_LINE_PRED_DIM+3]);
+  stop_line.setPitchStd(data[STOP_LINE_PRED_DIM+4]);
+  stop_line.setYawStd(data[STOP_LINE_PRED_DIM+5]);
+  stop_line.setSpeedAtLineStd(data[STOP_LINE_PRED_DIM+6]);
+  stop_line.setSecondsUntilLineStd(data[STOP_LINE_PRED_DIM+7]);
+}
+
 void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
   float desire_state_softmax[DESIRE_LEN];
   float desire_pred_softmax[4*DESIRE_LEN];
@@ -328,6 +364,9 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
   // meta
   fill_meta(framed.initMeta(), net_outputs.meta);
 
+  // stop line
+  fill_stop_line(framed.initStopLine(), net_outputs.stop_line, net_outputs.stop_line_prob);
+
   // leads
   auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
   float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};

selfdrive/modeld/models/driving.h

@@ -25,6 +25,8 @@ struct ModelDataRaw {
   float *road_edges;
   float *lead;
   float *lead_prob;
+  float *stop_line;
+  float *stop_line_prob;
   float *desire_state;
   float *meta;
   float *desire_pred;

selfdrive/ui/paint.cc

@@ -64,7 +64,25 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
   ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
 }
 
-static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
+static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
+  if (vd.cnt == 0) return;
+
+  const vertex_data *v = &vd.v[0];
+  nvgBeginPath(s->vg);
+  nvgMoveTo(s->vg, v[0].x, v[0].y);
+  for (int i = 1; i < vd.cnt; i++) {
+    nvgLineTo(s->vg, v[i].x, v[i].y);
+  }
+  nvgClosePath(s->vg);
+  if (color) {
+    nvgFillColor(s->vg, *color);
+  } else if (paint) {
+    nvgFillPaint(s->vg, *paint);
+  }
+  nvgFill(s->vg);
+}
+
+static void ui_draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
   // Draw lead car indicator
   auto [x, y] = vd;
 
@@ -87,22 +105,9 @@ static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader
   draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
 }
 
-static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) {
-  if (vd.cnt == 0) return;
-
-  const vertex_data *v = &vd.v[0];
-  nvgBeginPath(s->vg);
-  nvgMoveTo(s->vg, v[0].x, v[0].y);
-  for (int i = 1; i < vd.cnt; i++) {
-    nvgLineTo(s->vg, v[i].x, v[i].y);
-  }
-  nvgClosePath(s->vg);
-  if (color) {
-    nvgFillColor(s->vg, *color);
-  } else if (paint) {
-    nvgFillPaint(s->vg, *paint);
-  }
-  nvgFill(s->vg);
+static void ui_draw_stop_line(UIState *s, const cereal::ModelDataV2::StopLineData::Reader &stop_line_data, const line_vertices_data &vd) {
+  NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, stop_line_data.getProb());
+  ui_draw_line(s, vd, &color, nullptr);
 }
 
 static void ui_draw_vision_lane_lines(UIState *s) {
@@ -137,15 +142,19 @@ static void ui_draw_world(UIState *s) {
   // Draw lane edges and vision/mpc tracks
   ui_draw_vision_lane_lines(s);
 
-  // Draw lead indicators if openpilot is handling longitudinal
+  // Draw lead and stop line indicators if openpilot is handling longitudinal
   if (s->scene.longitudinal_control) {
     auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
     auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
+    auto stop_line = (*s->sm)["modelV2"].getModelV2().getStopLine();
     if (lead_one.getProb() > .5) {
-      draw_lead(s, lead_one, s->scene.lead_vertices[0]);
+      ui_draw_lead(s, lead_one, s->scene.lead_vertices[0]);
     }
     if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
-      draw_lead(s, lead_two, s->scene.lead_vertices[1]);
+      ui_draw_lead(s, lead_two, s->scene.lead_vertices[1]);
+    }
+    if (stop_line.getProb() > .5) {
+      ui_draw_stop_line(s, stop_line, s->scene.stop_line_vertices);
     }
   }
   nvgResetScissor(s->vg);

selfdrive/ui/ui.cc

@@ -65,6 +65,18 @@ static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTDa
   assert(pvd->cnt <= std::size(pvd->v));
 }
 
+static void update_stop_line_data(const UIState *s, const cereal::ModelDataV2::StopLineData::Reader &line,
+                                  float x_off, float y_off, float z_off, line_vertices_data *pvd) {
+  const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ();
+  vertex_data *v = &pvd->v[0];
+  v += calib_frame_to_full_frame(s, line_x + x_off, line_y - y_off, line_z + z_off, v);
+  v += calib_frame_to_full_frame(s, line_x + x_off, line_y + y_off, line_z + z_off, v);
+  v += calib_frame_to_full_frame(s, line_x - x_off, line_y + y_off, line_z + z_off, v);
+  v += calib_frame_to_full_frame(s, line_x - x_off, line_y - y_off, line_z + z_off, v);
+  pvd->cnt = v - pvd->v;
+  assert(pvd->cnt <= std::size(pvd->v));
+}
+
 static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
   UIScene &scene = s->scene;
   auto model_position = model.getPosition();
@@ -96,6 +108,12 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
   }
   max_idx = get_path_length_idx(model_position, max_distance);
   update_line_data(s, model_position, 0.5, 1.22, &scene.track_vertices, max_idx);
+
+  // update stop lines
+  const auto stop_line = model.getStopLine();
+  if (stop_line.getProb() > .5) {
+    update_stop_line_data(s, stop_line, .5, 2, 1.22, &scene.stop_line_vertices);
+  }
 }
 
 static void update_sockets(UIState *s) {

selfdrive/ui/ui.h

@@ -55,7 +55,7 @@ typedef struct Alert {
   }
 } Alert;
 
-const Alert CONTROLS_WAITING_ALERT = {"openpilot Unavailable", "Waiting for controls to start", 
+const Alert CONTROLS_WAITING_ALERT = {"openpilot Unavailable", "Waiting for controls to start",
                                       "controlsWaiting", cereal::ControlsState::AlertSize::MID,
                                       AudibleAlert::NONE};
 
@@ -106,6 +106,7 @@ typedef struct UIScene {
   line_vertices_data track_vertices;
   line_vertices_data lane_line_vertices[4];
   line_vertices_data road_edge_vertices[2];
+  line_vertices_data stop_line_vertices;
 
   bool dm_active, engageable;