TurnSpeedController: Implementation

common/params.cc

@@ -198,6 +198,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TermsVersion", PERSISTENT},
     {"Timezone", PERSISTENT},
     {"TrainingVersion", PERSISTENT},
+    {"TurnSpeedControl", PERSISTENT},
     {"TurnVisionControl", PERSISTENT},
     {"UbloxAvailable", PERSISTENT},
     {"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},

selfdrive/controls/lib/longitudinal_planner.py

@@ -17,6 +17,7 @@
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
 from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController
 from openpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver
+from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.system.swaglog import cloudlog
 
@@ -69,6 +70,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.cruise_source = 'cruise'
     self.vision_turn_controller = VisionTurnController(CP)
     self.speed_limit_controller = SpeedLimitController()
+    self.turn_speed_controller = TurnSpeedController()
     self.events = Events()
     self.personality = log.LongitudinalPersonality.standard
 
@@ -196,13 +198,19 @@ def publish(self, sm, pm):
     longitudinalPlan.isMapSpeedLimit = bool(self.speed_limit_controller.source == SpeedLimitResolver.Source.map_data)
     longitudinalPlan.eventsDEPRECATED = self.events.to_msg()
 
+    longitudinalPlan.turnSpeedControlState = self.turn_speed_controller.state
+    longitudinalPlan.turnSpeed = float(self.turn_speed_controller.speed_limit)
+    longitudinalPlan.distToTurn = float(self.turn_speed_controller.distance)
+    longitudinalPlan.turnSign = int(self.turn_speed_controller.turn_sign)
+
     pm.send('longitudinalPlan', plan_send)
 
   def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm):
     # Update controllers
     self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm)
     self.events = Events()
     self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.events)
+    self.turn_speed_controller.update(enabled, v_ego, a_ego, sm)
 
     # Pick solution with lowest velocity target.
     a_solutions = {'cruise': float("inf")}
@@ -216,6 +224,10 @@ def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm):
       a_solutions['limit'] = self.speed_limit_controller.a_target
       v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted
 
+    if self.turn_speed_controller.is_active:
+      a_solutions['turnlimit'] = self.turn_speed_controller.a_target
+      v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit
+
     source = min(v_solutions, key=v_solutions.get)
 
     return source, a_solutions[source], v_solutions[source]

selfdrive/manager/manager.py

@@ -52,6 +52,7 @@ def manager_init() -> None:
     ("ShowDebugUI", "1"),
     ("SpeedLimitControl", "1"),
     ("SpeedLimitPercOffset", "1"),
+    ("TurnSpeedControl", "1"),
     ("TurnVisionControl", "1"),
   ]
   if not PC:

selfdrive/ui/qt/offroad/settings.cc

@@ -95,6 +95,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "Set speed limit slightly higher than actual speed limit for a more natural drive.",
       "../assets/offroad/icon_speed_limit.png",
     },
+    {
+      "TurnSpeedControl",
+      "Enable Map Data Turn Control",
+      "Use curvature info from map data to define speed limits to take turns ahead",
+      "../assets/offroad/icon_openpilot.png",
+    },
     {
       "ShowDebugUI",
       "Show debug UI elements",

selfdrive/ui/qt/onroad.cc

@@ -9,6 +9,7 @@
 
 #include <QDebug>
 #include <QMouseEvent>
+#include <QPainterPath>
 
 #include "common/timing.h"
 #include "selfdrive/ui/qt/util.h"
@@ -298,6 +299,8 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par
   dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5});
   how_img = loadPixmap("../assets/img_hands_on_wheel.png", {img_size, img_size});
   map_img = loadPixmap("../assets/img_world_icon.png", {subsign_img_size, subsign_img_size});
