diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index ca3283d34..45a07c4b7 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -1,4 +1,5 @@ import colorsys +import math import numpy as np import pyray as rl from cereal import messaging, car @@ -88,6 +89,12 @@ class ModelRenderer(Widget): colors=[], stops=[], ) + self._path_gradient = Gradient( + start=(0.0, 1.0), + end=(0.0, 0.0), + colors=THROTTLE_COLORS, + stops=[0.0, 0.5, 1.0], + ) self._rainbow_path = RainbowPath() # Get longitudinal control setting from car parameters @@ -200,13 +207,24 @@ class ModelRenderer(Widget): def _update_model(self, lead, path_x_array): """Update model visualization data based on model message""" model_ui_enabled = self._params.get_bool("ModelUI", default=True) - custom_path_width = model_ui_enabled and self._param_float_changed("PathWidth", DEFAULT_PATH_WIDTH) - custom_lane_line_width = model_ui_enabled and self._param_float_changed("LaneLinesWidth", DEFAULT_LANE_LINES_WIDTH) - custom_road_edge_width = model_ui_enabled and self._param_float_changed("RoadEdgesWidth", DEFAULT_ROAD_EDGES_WIDTH) + path_width = 0.9 + lane_line_width = None + road_edge_width = None + if model_ui_enabled: + path_width_value = self._params.get_float("PathWidth", default=DEFAULT_PATH_WIDTH) + lane_line_width_value = self._params.get_float("LaneLinesWidth", default=DEFAULT_LANE_LINES_WIDTH) + road_edge_width_value = self._params.get_float("RoadEdgesWidth", default=DEFAULT_ROAD_EDGES_WIDTH) + custom_path_width = not math.isclose(path_width_value, DEFAULT_PATH_WIDTH, rel_tol=1e-5, abs_tol=1e-8) + custom_lane_line_width = not math.isclose(lane_line_width_value, DEFAULT_LANE_LINES_WIDTH, rel_tol=1e-5, abs_tol=1e-8) + custom_road_edge_width = not math.isclose(road_edge_width_value, DEFAULT_ROAD_EDGES_WIDTH, rel_tol=1e-5, abs_tol=1e-8) - path_width = self._path_width_to_half_m(self._params.get_float("PathWidth", default=DEFAULT_PATH_WIDTH)) if custom_path_width else 0.9 - lane_line_width = self._small_distance_to_half_m(self._params.get_float("LaneLinesWidth", default=DEFAULT_LANE_LINES_WIDTH)) if custom_lane_line_width else None - road_edge_width = self._small_distance_to_half_m(self._params.get_float("RoadEdgesWidth", default=DEFAULT_ROAD_EDGES_WIDTH)) if custom_road_edge_width else None + is_metric = self._params.get_bool("IsMetric") if custom_path_width or custom_lane_line_width or custom_road_edge_width else False + if custom_path_width: + path_width = self._path_width_to_half_m(path_width_value, is_metric) + if custom_lane_line_width: + lane_line_width = self._small_distance_to_half_m(lane_line_width_value, is_metric) + if custom_road_edge_width: + road_edge_width = self._small_distance_to_half_m(road_edge_width_value, is_metric) if model_ui_enabled and self._params.get_bool("DynamicPathWidth", default=False): if ui_state.status == UIStatus.ENGAGED: @@ -342,20 +360,26 @@ class ModelRenderer(Widget): return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha)) - def _get_ll_color(self, prob: float, adjacent: bool, left: bool): - alpha = np.clip(prob, 0.0, 0.7) + def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color]: stock_scheme = is_stock_color_scheme(self._params) line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status + + edge_color = get_param_color(self._params, "PathEdgesColor", 255) + if edge_color is None: + edge_color = (LANE_LINE_COLORS.get(line_status, LANE_LINE_COLORS[UIStatus.DISENGAGED]) if stock_scheme + else get_theme_color("PathEdge", rl.Color(0, 255, 64, 255))) + + lane_color = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a) + if lane_color is None: + lane_color = STOCK_LANE_LINES_COLOR if stock_scheme else get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR) + + return stock_scheme, edge_color, lane_color + + def _get_ll_color(self, prob: float, adjacent: bool, left: bool, stock_scheme: bool, + edge_color: rl.Color, lane_color: rl.Color): + alpha = np.clip(prob, 0.0, 0.7) if adjacent: - override = get_param_color(self._params, "PathEdgesColor", 255) - if override is not None: - color = with_alpha(override, int(alpha * override.a)) - elif stock_scheme: - base_color = LANE_LINE_COLORS.get(line_status, LANE_LINE_COLORS[UIStatus.DISENGAGED]) - color = rl.Color(base_color.r, base_color.g, base_color.b, int(alpha * 255)) - else: - base_color = get_theme_color("PathEdge", rl.Color(0, 255, 64, 255)) - color = with_alpha(base_color, int(alpha * base_color.a)) + color = rl.Color(edge_color.r, edge_color.g, edge_color.b, int(alpha * edge_color.a)) # turn adjacent lls orange if torque is high torque = self._torque_filter.x @@ -367,14 +391,7 @@ class ModelRenderer(Widget): np.interp(abs(torque), [0.6, 0.8], [0.0, 1.0]) ) else: - lane_lines_override = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a) - if lane_lines_override is not None: - lane_lines_color = lane_lines_override - elif stock_scheme: - lane_lines_color = STOCK_LANE_LINES_COLOR - else: - lane_lines_color = get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR) - color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a)) + color = rl.Color(lane_color.r, lane_color.g, lane_color.b, int(alpha * lane_color.a)) if stock_scheme and ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active: color = rl.Color(0, 0, 0, int(alpha * 255)) @@ -384,11 +401,13 @@ class ModelRenderer(Widget): def _draw_lane_lines(self): """Draw lane lines and road edges""" """Two closest lines should be green (lane line or road edges)""" + stock_scheme, edge_color, lane_color = self._lane_line_palette() for i, lane_line in enumerate(self._lane_lines): if lane_line.projected_points.size == 0: continue - color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1)) + color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1), + stock_scheme, edge_color, lane_color) draw_polygon(self._rect, lane_line.projected_points, color) for i, road_edge in enumerate(self._road_edges): @@ -396,7 +415,8 @@ class ModelRenderer(Widget): continue # if closest lane lines are not confident, make road edges green - color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0) + color = self._get_ll_color(float(1.0 - self._road_edge_stds[i]), float(self._lane_line_probs[i + 1]) < 0.25, i == 0, + stock_scheme, edge_color, lane_color) draw_polygon(self._rect, road_edge.projected_points, color) def _draw_path(self, sm): @@ -428,29 +448,19 @@ class ModelRenderer(Widget): blended_colors = self._blend_colors(NO_THROTTLE_COLORS, THROTTLE_COLORS, blend_factor) if lateral_ui_active and blend_factor < 1.0: blended_colors = self._blend_colors(blended_colors, THROTTLE_COLORS, 0.65) - gradient = Gradient( - start=(0.0, 1.0), - end=(0.0, 0.0), - colors=blended_colors, - stops=[0.0, 0.5, 1.0], - ) + self._path_gradient.colors = blended_colors if ui_state.status == UIStatus.DISENGAGED and not ui_state.always_on_lateral_active: draw_polygon(self._rect, self._path.projected_points, rl.Color(0, 0, 0, 90)) else: - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + draw_polygon(self._rect, self._path.projected_points, gradient=self._path_gradient) else: path_color = get_visual_color(self._params, "PathColor", "Path", rl.Color(48, 255, 156, 255)) - gradient = Gradient( - start=(0.0, 1.0), - end=(0.0, 0.0), - colors=[ - with_alpha(path_color, path_color.a), - with_alpha(path_color, int(path_color.a * 0.55)), - with_alpha(path_color, int(path_color.a * 0.10)), - ], - stops=[0.0, 0.5, 1.0], - ) - draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + self._path_gradient.colors = [ + with_alpha(path_color, path_color.a), + with_alpha(path_color, int(path_color.a * 0.55)), + with_alpha(path_color, int(path_color.a * 0.10)), + ] + draw_polygon(self._rect, self._path.projected_points, gradient=self._path_gradient) def _draw_lead_indicator(self): # Draw lead vehicles if available @@ -577,25 +587,18 @@ class ModelRenderer(Widget): int(inv_t * start.a + t * end.a) ) for start, end in zip(begin_colors, end_colors, strict=True)] - def _small_distance_to_half_m(self, value: float) -> float: + @staticmethod + def _small_distance_to_half_m(value: float, is_metric: bool) -> float: if value <= 0: return 0.0 - if self._params.get_bool("IsMetric"): + if is_metric: return value / 200.0 return value * (CV.INCH_TO_CM / 100.0) / 2.0 - def _path_width_to_half_m(self, value: float) -> float: + @staticmethod + def _path_width_to_half_m(value: float, is_metric: bool) -> float: if value <= 0: return 0.0 - if self._params.get_bool("IsMetric"): + if is_metric: return value / 2.0 return value * CV.FOOT_TO_METER / 2.0 - - def _param_float_changed(self, key: str, default: float) -> bool: - value = self._params.get(key, encoding="utf-8") - if value in (None, ""): - return False - try: - return not np.isclose(float(value), default) - except (TypeError, ValueError): - return False diff --git a/system/ui/lib/shader_polygon.py b/system/ui/lib/shader_polygon.py index 413c23374..32cf677bf 100644 --- a/system/ui/lib/shader_polygon.py +++ b/system/ui/lib/shader_polygon.py @@ -189,18 +189,15 @@ def _configure_shader_color(state: ShaderState, color: Optional[rl.Color], def triangulate(pts: np.ndarray) -> list[tuple[float, float]]: """Only supports simple polygons with two chains (ribbon).""" - # TODO: consider deduping close screenspace points # interleave points to produce a triangle strip - # assert len(pts) % 2 == 0, "Interleaving expects even number of points" if len(pts) % 2 != 0: pts = pts[:-1] - tri_strip = [] - for i in range(len(pts) // 2): - tri_strip.append(pts[i]) - tri_strip.append(pts[-i - 1]) - - return cast(list, np.array(tri_strip).tolist()) + half = len(pts) // 2 + tri_strip = np.empty_like(pts) + tri_strip[0::2] = pts[:half] + tri_strip[1::2] = pts[half:][::-1] + return cast(list, tri_strip.tolist()) def draw_polygon(origin_rect: rl.Rectangle, points: np.ndarray, diff --git a/system/ui/lib/tests/test_shader_polygon.py b/system/ui/lib/tests/test_shader_polygon.py new file mode 100644 index 000000000..52ea45f88 --- /dev/null +++ b/system/ui/lib/tests/test_shader_polygon.py @@ -0,0 +1,35 @@ +import numpy as np + +from openpilot.system.ui.lib.shader_polygon import triangulate + + +def test_triangulate_interleaves_polygon_chains(): + points = np.array([ + [1.0, 10.0], + [2.0, 20.0], + [3.0, 30.0], + [30.0, 300.0], + [20.0, 200.0], + [10.0, 100.0], + ], dtype=np.float32) + + assert triangulate(points) == [ + [1.0, 10.0], [10.0, 100.0], + [2.0, 20.0], [20.0, 200.0], + [3.0, 30.0], [30.0, 300.0], + ] + + +def test_triangulate_drops_unpaired_last_point(): + points = np.array([ + [1.0, 10.0], + [2.0, 20.0], + [20.0, 200.0], + [10.0, 100.0], + [99.0, 99.0], + ], dtype=np.float32) + + assert triangulate(points) == [ + [1.0, 10.0], [10.0, 100.0], + [2.0, 20.0], [20.0, 200.0], + ]