Revert "ui: remove map & navigation related code (#32836)"

This reverts commit 669553aa

selfdrive/ui/qt/home.cc

@@ -28,6 +28,7 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) {
   slayout->addWidget(home);
 
   onroad = new OnroadWindow(this);
+  QObject::connect(onroad, &OnroadWindow::mapPanelRequested, this, [=] { sidebar->hide(); });
   slayout->addWidget(onroad);
 
   body = new BodyWindow(this);
@@ -48,6 +49,10 @@ void HomeWindow::showSidebar(bool show) {
   sidebar->setVisible(show);
 }
 
+void HomeWindow::showMapPanel(bool show) {
+  onroad->showMapPanel(show);
+}
+
 void HomeWindow::updateState(const UIState &s) {
   const SubMaster &sm = *(s.sm);
 
@@ -81,7 +86,7 @@ void HomeWindow::showDriverView(bool show) {
 void HomeWindow::mousePressEvent(QMouseEvent* e) {
   // Handle sidebar collapsing
   if ((onroad->isVisible() || body->isVisible()) && (!sidebar->isVisible() || e->x() > sidebar->width())) {
-    sidebar->setVisible(!sidebar->isVisible());
+    sidebar->setVisible(!sidebar->isVisible() && !onroad->isMapVisible());
   }
 }
 

selfdrive/ui/qt/home.h

@@ -55,6 +55,7 @@ public slots:
   void offroadTransition(bool offroad);
   void showDriverView(bool show);
   void showSidebar(bool show);
+  void showMapPanel(bool show);
 
 protected:
   void mousePressEvent(QMouseEvent* e) override;

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -28,8 +28,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   const SubMaster &sm = *(s.sm);
 
   const bool cs_alive = sm.alive("controlsState");
+  const bool nav_alive = sm.alive("navInstruction") && sm["navInstruction"].getValid();
   const auto cs = sm["controlsState"].getControlsState();
   const auto car_state = sm["carState"].getCarState();
+  const auto nav_instruction = sm["navInstruction"].getNavInstruction();
 
   // Handle older routes where vCruiseCluster is not set
   float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster();
@@ -45,6 +47,12 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   speed = cs_alive ? std::max<float>(0.0, v_ego) : 0.0;
   speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
 
+  auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
+  speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
+  speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+
+  has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
+  has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
   is_metric = s.scene.is_metric;
   speedUnit =  s.scene.is_metric ? tr("km/h") : tr("mph");
   hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE);
@@ -70,18 +78,30 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   bg.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0));
   p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
 
+  QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
   QString speedStr = QString::number(std::nearbyint(speed));
   QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–";
 
-  // Draw outer box + border to contain set speed
+  // Draw outer box + border to contain set speed and speed limit
+  const int sign_margin = 12;
+  const int us_sign_height = 186;
+  const int eu_sign_size = 176;
+
   const QSize default_size = {172, 204};
   QSize set_speed_size = default_size;
-  if (is_metric) set_speed_size.rwidth() = 200;
+  if (is_metric || has_eu_speed_limit) set_speed_size.rwidth() = 200;
+  if (has_us_speed_limit && speedLimitStr.size() >= 3) set_speed_size.rwidth() = 223;
+
+  if (has_us_speed_limit) set_speed_size.rheight() += us_sign_height + sign_margin;
+  else if (has_eu_speed_limit) set_speed_size.rheight() += eu_sign_size + sign_margin;
+
+  int top_radius = 32;
+  int bottom_radius = has_eu_speed_limit ? 100 : 32;
 
   QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
   p.setPen(QPen(whiteColor(75), 6));
   p.setBrush(blackColor(166));
-  p.drawRoundedRect(set_speed_rect, 32, 32);
+  drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
 
   // Draw MAX
   QColor max_color = QColor(0x80, 0xd8, 0xa6, 0xff);