+  left_img = loadPixmap("../assets/img_turn_left_icon.png", {subsign_img_size, subsign_img_size});
+  right_img = loadPixmap("../assets/img_turn_right_icon.png", {subsign_img_size, subsign_img_size});
 }
 
 void AnnotatedCameraWidget::updateState(const UIState &s) {
@@ -388,6 +391,17 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   mapSourcedSpeedLimit = lp.getIsMapSpeedLimit();
   slcActive = !sl_inactive && !sl_temp_inactive;
 
+  const float tsc_speed = lp.getTurnSpeed() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
+  const auto tscState = lp.getTurnSpeedControlState();
+  const int t_distance = int(lp.getDistToTurn() * (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH) / 10.0) * 10;
+  const QString t_distance_str(QString::number(t_distance) + (s.scene.is_metric ? "m" : "f"));
+
+  showTurnSpeedLimit = tsc_speed > 0.0 && (tsc_speed < v_ego || s.scene.show_debug_ui);
+  turnSpeedLimit = QString::number(std::nearbyint(tsc_speed));
+  tscSubText = t_distance > 0 ? t_distance_str : QString("");
+  tscActive = tscState > cereal::LongitudinalPlan::SpeedLimitControlState::TEMP_INACTIVE;
+  curveSign = lp.getTurnSign();
+
   // DM icon transition
   dm_fade_state = std::clamp(dm_fade_state+0.2*(0.5-dmActive), 0.0, 1.0);
 
@@ -498,6 +512,13 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
                                vtcColor, vtcSpeed, 100);
   }
 
