Merge branch 'master' into visuals-radar-tracks

This reverts commit efd499448a.

common/params_keys.h

@@ -176,6 +176,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"RadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},

selfdrive/ui/mici/layouts/settings/toggles.py

@@ -21,8 +21,10 @@ class TogglesLayoutMici(NavScroller):
     record_front = BigParamControl("record & upload driver camera", "RecordFront", toggle_callback=restart_needed_callback)
     record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
     enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
+    radar_tracks = BigParamControl("radar tracks", "RadarTracks")
 
     self._scroller.add_widgets([
+      radar_tracks,
       self._personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
@@ -35,6 +37,7 @@ class TogglesLayoutMici(NavScroller):
 
     # Toggle lists
     self._refresh_toggles = (
+      ("RadarTracks", radar_tracks),
       ("ExperimentalMode", self._experimental_btn),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),

selfdrive/ui/mici/onroad/model_renderer.py

@@ -153,6 +153,9 @@ class ModelRenderer(Widget, ModelRendererSP):
       self._draw_lane_lines()
       self._draw_path(sm)
 
+    if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
+      self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, track_size=3)
+
     # if render_lead_indicator and radar_state:
     #   self._draw_lead_indicator()
 

selfdrive/ui/onroad/model_renderer.py

@@ -135,6 +135,9 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP):
     self._draw_lane_lines()
     self._draw_path(sm)
 
+    if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame:
+      self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z)
+
     if render_lead_indicator and radar_state:
       self._draw_lead_indicator()
       self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles)

selfdrive/ui/sunnypilot/onroad/model_renderer.py

@@ -6,9 +6,12 @@ See the LICENSE.md file in the root directory for more details.
 """
 from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics
 from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath
+from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks
 
 
 class ModelRendererSP:
   def __init__(self):
     self.rainbow_path = RainbowPath()
     self.chevron_metrics = ChevronMetrics()
+    self.radar_tracks = RadarTracks()
+

selfdrive/ui/sunnypilot/onroad/radar_tracks.py

@@ -0,0 +1,23 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import math
+import pyray as rl
+
+
+class RadarTracks:
+  def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=6):
+    for track in live_tracks.points:
+      d_rel, y_rel, v_rel, a_rel = track.dRel, track.yRel, track.vRel, track.aRel
+      if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel) and math.isfinite(a_rel)):
+        continue
+
+      pt = map_to_screen(d_rel, -y_rel, path_offset_z)
+      if pt is None:
+        continue
+
+      x, y = pt
+      rl.draw_circle(int(x), int(y), track_size, rl.Color(0, 255, 64, 255))

selfdrive/ui/sunnypilot/ui_state.py

@@ -146,6 +146,7 @@ class UIStateSP:
     self.true_v_ego_ui = self.params.get_bool("TrueVEgoUI")
     self.turn_signals = self.params.get_bool("ShowTurnSignals")
     self.boot_offroad_mode = self.params.get("DeviceBootMode", return_default=True)
+    self.radar_tracks = self.params.get_bool("RadarTracks")
 
 
 class DeviceSP:

selfdrive/ui/ui_state.py

@@ -60,6 +60,7 @@ class UIState(UIStateSP):
         "carControl",
         "liveParameters",
         "rawAudioData",
+        "liveTracks",
       ] + self.sm_services_ext
     )
 

sunnypilot/sunnylink/params_metadata.json

@@ -1086,6 +1086,10 @@
     "title": "Quiet Mode",
     "description": ""
   },
+  "RadarTracks": {
+    "title": "Radar Tracks",
+    "description": ""
+  },
   "RainbowMode": {
     "title": "Rainbow Mode",
     "description": ""