@@ -91,6 +111,12 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
       max_color = whiteColor();
     } else if (status == STATUS_OVERRIDE) {
       max_color = QColor(0x91, 0x9b, 0x95, 0xff);
+    } else if (speedLimit > 0) {
+      auto interp_color = [=](QColor c1, QColor c2, QColor c3) {
+        return speedLimit > 0 ? interpColor(setSpeed, {speedLimit + 5, speedLimit + 15, speedLimit + 25}, {c1, c2, c3}) : c1;
+      };
+      max_color = interp_color(max_color, QColor(0xff, 0xe4, 0xbf), QColor(0xff, 0xbf, 0xbf));
+      set_speed_color = interp_color(set_speed_color, QColor(0xff, 0x95, 0x00), QColor(0xff, 0x00, 0x00));
     }
   } else {
     max_color = QColor(0xa6, 0xa6, 0xa6, 0xff);
@@ -103,6 +129,35 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   p.setPen(set_speed_color);
   p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
 
+  const QRect sign_rect = set_speed_rect.adjusted(sign_margin, default_size.height(), -sign_margin, -sign_margin);
+  // US/Canada (MUTCD style) sign
+  if (has_us_speed_limit) {
+    p.setPen(Qt::NoPen);
+    p.setBrush(whiteColor());
+    p.drawRoundedRect(sign_rect, 24, 24);
+    p.setPen(QPen(blackColor(), 6));
+    p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
+
+    p.setFont(InterFont(28, QFont::DemiBold));
+    p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+    p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+    p.setFont(InterFont(70, QFont::Bold));
+    p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+  }
+
+  // EU (Vienna style) sign
+  if (has_eu_speed_limit) {
+    p.setPen(Qt::NoPen);
+    p.setBrush(whiteColor());
+    p.drawEllipse(sign_rect);
+    p.setPen(QPen(Qt::red, 20));
+    p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
+
+    p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
+    p.setPen(blackColor());
+    p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
+  }
+
   // current speed
   p.setFont(InterFont(176, QFont::Bold));
   drawText(p, rect().center().x(), 210, speedStr);

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -22,12 +22,15 @@ private:
   float speed;
   QString speedUnit;
   float setSpeed;
+  float speedLimit;
   bool is_cruise_set = false;
   bool is_metric = false;
   bool dmActive = false;
   bool hideBottomIcons = false;
   bool rightHandDM = false;
   float dm_fade_state = 1.0;
+  bool has_us_speed_limit = false;
+  bool has_eu_speed_limit = false;
   bool v_ego_cluster_seen = false;
   int status = STATUS_DISENGAGED;
   std::unique_ptr<PubMaster> pm;

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -44,6 +44,12 @@ void OnroadWindow::updateState(const UIState &s) {
     return;
   }
 
+  if (s.scene.map_on_left) {
+    split->setDirection(QBoxLayout::LeftToRight);
+  } else {
+    split->setDirection(QBoxLayout::RightToLeft);
+  }
+
   alerts->updateState(s);
   nvg->updateState(s);
 
@@ -55,6 +61,11 @@ void OnroadWindow::updateState(const UIState &s) {
   }
 }
 
+void OnroadWindow::mousePressEvent(QMouseEvent* e) {
+  // propagation event to parent(HomeWindow)
+  QWidget::mousePressEvent(e);
+}
+
 void OnroadWindow::offroadTransition(bool offroad) {
   alerts->clear();
 }

selfdrive/ui/qt/onroad/onroad_home.h

@@ -8,12 +8,20 @@ class OnroadWindow : public QWidget {
 
 public:
   OnroadWindow(QWidget* parent = 0);
+  bool isMapVisible() const { return map && map->isVisible(); }
+  void showMapPanel(bool show) { if (map) map->setVisible(show); }
+
+signals:
+  void mapPanelRequested();
 
 private:
+  void createMapWidget();
   void paintEvent(QPaintEvent *event);
+  void mousePressEvent(QMouseEvent* e) override;
   OnroadAlerts *alerts;
   AnnotatedCameraWidget *nvg;
   QColor bg = bg_colors[STATUS_DISENGAGED];
+  QWidget *map = nullptr;
   QHBoxLayout* split;
 
 private slots:

selfdrive/ui/qt/util.cc

@@ -156,6 +156,60 @@ QPixmap loadPixmap(const QString &fileName, const QSize &size, Qt::AspectRatioMo
   }
 }
 
