diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index db316aa636..ea15a58e19 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -1,3 +1,4 @@ +import math import colorsys import numpy as np import pyray as rl @@ -145,6 +146,9 @@ class ModelRenderer(Widget): self._draw_lane_lines() self._draw_path(sm) + if (sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame): + self._draw_radar_tracks(sm['liveTracks']) + # if render_lead_indicator and radar_state: # self._draw_lead_indicator() @@ -357,6 +361,19 @@ class ModelRenderer(Widget): else: draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + def _draw_radar_tracks(self, live_tracks): + 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 = self._map_to_screen(d_rel, -y_rel, self._path_offset_z) + if pt is None: + continue + + x, y = pt + rl.draw_circle(int(x), int(y), 3, rl.Color(0, 255, 64, 255)) + def _draw_lead_indicator(self): # Draw lead vehicles if available for lead in self._lead_vehicles: diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index cae9765341..8ef4d1ac94 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -1,3 +1,4 @@ +import math import colorsys import numpy as np import pyray as rl @@ -130,6 +131,9 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_lane_lines() self._draw_path(sm) + if (sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame): + self._draw_radar_tracks(sm['liveTracks']) + 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) @@ -308,6 +312,19 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): ) draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + def _draw_radar_tracks(self, live_tracks): + 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 = self._map_to_screen(d_rel, -y_rel, self._path_offset_z) + if pt is None: + continue + + x, y = pt + rl.draw_circle(int(x), int(y), 6, rl.Color(0, 255, 64, 255)) + def _draw_lead_indicator(self): # Draw lead vehicles if available for lead in self._lead_vehicles: diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index a86c84ada3..0fdc38df9c 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -60,6 +60,7 @@ class UIState(UIStateSP): "carControl", "liveParameters", "rawAudioData", + "liveTracks", ] + self.sm_services_ext )