+  // Turn Speed Sign
+  if (showTurnSpeedLimit) {
+    QRect rc = speed_sgn_rc;
+    rc.moveTop(speed_sgn_rc.bottom() + sign_margin);
+    drawTrunSpeedSign(p, rc, turnSpeedLimit, tscSubText, curveSign, tscActive);
+  }
+
   // Speed Limit Sign
   if (showSpeedLimit) {
     drawSpeedSign(p, speed_sgn_rc, speedLimitSLC, slcSubText, slcSubTextSize, mapSourcedSpeedLimit, slcActive);
@@ -563,7 +584,7 @@ void AnnotatedCameraWidget::drawCircle(QPainter &p, int x, int y, int r, QBrush
 }
 
 void AnnotatedCameraWidget::drawSpeedSign(QPainter &p, QRect rc, const QString &speed_limit, const QString &sub_text, 
-                              int subtext_size, bool is_map_sourced, bool is_active) {
+                                          int subtext_size, bool is_map_sourced, bool is_active) {
   const QColor ring_color = is_active ? QColor(255, 0, 0, 255) : QColor(0, 0, 0, 50);
   const QColor inner_color = QColor(255, 255, 255, is_active ? 255 : 85);
   const QColor text_color = QColor(0, 0, 0, is_active ? 255 : 85);
@@ -588,6 +609,60 @@ void AnnotatedCameraWidget::drawSpeedSign(QPainter &p, QRect rc, const QString &
   }
 }
 
+void AnnotatedCameraWidget::drawTrunSpeedSign(QPainter &p, QRect rc, const QString &turn_speed, const QString &sub_text, 
+                                              int curv_sign, bool is_active) {
+  const QColor border_color = is_active ? QColor(255, 0, 0, 255) : QColor(0, 0, 0, 50);
+  const QColor inner_color = QColor(255, 255, 255, is_active ? 255 : 85);
+  const QColor text_color = QColor(0, 0, 0, is_active ? 255 : 85);
+
+  const int x = rc.center().x();
+  const int y = rc.center().y();
+  const int width = rc.width();
+
+  const float stroke_w = 15.0;
+  const float cS = stroke_w / 2.0 + 4.5;  // half width of the stroke on the corners of the triangle
+  const float R = width / 2.0 - stroke_w / 2.0;
+  const float A = 0.73205;
+  const float h2 = 2.0 * R / (1.0 + A);
+  const float h1 = A * h2;
+  const float L = 4.0 * R / sqrt(3.0);
+
+  // Draw the internal triangle, compensate for stroke width. Needed to improve rendering when in inactive 
+  // state due to stroke transparency being different from inner transparency.
+  QPainterPath path;
+  path.moveTo(x, y - R + cS);
+  path.lineTo(x - L / 2.0 + cS, y + h1 + h2 - R - stroke_w / 2.0);
+  path.lineTo(x + L / 2.0 - cS, y + h1 + h2 - R - stroke_w / 2.0);
+  path.lineTo(x, y - R + cS);
+  p.setPen(Qt::NoPen);
+  p.setBrush(inner_color);
+  p.drawPath(path);
+
+  // Draw the stroke
+  QPainterPath stroke_path;
+  stroke_path.moveTo(x, y - R);
+  stroke_path.lineTo(x - L / 2.0, y + h1 + h2 - R);
+  stroke_path.lineTo(x + L / 2.0, y + h1 + h2 - R);
+  stroke_path.lineTo(x, y - R);
+  p.setPen(QPen(border_color, stroke_w, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
+  p.setBrush(Qt::NoBrush);
+  p.drawPath(stroke_path);
+
+  // Draw the turn sign
+  if (curv_sign != 0) {
+    p.setPen(Qt::NoPen);
+    p.setOpacity(is_active ? 1.0 : 0.3);
+    p.drawPixmap(int(x - (subsign_img_size / 2)), int(y - R + stroke_w + 30), curv_sign > 0 ? left_img : right_img);
+    p.setOpacity(1.0);
+  }
+
+  // Draw the texts.
+  p.setFont(InterFont(67, QFont::Bold));
+  drawCenteredText(p, x, y + 25, turn_speed, text_color);
+  p.setFont(InterFont(22, QFont::Bold));
+  drawCenteredText(p, x, y + 65, sub_text, text_color);
+}
+
 void AnnotatedCameraWidget::updateFrameMat() {
   CameraWidget::updateFrameMat();
   UIState *s = uiState();

selfdrive/ui/qt/onroad.h

@@ -83,6 +83,12 @@ class AnnotatedCameraWidget : public CameraWidget {
   Q_PROPERTY(bool mapSourcedSpeedLimit MEMBER mapSourcedSpeedLimit);
   Q_PROPERTY(bool slcActive MEMBER slcActive);
 
+  Q_PROPERTY(bool showTurnSpeedLimit MEMBER showTurnSpeedLimit);
+  Q_PROPERTY(QString turnSpeedLimit MEMBER turnSpeedLimit);
+  Q_PROPERTY(QString tscSubText MEMBER tscSubText);
+  Q_PROPERTY(bool tscActive MEMBER tscActive);
+  Q_PROPERTY(int curveSign MEMBER curveSign);
+
 public:
   explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0);
   void updateState(const UIState &s);
@@ -97,12 +103,16 @@ class AnnotatedCameraWidget : public CameraWidget {
   void drawCircle(QPainter &p, int x, int y, int r, QBrush bg);
   void drawSpeedSign(QPainter &p, QRect rc, const QString &speed, const QString &sub_text, int subtext_size, 
                      bool is_map_sourced, bool is_active);
+  void drawTrunSpeedSign(QPainter &p, QRect rc, const QString &speed, const QString &sub_text, int curv_sign, 
+                         bool is_active);
 
   QVBoxLayout *main_layout;
   ExperimentalButton *experimental_btn;
   QPixmap dm_img;
   QPixmap how_img;
   QPixmap map_img;
+  QPixmap left_img;
+  QPixmap right_img;
   float speed;
   const int subsign_img_size = 35;
   QString speedUnit;
@@ -141,6 +151,12 @@ class AnnotatedCameraWidget : public CameraWidget {
   bool mapSourcedSpeedLimit = false;
   bool slcActive = false;
 
+  bool showTurnSpeedLimit = false;
+  QString turnSpeedLimit;
+  QString tscSubText;
+  bool tscActive = false;
+  int curveSign = 0;
+
 protected:
   void paintGL() override;
   void initializeGL() override;