+void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom){
+  qreal w_2 = rect.width() / 2;
+  qreal h_2 = rect.height() / 2;
+
+  xRadiusTop = 100 * qMin(xRadiusTop, w_2) / w_2;
+  yRadiusTop = 100 * qMin(yRadiusTop, h_2) / h_2;
+
+  xRadiusBottom = 100 * qMin(xRadiusBottom, w_2) / w_2;
+  yRadiusBottom = 100 * qMin(yRadiusBottom, h_2) / h_2;
+
+  qreal x = rect.x();
+  qreal y = rect.y();
+  qreal w = rect.width();
+  qreal h = rect.height();
+
+  qreal rxx2Top = w*xRadiusTop/100;
+  qreal ryy2Top = h*yRadiusTop/100;
+
+  qreal rxx2Bottom = w*xRadiusBottom/100;
+  qreal ryy2Bottom = h*yRadiusBottom/100;
+
+  QPainterPath path;
+  path.arcMoveTo(x, y, rxx2Top, ryy2Top, 180);
+  path.arcTo(x, y, rxx2Top, ryy2Top, 180, -90);
+  path.arcTo(x+w-rxx2Top, y, rxx2Top, ryy2Top, 90, -90);
+  path.arcTo(x+w-rxx2Bottom, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 0, -90);
+  path.arcTo(x, y+h-ryy2Bottom, rxx2Bottom, ryy2Bottom, 270, -90);
+  path.closeSubpath();
+
+  painter.drawPath(path);
+}
+
+QColor interpColor(float xv, std::vector<float> xp, std::vector<QColor> fp) {
+  assert(xp.size() == fp.size());
+
+  int N = xp.size();
+  int hi = 0;
+
+  while (hi < N and xv > xp[hi]) hi++;
+  int low = hi - 1;
+
+  if (hi == N && xv > xp[low]) {
+    return fp[fp.size() - 1];
+  } else if (hi == 0){
+    return fp[0];
+  } else {
+    return QColor(
+      (xv - xp[low]) * (fp[hi].red() - fp[low].red()) / (xp[hi] - xp[low]) + fp[low].red(),
+      (xv - xp[low]) * (fp[hi].green() - fp[low].green()) / (xp[hi] - xp[low]) + fp[low].green(),
+      (xv - xp[low]) * (fp[hi].blue() - fp[low].blue()) / (xp[hi] - xp[low]) + fp[low].blue(),
+      (xv - xp[low]) * (fp[hi].alpha() - fp[low].alpha()) / (xp[hi] - xp[low]) + fp[low].alpha());
+  }
+}
+
 static QHash<QString, QByteArray> load_bootstrap_icons() {
   QHash<QString, QByteArray> icons;
 

selfdrive/ui/qt/util.h

@@ -26,6 +26,9 @@ void initApp(int argc, char *argv[], bool disable_hidpi = true);
 QWidget* topWidget(QWidget* widget);
 QPixmap loadPixmap(const QString &fileName, const QSize &size = {}, Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio);
 QPixmap bootstrapPixmap(const QString &id);
+
+void drawRoundedRect(QPainter &painter, const QRectF &rect, qreal xRadiusTop, qreal yRadiusTop, qreal xRadiusBottom, qreal yRadiusBottom);
+QColor interpColor(float xv, std::vector<float> xp, std::vector<QColor> fp);
 bool hasLongitudinalControl(const cereal::CarParams::Reader &car_params);
 
 struct InterFont : public QFont {

selfdrive/ui/translations/main_ar.ts

@@ -113,6 +113,14 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>SPEED</source>
+        <translation>SPEED</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation>LIMIT</translation>
+    </message>
 </context>
 <context>
     <name>AutoLaneChangeTimer</name>

selfdrive/ui/translations/main_es.ts

@@ -101,6 +101,14 @@
         <source>MAX</source>
         <translation>MAX</translation>
     </message>
+    <message>
+        <source>SPEED</source>
+        <translation>VELOCIDAD</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation>LIMITE</translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -216,6 +216,14 @@ Please use caution when using this feature. Only use the blinker when traffic an
         <source> cm</source>
         <translation> 厘米</translation>
     </message>
+    <message>
+        <source>SPEED</source>
+        <translation>SPEED</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation>LIMIT</translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -101,6 +101,14 @@
         <source>MAX</source>
         <translation>最高</translation>
     </message>
+    <message>
+        <source>SPEED</source>
+        <translation>速度</translation>
+    </message>
+    <message>
+        <source>LIMIT</source>
+        <translation>速限</translation>
+    </message>
 </context>
 <context>
     <name>ConfirmationDialog</name>

selfdrive/ui/ui.cc

@@ -214,6 +214,7 @@ static void update_state(UIState *s) {
 void ui_update_params(UIState *s) {
   auto params = Params();
   s->scene.is_metric = params.getBool("IsMetric");
+  s->scene.map_on_left = params.getBool("NavSettingLeftSide");
 }
 
 void UIState::updateStatus() {
@@ -243,7 +244,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
-    "wideRoadCameraState", "managerState", "clocks",
+    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "clocks",
   });
 
   Params params;

selfdrive/ui/ui.h

@@ -98,7 +98,7 @@ typedef struct UIScene {
   cereal::LongitudinalPersonality personality;
 
   float light_sensor = -1;
-  bool started, ignition, is_metric, longitudinal_control;
+  bool started, ignition, is_metric, map_on_left, longitudinal_control;
   bool world_objects_visible = false;
   uint64_t started_frame;
 } UIScene;